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>

board/can_comms.h

@@ -1,34 +1,52 @@
+/*
+  CAN transactions to and from the host come in the form of
+  a 4 byte value called CAN_TRANSACTION_MAGIC followed by
+  a certain number of CANPacket_t. The transaction is split
+  into multiple transfers or chunks.
+
+  * comms_can_read outputs this buffer in chunks of a specified length.
+    chunks are always the given length, except the last one.
+  * comms_can_write reads in this buffer in chunks. start of the transaction
+    is denoted by the CAN_TRANSACTION_MAGIC value.
+  * both functions maintain an overflow buffer for a partial CANPacket_t that
+    spans multiple transfers/chunks.
+*/
+
 typedef struct {
   uint32_t ptr;
   uint32_t tail_size;
   uint8_t data[72];
-  uint8_t counter;
 } asm_buffer;
 
-asm_buffer can_read_buffer = {.ptr = 0U, .tail_size = 0U, .counter = 0U};
+asm_buffer can_read_buffer = {.ptr = 0U, .tail_size = 0U};
 uint32_t total_rx_size = 0U;
+bool add_magic = true;
 
 int comms_can_read(uint8_t *data, uint32_t max_len) {
-  uint32_t pos = 1;
-  data[0] = can_read_buffer.counter;
+  uint32_t pos = 0U;
+  bool added_magic = false;
+
+  if (add_magic && (max_len >= sizeof(uint32_t))) {
+    // Start of a transaction
+    *((uint32_t *)(void *) &data[0]) = CAN_TRANSACTION_MAGIC;
+    pos += sizeof(uint32_t);
+    add_magic = false;
+    can_read_buffer.ptr = 0U;
+    total_rx_size = 0U;
+    added_magic = true;
+  }
+
   // Send tail of previous message if it is in buffer
   if (can_read_buffer.ptr > 0U) {
-    if (can_read_buffer.ptr <= 63U) {
-      (void)memcpy(&data[pos], can_read_buffer.data, can_read_buffer.ptr);
-      pos += can_read_buffer.ptr;
-      can_read_buffer.ptr = 0U;
-    } else {
-      (void)memcpy(&data[pos], can_read_buffer.data, 63U);
-      can_read_buffer.ptr = can_read_buffer.ptr - 63U;
-      (void)memcpy(can_read_buffer.data, &can_read_buffer.data[63], can_read_buffer.ptr);
-      pos += 63U;
-    }
+    uint32_t overflow_len = MIN(max_len - pos, can_read_buffer.ptr);
+    (void)memcpy(&data[pos], can_read_buffer.data, overflow_len);
+    pos += overflow_len;
+    (void)memcpy(can_read_buffer.data, &can_read_buffer.data[overflow_len], can_read_buffer.ptr - overflow_len);
+    can_read_buffer.ptr -= overflow_len;
   }
 
-  if (total_rx_size > MAX_EP1_CHUNK_PER_BULK_TRANSFER) {
-    total_rx_size = 0U;
-    can_read_buffer.counter = 0U;
-  } else {
+  if ((total_rx_size + pos) < MAX_EP1_CHUNK_PER_BULK_TRANSFER) {
+    // Fill rest of buffer with new data
     CANPacket_t can_packet;
     while ((pos < max_len) && can_pop(&can_rx_q, &can_packet)) {
       uint32_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[can_packet.data_len_code];
@@ -43,61 +61,74 @@ int comms_can_read(uint8_t *data, uint32_t max_len) {
         pos = max_len;
       }
     }
-    can_read_buffer.counter++;
-    total_rx_size += pos;
   }
+
   if (pos != max_len) {
-    can_read_buffer.counter = 0U;
-    total_rx_size = 0U;
+    // Final packet for this transaction, prepare for the next one
+    add_magic = true;
+  }
+
+  if (added_magic && (pos == sizeof(uint32_t))) {
+    // Magic alone doesn't make sense
+    pos = 0U;
   }
-  if (pos <= 1U) { pos = 0U; }
+
+  total_rx_size += pos;
   return pos;
 }
 
-asm_buffer can_write_buffer = {.ptr = 0U, .tail_size = 0U, .counter = 0U};
+asm_buffer can_write_buffer = {.ptr = 0U, .tail_size = 0U};
 
 // send on CAN
 void comms_can_write(uint8_t *data, uint32_t len) {
-  // Got first packet from a stream, resetting buffer and counter
-  if (data[0] == 0U) {
-    can_write_buffer.counter = 0U;
+  uint32_t pos = 0U;
+
+  if (*((uint32_t *)(void *) &data[0]) == CAN_TRANSACTION_MAGIC) {
+    // Got first packet from a stream, resetting buffer and counter
     can_write_buffer.ptr = 0U;
     can_write_buffer.tail_size = 0U;
+    pos += sizeof(uint32_t);
   }
+
   // Assembling can message with data from buffer
-  if (data[0] == can_write_buffer.counter) {
-    uint32_t pos = 1U;
-    can_write_buffer.counter++;
-    if (can_write_buffer.ptr != 0U) {
-      if (can_write_buffer.tail_size <= 63U) {
-        CANPacket_t to_push;
-        (void)memcpy(&can_write_buffer.data[can_write_buffer.ptr], &data[pos], can_write_buffer.tail_size);
-        (void)memcpy(&to_push, can_write_buffer.data, can_write_buffer.ptr + can_write_buffer.tail_size);
-        can_send(&to_push, to_push.bus, false);
-        pos += can_write_buffer.tail_size;
-        can_write_buffer.ptr = 0U;
-        can_write_buffer.tail_size = 0U;
-      } else {
-        (void)memcpy(&can_write_buffer.data[can_write_buffer.ptr], &data[pos], len - pos);
-        can_write_buffer.tail_size -= 63U;
-        can_write_buffer.ptr += 63U;
-        pos += 63U;
-      }
+  if (can_write_buffer.ptr != 0U) {
+    if (can_write_buffer.tail_size <= (len - pos)) {
+      // we have enough data to complete the buffer
+      CANPacket_t to_push;
+      (void)memcpy(&can_write_buffer.data[can_write_buffer.ptr], &data[pos], can_write_buffer.tail_size);
+      can_write_buffer.ptr += can_write_buffer.tail_size;
+      pos += can_write_buffer.tail_size;
+
+      // send out
+      (void)memcpy(&to_push, can_write_buffer.data, can_write_buffer.ptr);
+      can_send(&to_push, to_push.bus, false);
+
+      // reset overflow buffer
+      can_write_buffer.ptr = 0U;
+      can_write_buffer.tail_size = 0U;
+    } else {
+      // maybe next time
+      uint32_t data_size = len - pos;
+      (void) memcpy(&can_write_buffer.data[can_write_buffer.ptr], &data[pos], data_size);
+      can_write_buffer.tail_size -= data_size;
+      can_write_buffer.ptr += data_size;
+      pos += data_size;
     }
+  }
 
-    while (pos < len) {
-      uint32_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[(data[pos] >> 4U)];
-      if ((pos + pckt_len) <= len) {
-        CANPacket_t to_push;
-        (void)memcpy(&to_push, &data[pos], pckt_len);
-        can_send(&to_push, to_push.bus, false);
-        pos += pckt_len;
-      } else {
-        (void)memcpy(can_write_buffer.data, &data[pos], len - pos);
-        can_write_buffer.ptr = len - pos;
-        can_write_buffer.tail_size = pckt_len - can_write_buffer.ptr;
-        pos += can_write_buffer.ptr;
-      }
+  // rest of the message
+  while (pos < len) {
+    uint32_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[(data[pos] >> 4U)];
+    if ((pos + pckt_len) <= len) {
+      CANPacket_t to_push;
+      (void)memcpy(&to_push, &data[pos], pckt_len);
+      can_send(&to_push, to_push.bus, false);
+      pos += pckt_len;
+    } else {
+      (void)memcpy(can_write_buffer.data, &data[pos], len - pos);
+      can_write_buffer.ptr = len - pos;
+      can_write_buffer.tail_size = pckt_len - can_write_buffer.ptr;
+      pos += can_write_buffer.ptr;
     }
   }
 }

board/can_definitions.h

@@ -4,9 +4,10 @@ const uint8_t PANDA_CAN_CNT = 3U;
 const uint8_t PANDA_BUS_CNT = 4U;
 
 // bump this when changing the CAN packet
-#define CAN_PACKET_VERSION 2
+#define CAN_PACKET_VERSION 3
 
 #define CANPACKET_HEAD_SIZE 5U
+#define CAN_TRANSACTION_MAGIC 0x43414E2FU
 
 #if !defined(STM32F4) && !defined(STM32F2)
   #define CANFD

board/drivers/usb.h

@@ -352,7 +352,7 @@ uint8_t winusb_20_desc[WINUSB_PLATFORM_DESCRIPTOR_LENGTH] = {
 
 // current packet
 USB_Setup_TypeDef setup;
-uint8_t usbdata[0x100];
+uint8_t usbdata[0x100] __attribute__((aligned(4)));
 uint8_t* ep0_txdata = NULL;
 uint16_t ep0_txlen = 0;
 bool outep3_processing = false;

board/safety_declarations.h

@@ -1,3 +1,5 @@
+#pragma once
+
 #define GET_BIT(msg, b) (((msg)->data[((b) / 8U)] >> ((b) % 8U)) & 0x1U)
 #define GET_BYTE(msg, b) ((msg)->data[(b)])
 #define GET_BYTES_04(msg) ((msg)->data[0] | ((msg)->data[1] << 8U) | ((msg)->data[2] << 16U) | ((msg)->data[3] << 24U))

python/__init__.py

@@ -29,18 +29,18 @@
 
 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")

tests/libpanda/SConscript

@@ -6,7 +6,7 @@ env = Environment(
     '-std=gnu11',
     '-Wfatal-errors',
   ],
-  CPPPATH=[".", "#board/"],
+  CPPPATH=[".", "../../board/"],
 )
 
 env.SharedLibrary("libpanda.so", ["panda.c",])

tests/usbprotocol/test_comms.py

@@ -64,7 +64,6 @@ def test_can_send_usb(self):
           self.assertEqual(len(queue_msgs), len(msgs))
           self.assertEqual(queue_msgs, msgs)
 
-  @unittest.skip("fails on current implementation")
   def test_can_receive_usb(self):
     msgs = random_can_messages(50000)
     packets = [libpanda_py.make_CANPacket(m[0], m[3], m[2]) for m in msgs]

tests/usbprotocol/test_pandalib.py

@@ -1,13 +1,14 @@
 #!/usr/bin/env python3
 import random
 import unittest
+
 from panda import pack_can_buffer, unpack_can_buffer, DLC_TO_LEN
 
 class PandaTestPackUnpack(unittest.TestCase):
   def test_panda_lib_pack_unpack(self):
     to_pack = []
     for _ in range(10000):
-      address = random.randint(1, 0x1FFFFFFF)
+      address = random.randint(1, (1 << 29) - 1)
       data = bytes([random.getrandbits(8) for _ in range(DLC_TO_LEN[random.randrange(0, len(DLC_TO_LEN))])])
       to_pack.append((address, 0, data, 0))
 
@@ -16,7 +17,7 @@ def test_panda_lib_pack_unpack(self):
     for dat in packed:
       unpacked.extend(unpack_can_buffer(dat))
 
-    assert unpacked == to_pack
+    self.assertEqual(unpacked, to_pack)
 
 if __name__ == "__main__":
   unittest.main()