diff --git a/panda b/panda index 2ae7b9a4d5..288e14cde9 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 2ae7b9a4d59101c0a1a9015feb3e835763db5686 +Subproject commit 288e14cde904a5755ebb32b0853631ac7fbf5968 diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 788cdf5cf1..db9bcbab51 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -169,22 +169,15 @@ static uint8_t len_to_dlc(uint8_t len) { } } -static void write_packet(uint8_t *dest, int *write_pos, const uint8_t *src, size_t size) { - for (int i = 0, &pos = *write_pos; i < size; ++i, ++pos) { - // Insert counter every 64 bytes (first byte of 64 bytes USB packet) - if (pos % USBPACKET_MAX_SIZE == 0) { - dest[pos] = pos / USBPACKET_MAX_SIZE; - pos++; - } - dest[pos] = src[i]; - } -} - void Panda::pack_can_buffer(const capnp::List::Reader &can_data_list, std::function write_func) { 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(); @@ -202,16 +195,19 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data header.data_len_code = data_len_code; header.bus = bus - bus_offset; - write_packet(send_buf, &pos, (uint8_t *)&header, sizeof(can_header)); - write_packet(send_buf, &pos, (uint8_t *)can_data.begin(), can_data.size()); + 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(); + if (pos >= USB_TX_SOFT_LIMIT) { write_func(send_buf, pos); - pos = 0; + pos = sizeof(uint32_t); } } // send remaining packets - if (pos > 0) write_func(send_buf, pos); + if (pos > sizeof(uint32_t)) write_func(send_buf, pos); } void Panda::can_send(capnp::List::Reader can_data_list) { @@ -235,20 +231,23 @@ bool Panda::can_receive(std::vector& out_vec) { bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec) { recv_buf.clear(); - for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) { - if (data[i] != i / USBPACKET_MAX_SIZE) { - LOGE("CAN: MALFORMED USB RECV PACKET"); - handle->comms_healthy = false; - return false; - } - int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i)); - recv_buf.insert(recv_buf.end(), &data[i + 1], &data[i + chunk_len]); + + if (size < sizeof(uint32_t)) { + return true; + } + + uint32_t magic; + memcpy(&magic, &data[0], sizeof(uint32_t)); + if (magic != CAN_TRANSACTION_MAGIC) { + LOGE("CAN: MALFORMED CAN RECV PACKET"); + handle->comms_healthy = false; + return false; } - int pos = 0; - while (pos < recv_buf.size()) { + int pos = sizeof(uint32_t); + while (pos < size) { can_header header; - memcpy(&header, &recv_buf[pos], CANPACKET_HEAD_SIZE); + memcpy(&header, &data[pos], sizeof(can_header)); can_frame &canData = out_vec.emplace_back(); canData.busTime = 0; @@ -262,9 +261,9 @@ bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &o } const uint8_t data_len = dlc_to_len[header.data_len_code]; - canData.dat.assign((char *)&recv_buf[pos + CANPACKET_HEAD_SIZE], data_len); + canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len); - pos += CANPACKET_HEAD_SIZE + data_len; + pos += sizeof(can_header) + data_len; } return true; } diff --git a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc index c7d14fb0ef..cc3b4bce9a 100644 --- a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc +++ b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc @@ -48,7 +48,7 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState 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(); + total_pakets_size += sizeof(can_header) + dat.size(); } can_data_list = can_list.asReader(); @@ -58,14 +58,11 @@ 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) { - 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, size_left); - unpacked_data.insert(unpacked_data.end(), &chunk[i + 1], &chunk[i + len]); - size_left -= len; - } + 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]); }); REQUIRE(unpacked_data.size() == total_pakets_size); @@ -73,9 +70,9 @@ void PandaTest::test_can_send() { 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); + memcpy(&header, &unpacked_data[pos], sizeof(can_header)); const uint8_t data_len = dlc_to_len[header.data_len_code]; - pckt_len = CANPACKET_HEAD_SIZE + data_len; + pckt_len = sizeof(can_header) + data_len; REQUIRE(header.addr == cnt); REQUIRE(test_data.find(data_len) != test_data.end());