From 8a09725764be9a1b5f7427ce99ee2e325f848c01 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pierre-Luc=20Tessier=20Gagn=C3=A9?= <ptessie3@ford.com>
Date: Fri, 3 May 2019 10:19:23 -0400
Subject: [PATCH] Adding CAN FD 64 frame support to blf reader

---
 can/io/blf.py | 39 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 39 insertions(+)

diff --git a/can/io/blf.py b/can/io/blf.py
index 059da5ec9..d162fdebc 100644
--- a/can/io/blf.py
+++ b/can/io/blf.py
@@ -66,6 +66,13 @@ class BLFParseError(Exception):
 # valid data bytes, data
 CAN_FD_MSG_STRUCT = struct.Struct("<HBBLLBBB5x64s")
 
+# channel, dlc, valid payload length of data, tx count, arbitration id,
+# frame length, flags, bit rate used in arbitration phase,
+# bit rate used in data phase, time offset of brs field,
+# time offset of crc delimiter field, bit count, direction,
+# offset if extDataOffset is used, crc
+CAN_FD_MSG_64_STRUCT = struct.Struct("<BBBBLLLLLLLHBBL")
+
 # channel, length, flags, ecc, position, dlc, frame length, id, flags ext, data
 CAN_ERROR_EXT_STRUCT = struct.Struct("<HHLBBBxLLH2x8s")
 
@@ -81,6 +88,7 @@ class BLFParseError(Exception):
 CAN_MESSAGE2 = 86
 GLOBAL_MARKER = 96
 CAN_FD_MESSAGE = 100
+CAN_FD_MESSAGE_64 = 101
 
 NO_COMPRESSION = 0
 ZLIB_DEFLATE = 2
@@ -91,6 +99,12 @@ class BLFParseError(Exception):
 BRS = 0x2
 ESI = 0x4
 
+# CAN FD 64 Flags
+REMOTE_FLAG_64 = 0x0010
+EDL_64 = 0x1000
+BRS_64 = 0x2000
+ESI_64 = 0x4000
+
 TIME_TEN_MICS = 0x00000001
 TIME_ONE_NANS = 0x00000002
 
@@ -236,6 +250,29 @@ def __iter__(self):
                                       data=can_data[:length],
                                       channel=channel - 1)
                         yield msg
+                    elif obj_type == CAN_FD_MESSAGE_64:
+                        (
+                            channel, dlc, _, _, can_id, _, fd_flags
+                         ) = CAN_FD_MSG_64_STRUCT.unpack_from(data, pos)[:7]
+                        length = dlc2len(dlc)
+                        can_data = struct.unpack_from(
+                            "<{}s".format(length),
+                            data,
+                            pos + CAN_FD_MSG_64_STRUCT.size
+                        )[0]
+                        msg = Message(
+                            timestamp=timestamp,
+                            arbitration_id=can_id & 0x1FFFFFFF,
+                            is_extended_id=bool(can_id & CAN_MSG_EXT),
+                            is_remote_frame=bool(fd_flags & REMOTE_FLAG_64),
+                            is_fd=bool(fd_flags & EDL_64),
+                            bitrate_switch=bool(fd_flags & BRS_64),
+                            error_state_indicator=bool(fd_flags & ESI_64),
+                            dlc=length,
+                            data=can_data[:length],
+                            channel=channel - 1
+                        )
+                        yield msg
                     elif obj_type == CAN_ERROR_EXT:
                         (channel, _, _, _, _, dlc, _, can_id, _,
                          can_data) = CAN_ERROR_EXT_STRUCT.unpack_from(data, pos)
@@ -247,6 +284,8 @@ def __iter__(self):
                                       data=can_data[:dlc],
                                       channel=channel - 1)
                         yield msg
+                    # else:
+                    #     LOG.warning("Unknown object type (%d)", obj_type)
 
                     pos = next_pos