From f2cab9a00c8b9da221c39bb8b5b525b1123a3100 Mon Sep 17 00:00:00 2001 From: Robbe Derks Date: Fri, 13 Jan 2023 15:01:55 -0800 Subject: [PATCH] No more magic for the can chunks (#26861) * remove magic and add checksum * add comms reset * bump submodule old-commit-hash: 3136985b950e79cf2943dafa2cea4db5ad0f528b --- panda | 2 +- selfdrive/boardd/panda.cc | 71 ++++++++++++------- selfdrive/boardd/panda.h | 8 ++- .../boardd/tests/test_boardd_usbprotocol.cc | 37 +++++++--- 4 files changed, 80 insertions(+), 38 deletions(-) diff --git a/panda b/panda index 0b3b906036..11d90f9e78 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 0b3b9060365739eeeddb1528cfe4018fec4efe7a +Subproject commit 11d90f9e78b1c070e44e02d5d8c2b18790617324 diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 3e447c830e..d4d53aa230 100644 --- a/selfdrive/boardd/panda.cc +++ b/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::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::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], (uint8_t *)can_data.begin(), can_data.size()); - pos += can_data.size(); + memcpy(&send_buf[pos + sizeof(can_header)], (uint8_t *)can_data.begin(), can_data.size()); + uint32_t msg_size = sizeof(can_header) + 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::Reader can_data_list) { @@ -217,36 +220,35 @@ void Panda::can_send(capnp::List::Reader can_data_list) { } bool Panda::can_receive(std::vector& out_vec) { - uint8_t data[RECV_SIZE]; - int recv = handle->bulk_read(0x81, (uint8_t*)data, RECV_SIZE); + int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], 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 &out_vec) { - if (size < sizeof(uint32_t)) { - return true; - } +void Panda::can_reset_communications() { + handle->control_write(0xc0, 0, 0); +} - uint32_t magic; - memcpy(&magic, &data[0], sizeof(uint32_t)); - if (magic != CAN_TRANSACTION_MAGIC) { - LOGE("CAN recv: buffer didn't start with magic"); - handle->comms_healthy = false; - return false; - } +bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec) { + int pos = 0; - int pos = sizeof(uint32_t); - while (pos < size) { + while (pos <= size - sizeof(can_header)) { 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 &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; +} diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 75fec57a3e..156efe7ef3 100644 --- a/selfdrive/boardd/panda.h +++ b/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::Reader can_data_list); bool can_receive(std::vector& 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::Reader &can_data_list, std::function write_func); - bool unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec); + bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec); + uint8_t calculate_checksum(uint8_t *data, uint32_t len); }; diff --git a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc index cc3b4bce9a..3b78eb7bd2 100644 --- a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc +++ b/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 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 unpacked_data; this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) { - uint32_t magic; - memcpy(&magic, &chunk[0], sizeof(uint32_t)); - - REQUIRE(magic == CAN_TRANSACTION_MAGIC); - unpacked_data.insert(unpacked_data.end(), &chunk[sizeof(uint32_t)], &chunk[size]); + unpacked_data.insert(unpacked_data.end(), chunk, &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 frames; - this->pack_can_buffer(can_data_list, [&](uint8_t *data, size_t size) { - this->unpack_can_buffer(data, size, frames); + this->pack_can_buffer(can_data_list, [&](uint8_t *data, uint32_t size) { + 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); + } }