From c77354009c61da8a322dc9aaaa4c1ecaec432449 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Wed, 24 Nov 2021 12:15:22 +0800 Subject: [PATCH] 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 | 1 + selfdrive/boardd/.gitignore | 1 + selfdrive/boardd/SConscript | 5 +- selfdrive/boardd/panda.cc | 19 ++- selfdrive/boardd/panda.h | 8 ++ .../boardd/tests/test_boardd_usbprotocol.cc | 115 ++++++++++++++++++ 6 files changed, 144 insertions(+), 5 deletions(-) create mode 100644 selfdrive/boardd/tests/test_boardd_usbprotocol.cc diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index aa217735b..3b0cc2996 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.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 && \ diff --git a/selfdrive/boardd/.gitignore b/selfdrive/boardd/.gitignore index 1f653bde8..e8daa2ef2 100644 --- a/selfdrive/boardd/.gitignore +++ b/selfdrive/boardd/.gitignore @@ -1,2 +1,3 @@ boardd boardd_api_impl.cpp +tests/test_boardd_usbprotocol diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index f2a1f3f7b..07ded56e1 100644 --- a/selfdrive/boardd/SConscript +++ b/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) diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 78b123f69..6dc458a8c 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -363,7 +363,8 @@ uint8_t Panda::len_to_dlc(uint8_t len) { } } -void Panda::can_send(capnp::List::Reader can_data_list) { +void Panda::pack_can_buffer(const capnp::List::Reader &can_data_list, + std::function 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::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::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& 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& 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 &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); diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 4be9454cf..9be454a93 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -113,4 +114,11 @@ class Panda { uint8_t len_to_dlc(uint8_t len); void can_send(capnp::List::Reader can_data_list); bool can_receive(std::vector& out_vec); + +protected: + // for unit tests + Panda(uint32_t bus_offset) : bus_offset(bus_offset) {} + void pack_can_buffer(const capnp::List::Reader &can_data_list, + std::function write_func); + bool unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec); }; diff --git a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc new file mode 100644 index 000000000..58cea3c68 --- /dev/null +++ b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc @@ -0,0 +1,115 @@ +#define CATCH_CONFIG_MAIN +#define CATCH_CONFIG_ENABLE_BENCHMARKING +#include + +#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 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 test_data; + int can_list_size = 0; + int total_pakets_size = 0; + MessageBuilder msg; + capnp::List::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 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 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 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(); + } +}