Simple CAN chunking (#1011)

* simple chunking

* make pylint happy

* misra happy?

* good practice anyways since we cast to a uint32_t later

* fix bug dropping packets

* minor fixes + prepare for shared lib testing

* working library now

* first queue test

* can send test

* fix running in github actions?

* add big rx test and fix it

* don't complain about empty buffers

* disable for now

* comment

* test runs

* some cleanup

* merge those

* test works

* rm that

* comment

* proper logging

* makes things too slow

Co-authored-by: Comma Device <device@comma.ai>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

python/__init__.py

@@ -29,18 +29,18 @@ BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
 
 DEBUG = os.getenv("PANDADEBUG") is not None
 
+CAN_TRANSACTION_MAGIC = struct.pack("<I", 0x43414E2F)
 USBPACKET_MAX_SIZE = 0x40
 CANPACKET_HEAD_SIZE = 0x5
 DLC_TO_LEN = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64]
 LEN_TO_DLC = {length: dlc for (dlc, length) in enumerate(DLC_TO_LEN)}
 
 def pack_can_buffer(arr):
-  snds = [b'']
-  idx = 0
+  snds = [CAN_TRANSACTION_MAGIC]
   for address, _, dat, bus in arr:
     assert len(dat) in LEN_TO_DLC
-    if DEBUG:
-      print(f"  W 0x{address:x}: 0x{dat.hex()}")
+    #logging.debug("  W 0x%x: 0x%s", address, dat.hex())
+
     extended = 1 if address >= 0x800 else 0
     data_len_code = LEN_TO_DLC[len(dat)]
     header = bytearray(5)
@@ -50,57 +50,50 @@ def pack_can_buffer(arr):
     header[2] = (word_4b >> 8) & 0xFF
     header[3] = (word_4b >> 16) & 0xFF
     header[4] = (word_4b >> 24) & 0xFF
-    snds[idx] += header + dat
-    if len(snds[idx]) > 256: # Limit chunks to 256 bytes
-      snds.append(b'')
-      idx += 1
 
-  # Apply counter to each 64 byte packet
-  for idx in range(len(snds)):
-    tx = b''
-    counter = 0
-    for i in range (0, len(snds[idx]), 63):
-      tx += bytes([counter]) + snds[idx][i:i+63]
-      counter += 1
-    snds[idx] = tx
+    snds[-1] += header + dat
+    if len(snds[-1]) > 256: # Limit chunks to 256 bytes
+      snds.append(CAN_TRANSACTION_MAGIC)
+
   return snds
 
 def unpack_can_buffer(dat):
   ret = []
-  counter = 0
-  tail = bytearray()
-  for i in range(0, len(dat), 64):
-    if counter != dat[i]:
-      print("CAN: LOST RECV PACKET COUNTER")
-      break
-    counter+=1
-    chunk = tail + dat[i+1:i+64]
-    tail = bytearray()
-    pos = 0
-    while pos<len(chunk):
-      data_len = DLC_TO_LEN[(chunk[pos]>>4)]
-      pckt_len = CANPACKET_HEAD_SIZE + data_len
-      if pckt_len <= len(chunk[pos:]):
-        header = chunk[pos:pos+CANPACKET_HEAD_SIZE]
-        if len(header) < 5:
-          print("CAN: MALFORMED USB RECV PACKET")
-          break
-        bus = (header[0] >> 1) & 0x7
-        address = (header[4] << 24 | header[3] << 16 | header[2] << 8 | header[1]) >> 3
-        returned = (header[1] >> 1) & 0x1
-        rejected = header[1] & 0x1
-        data = chunk[pos + CANPACKET_HEAD_SIZE:pos + CANPACKET_HEAD_SIZE + data_len]
-        if returned:
-          bus += 128
-        if rejected:
-          bus += 192
-        if DEBUG:
-          print(f"  R 0x{address:x}: 0x{data.hex()}")
-        ret.append((address, 0, data, bus))
-        pos += pckt_len
-      else:
-        tail = chunk[pos:]
-        break
+  if len(dat) < len(CAN_TRANSACTION_MAGIC):
+    return ret
+
+  if dat[:len(CAN_TRANSACTION_MAGIC)] != CAN_TRANSACTION_MAGIC:
+    logging.error("CAN: recv didn't start with magic")
+    return ret
+
+  dat = dat[len(CAN_TRANSACTION_MAGIC):]
+
+  while len(dat) >= CANPACKET_HEAD_SIZE:
+    data_len = DLC_TO_LEN[(dat[0]>>4)]
+
+    header = dat[:CANPACKET_HEAD_SIZE]
+    dat = dat[CANPACKET_HEAD_SIZE:]
+
+    bus = (header[0] >> 1) & 0x7
+    address = (header[4] << 24 | header[3] << 16 | header[2] << 8 | header[1]) >> 3
+
+    if (header[1] >> 1) & 0x1:
+      # returned
+      bus += 128
+    if header[1] & 0x1:
+      # rejected
+      bus += 192
+
+    data = dat[:data_len]
+    dat = dat[data_len:]
+
+    #logging.debug("  R 0x%x: 0x%s", address, data.hex())
+
+    ret.append((address, 0, data, bus))
+
+  if len(dat) > 0:
+    logging.error("CAN: malformed packet. leftover data")
+
   return ret
 
 def ensure_health_packet_version(fn):
@@ -192,7 +185,7 @@ class Panda:
   HW_TYPE_RED_PANDA_V2 = b'\x08'
   HW_TYPE_TRES = b'\x09'
 
-  CAN_PACKET_VERSION = 2
+  CAN_PACKET_VERSION = 3
   HEALTH_PACKET_VERSION = 11
   CAN_HEALTH_PACKET_VERSION = 3
   HEALTH_STRUCT = struct.Struct("<IIIIIIIIIBBBBBBHBBBHfBB")