panda: add unit tests for usb protocol (pack/unpack) (#22955)

* prepare for unit tests

* add to selfdrive_tests.yaml

* test header

* test chunk count

* rename test function

* continue

* don't check chunks count

* test recv_can

* continue

* small cleanup

* merge master

* cleanup

* rename functions

* test different packet size

* fix operator precedence problem

* refactor unpack_can_buffer

* cleanup test

* cleanup unpack_can_buffer

* add test for multiple pandas

* rename to test_panda

* restore test_boardd

* rename to test_boardd_usbprotocol

* fix typo

* bus_offset = [0,4]

* change src

* use USBPACKET_MAX_SIZE

.github/workflows/selfdrive_tests.yaml

@@ -247,6 +247,7 @@ jobs:
                         $UNIT_TEST selfdrive/athena && \
                         $UNIT_TEST selfdrive/thermald && \
                         $UNIT_TEST tools/lib/tests && \
+                        ./selfdrive/boardd/tests/test_boardd_usbprotocol && \
                         ./selfdrive/common/tests/test_util && \
                         ./selfdrive/loggerd/tests/test_logger &&\
                         ./selfdrive/proclogd/tests/test_proclog && \

selfdrive/boardd/.gitignore

@@ -1,2 +1,3 @@
 boardd
 boardd_api_impl.cpp
+tests/test_boardd_usbprotocol

selfdrive/boardd/SConscript

@@ -1,6 +1,9 @@
 Import('env', 'envCython', 'common', 'cereal', 'messaging')
 
-env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
+libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']
+env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs)
 env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
 
 envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
+if GetOption('test'):
+  env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc'], LIBS=libs)

selfdrive/boardd/panda.cc

@@ -363,7 +363,8 @@ uint8_t Panda::len_to_dlc(uint8_t len) {
   }
 }
 
-void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
+void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
+                         std::function<void(uint8_t *, size_t)> write_func) {
   if (send.size() < (can_data_list.size() * CANPACKET_MAX_SIZE)) {
     send.resize(can_data_list.size() * CANPACKET_MAX_SIZE);
   }
@@ -410,11 +411,17 @@ void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
         ptr += copy_size + 1;
         counter++;
       }
-      usb_bulk_write(3, to_write, ptr, 5);
+      write_func(to_write, ptr);
     }
   }
 }
 
+void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
+  pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) {
+    usb_bulk_write(3, data, size, 5);
+  });
+}
+
 bool Panda::can_receive(std::vector<can_frame>& out_vec) {
   uint8_t data[RECV_SIZE];
   int recv = usb_bulk_read(0x81, (uint8_t*)data, RECV_SIZE);
@@ -429,18 +436,22 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
   if (!comms_healthy) {
     return false;
   }
+  return unpack_can_buffer(data, recv, out_vec);
+}
+
+bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &out_vec) {
 
   static uint8_t tail[CANPACKET_MAX_SIZE];
   uint8_t tail_size = 0;
   uint8_t counter = 0;
-  for (int i = 0; i < recv; i += USBPACKET_MAX_SIZE) {
+  for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) {
     // Check for counter every 64 bytes (length of USB packet)
     if (counter != data[i]) {
       LOGE("CAN: MALFORMED USB RECV PACKET");
       break;
     }
     counter++;
-    uint8_t chunk_len = ((recv - i) > USBPACKET_MAX_SIZE) ? 63 : (recv - i - 1); // as 1 is always reserved for counter
+    uint8_t chunk_len = ((size - i) > USBPACKET_MAX_SIZE) ? 63 : (size - i - 1); // as 1 is always reserved for counter
     uint8_t chunk[USBPACKET_MAX_SIZE + CANPACKET_MAX_SIZE];
     memcpy(chunk, tail, tail_size);
     memcpy(&chunk[tail_size], &data[i+1], chunk_len);

selfdrive/boardd/panda.h

@@ -3,6 +3,7 @@
 #include <atomic>
 #include <cstdint>
 #include <ctime>
+#include <functional>
 #include <list>
 #include <mutex>
 #include <optional>
@@ -113,4 +114,11 @@ class Panda {
   uint8_t len_to_dlc(uint8_t len);
   void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
   bool can_receive(std::vector<can_frame>& out_vec);
+
+protected:
+  // for unit tests
+  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);
 };

selfdrive/boardd/tests/test_boardd_usbprotocol.cc

@@ -0,0 +1,115 @@
+#define CATCH_CONFIG_MAIN
+#define CATCH_CONFIG_ENABLE_BENCHMARKING
+#include <random>
+
+#include "catch2/catch.hpp"
+#include "cereal/messaging/messaging.h"
+#include "selfdrive/boardd/panda.h"
+
+const unsigned char dlc_to_len[] = {0U, 1U, 2U, 3U, 4U, 5U, 6U, 7U, 8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U};
+
+int random_int(int min, int max) {
+  std::random_device dev;
+  std::mt19937 rng(dev());
+  std::uniform_int_distribution<std::mt19937::result_type> dist(min, max);
+  return dist(rng);
+}
+
+struct PandaTest : public Panda {
+  PandaTest(uint32_t bus_offset, int can_list_size);
+  void test_can_send();
+  void test_can_recv();
+
+  std::map<int, std::string> test_data;
+  int can_list_size = 0;
+  int total_pakets_size = 0;
+  MessageBuilder msg;
+  capnp::List<cereal::CanData>::Reader can_data_list;
+};
+
+PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size) : can_list_size(can_list_size), Panda(bus_offset_) {
+  // prepare test data
+  for (int i = 0; i < std::size(dlc_to_len); ++i) {
+    std::random_device rd;
+    std::independent_bits_engine<std::default_random_engine, CHAR_BIT, unsigned char> rbe(rd());
+
+    int data_len = dlc_to_len[i];
+    std::string bytes(data_len, '\0');
+    std::generate(bytes.begin(), bytes.end(), std::ref(rbe));
+    test_data[data_len] = bytes;
+  }
+
+  // generate can messages for this panda
+  auto can_list = msg.initEvent().initSendcan(can_list_size);
+  for (uint8_t i = 0; i < can_list_size; ++i) {
+    auto can = can_list[i];
+    uint32_t id = random_int(0, std::size(dlc_to_len) - 1);
+    const std::string &dat = test_data[dlc_to_len[id]];
+    can.setAddress(i);
+    can.setSrc(random_int(0, 3) + bus_offset);
+    can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size()));
+    total_pakets_size += CANPACKET_HEAD_SIZE + dat.size();
+  }
+
+  can_data_list = can_list.asReader();
+  INFO("test " << can_list_size << " packets, total size " << total_pakets_size);
+}
+
+void PandaTest::test_can_send() {
+  std::vector<uint8_t> unpacked_data;
+  this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) {
+    int size_left = size;
+    for (int i = 0, counter = 0; i < size; i += USBPACKET_MAX_SIZE, counter++) {
+      REQUIRE(chunk[i] == counter);
+
+      const int len = std::min(USBPACKET_MAX_SIZE, (uint32_t)size_left);
+      unpacked_data.insert(unpacked_data.end(), &chunk[i + 1], &chunk[i + len]);
+      size_left -= len;
+    }
+  });
+  REQUIRE(unpacked_data.size() == total_pakets_size);
+
+  int cnt = 0;
+  INFO("test can message integrity");
+  for (int pos = 0, pckt_len = 0; pos < unpacked_data.size(); pos += pckt_len) {
+    can_header header;
+    memcpy(&header, &unpacked_data[pos], CANPACKET_HEAD_SIZE);
+    const uint8_t data_len = dlc_to_len[header.data_len_code];
+    pckt_len = CANPACKET_HEAD_SIZE + data_len;
+
+    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);
+    ++cnt;
+  }
+  REQUIRE(cnt == can_list_size);
+}
+
+void PandaTest::test_can_recv() {
+  std::vector<can_frame> frames;
+  this->pack_can_buffer(can_data_list, [&](uint8_t *data, size_t size) {
+    this->unpack_can_buffer(data, size, frames);
+  });
+
+  REQUIRE(frames.size() == can_list_size);
+  for (int i = 0; i < frames.size(); ++i) {
+    REQUIRE(frames[i].address == i);
+    REQUIRE(test_data.find(frames[i].dat.size()) != test_data.end());
+    const std::string &dat = test_data[frames[i].dat.size()];
+    REQUIRE(memcmp(dat.data(), frames[i].dat.data(), dat.size()) == 0);
+  }
+}
+
+TEST_CASE("send/recv can packets") {
+  auto bus_offset = GENERATE(0, 4);
+  auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200);
+  PandaTest test(bus_offset, can_list_size);
+
+  SECTION("can_send") {
+    test.test_can_send();
+  }
+  SECTION("can_receive") {
+    test.test_can_recv();
+  }
+}