No more magic for the can chunks (#26861)

* remove magic and add checksum

* add comms reset

* bump submodule

panda

@@ -1 +1 @@
-Subproject commit 0b3b9060365739eeeddb1528cfe4018fec4efe7a
+Subproject commit 11d90f9e78b1c070e44e02d5d8c2b18790617324

selfdrive/boardd/panda.cc

@@ -26,6 +26,8 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
             (hw_type == cereal::PandaState::PandaType::DOS) ||
             (hw_type == cereal::PandaState::PandaType::TRES);
 
+  can_reset_communications();
+
   return;
 }
 
@@ -174,10 +176,6 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
   int32_t pos = 0;
   uint8_t send_buf[2 * USB_TX_SOFT_LIMIT];
 
-  uint32_t magic = CAN_TRANSACTION_MAGIC;
-  memcpy(&send_buf[0], &magic, sizeof(uint32_t));
-  pos += sizeof(uint32_t);
-
   for (auto cmsg : can_data_list) {
     // check if the message is intended for this panda
     uint8_t bus = cmsg.getSrc();
@@ -194,20 +192,25 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
     header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0;
     header.data_len_code = data_len_code;
     header.bus = bus - bus_offset;
+    header.checksum = 0;
 
     memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header));
-    pos += sizeof(can_header);
+    memcpy(&send_buf[pos + sizeof(can_header)], (uint8_t *)can_data.begin(), can_data.size());
-    memcpy(&send_buf[pos], (uint8_t *)can_data.begin(), can_data.size());
+    uint32_t msg_size = sizeof(can_header) + can_data.size();
-    pos += can_data.size();
+
+    // set checksum
+    ((can_header *) &send_buf[pos])->checksum = calculate_checksum(&send_buf[pos], msg_size);
+
+    pos += msg_size;
 
     if (pos >= USB_TX_SOFT_LIMIT) {
       write_func(send_buf, pos);
-      pos = sizeof(uint32_t);
+      pos = 0;
     }
   }
 
   // send remaining packets
-  if (pos > sizeof(uint32_t)) write_func(send_buf, pos);
+  if (pos > 0) write_func(send_buf, pos);
 }
 
 void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
@@ -217,36 +220,35 @@ void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
 }
 
 bool Panda::can_receive(std::vector<can_frame>& out_vec) {
-  uint8_t data[RECV_SIZE];
+  int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE);
-  int recv = handle->bulk_read(0x81, (uint8_t*)data, RECV_SIZE);
   if (!comms_healthy()) {
     return false;
   }
   if (recv == RECV_SIZE) {
     LOGW("Panda receive buffer full");
   }
+  receive_buffer_size += recv;
 
-  return (recv <= 0) ? true : unpack_can_buffer(data, recv, out_vec);
+  return (recv <= 0) ? true : unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec);
 }
 
-bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &out_vec) {
+void Panda::can_reset_communications() {
-  if (size < sizeof(uint32_t)) {
+  handle->control_write(0xc0, 0, 0);
-    return true;
+}
-  }
 
-  uint32_t magic;
+bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec) {
-  memcpy(&magic, &data[0], sizeof(uint32_t));
+  int pos = 0;
-  if (magic != CAN_TRANSACTION_MAGIC) {
-    LOGE("CAN recv: buffer didn't start with magic");
-    handle->comms_healthy = false;
-    return false;
-  }
 
-  int pos = sizeof(uint32_t);
+  while (pos <= size - sizeof(can_header)) {
-  while (pos < size) {
     can_header header;
     memcpy(&header, &data[pos], sizeof(can_header));
 
+    const uint8_t data_len = dlc_to_len[header.data_len_code];
+    if (pos + sizeof(can_header) + data_len > size) {
+      // we don't have all the data for this message yet
+      break;
+    }
+
     can_frame &canData = out_vec.emplace_back();
     canData.busTime = 0;
     canData.address = header.addr;
@@ -258,10 +260,27 @@ bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &o
       canData.src += CAN_RETURNED_BUS_OFFSET;
     }
 
-    const uint8_t data_len = dlc_to_len[header.data_len_code];
+    if (calculate_checksum(&data[pos], sizeof(can_header) + data_len) != 0) {
+      LOGE("Panda CAN checksum failed");
+      return false;
+    }
+
     canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len);
 
     pos += sizeof(can_header) + data_len;
   }
+
+  // move the overflowing data to the beginning of the buffer for the next round
+  memmove(data, &data[pos], size - pos);
+  size -= pos;
+
   return true;
 }
+
+uint8_t Panda::calculate_checksum(uint8_t *data, uint32_t len) {
+  uint8_t checksum = 0U;
+  for (uint32_t i = 0U; i < len; i++) {
+    checksum ^= data[i];
+  }
+  return checksum;
+}

selfdrive/boardd/panda.h

@@ -30,6 +30,7 @@ struct __attribute__((packed)) can_header {
   uint8_t returned : 1;
   uint8_t extended : 1;
   uint32_t addr : 29;
+  uint8_t checksum : 8;
 };
 
 struct can_frame {
@@ -80,11 +81,16 @@ public:
   void set_canfd_non_iso(uint16_t bus, bool non_iso);
   void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
   bool can_receive(std::vector<can_frame>& out_vec);
+  void can_reset_communications();
 
 protected:
   // for unit tests
+  uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64];
+  uint32_t receive_buffer_size = 0;
+
   Panda(uint32_t bus_offset) : bus_offset(bus_offset) {}
   void pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
                          std::function<void(uint8_t *, size_t)> write_func);
-  bool unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &out_vec);
+  bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec);
+  uint8_t calculate_checksum(uint8_t *data, uint32_t len);
 };

selfdrive/boardd/tests/test_boardd_usbprotocol.cc

@@ -16,7 +16,8 @@ int random_int(int min, int max) {
 struct PandaTest : public Panda {
   PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type);
   void test_can_send();
-  void test_can_recv();
+  void test_can_recv(uint32_t chunk_size = 0);
+  void test_chunked_can_recv();
 
   std::map<int, std::string> test_data;
   int can_list_size = 0;
@@ -58,11 +59,7 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState
 void PandaTest::test_can_send() {
   std::vector<uint8_t> unpacked_data;
   this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) {
-    uint32_t magic;
+    unpacked_data.insert(unpacked_data.end(), chunk, &chunk[size]);
-    memcpy(&magic, &chunk[0], sizeof(uint32_t));
-
-    REQUIRE(magic == CAN_TRANSACTION_MAGIC);
-    unpacked_data.insert(unpacked_data.end(), &chunk[sizeof(uint32_t)], &chunk[size]);
   });
   REQUIRE(unpacked_data.size() == total_pakets_size);
 
@@ -77,16 +74,30 @@ void PandaTest::test_can_send() {
     REQUIRE(header.addr == cnt);
     REQUIRE(test_data.find(data_len) != test_data.end());
     const std::string &dat = test_data[data_len];
-    REQUIRE(memcmp(dat.data(), &unpacked_data[pos + 5], dat.size()) == 0);
+    REQUIRE(memcmp(dat.data(), &unpacked_data[pos + sizeof(can_header)], dat.size()) == 0);
     ++cnt;
   }
   REQUIRE(cnt == can_list_size);
 }
 
-void PandaTest::test_can_recv() {
+void PandaTest::test_can_recv(uint32_t rx_chunk_size) {
   std::vector<can_frame> frames;
-  this->pack_can_buffer(can_data_list, [&](uint8_t *data, size_t size) {
+  this->pack_can_buffer(can_data_list, [&](uint8_t *data, uint32_t size) {
-    this->unpack_can_buffer(data, size, frames);
+    if (rx_chunk_size == 0) {
+      REQUIRE(this->unpack_can_buffer(data, size, frames));
+    } else {
+      this->receive_buffer_size = 0;
+      uint32_t pos = 0;
+
+      while(pos < size) {
+        uint32_t chunk_size = std::min(rx_chunk_size, size - pos);
+        memcpy(&this->receive_buffer[this->receive_buffer_size], &data[pos], chunk_size);
+        this->receive_buffer_size += chunk_size;
+        pos += chunk_size;
+
+        REQUIRE(this->unpack_can_buffer(this->receive_buffer, this->receive_buffer_size, frames));
+      }
+    }
   });
 
   REQUIRE(frames.size() == can_list_size);
@@ -109,6 +120,9 @@ TEST_CASE("send/recv CAN 2.0 packets") {
   SECTION("can_receive") {
     test.test_can_recv();
   }
+  SECTION("chunked_can_receive") {
+    test.test_can_recv(0x40);
+  }
 }
 
 TEST_CASE("send/recv CAN FD packets") {
@@ -122,4 +136,7 @@ TEST_CASE("send/recv CAN FD packets") {
   SECTION("can_receive") {
     test.test_can_recv();
   }
+  SECTION("chunked_can_receive") {
+    test.test_can_recv(0x40);
+  }
 }