diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 78e32716cf..9c59bba6fc 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -425,64 +425,44 @@ 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 = usb_bulk_read(0x81, (uint8_t*)data, RECV_SIZE); - - // Not sure if this can happen - if (recv < 0) recv = 0; - - if (recv == RECV_SIZE) { - LOGW("Receive buffer full"); - } - if (!comms_healthy) { return false; } - return unpack_can_buffer(data, recv, out_vec); + if (recv == RECV_SIZE) { + LOGW("Panda receive buffer full"); + } + + return (recv <= 0) ? true : 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; + recv_buf.clear(); for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) { - // Check for counter every 64 bytes (length of USB packet) - if (counter != data[i]) { + if (data[i] != i / USBPACKET_MAX_SIZE) { LOGE("CAN: MALFORMED USB RECV PACKET"); - break; - } - 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); - chunk_len += tail_size; - tail_size = 0; - uint8_t pos = 0; - while (pos < chunk_len) { - uint8_t data_len = dlc_to_len[(chunk[pos] >> 4)]; - uint8_t pckt_len = CANPACKET_HEAD_SIZE + data_len; - if (pckt_len <= (chunk_len - pos)) { - can_header header; - memcpy(&header, &chunk[pos], CANPACKET_HEAD_SIZE); - - can_frame &canData = out_vec.emplace_back(); - canData.busTime = 0; - canData.address = header.addr; - canData.src = header.bus + bus_offset; - - if (header.rejected) { canData.src += CANPACKET_REJECTED; } - if (header.returned) { canData.src += CANPACKET_RETURNED; } - canData.dat.assign((char*)&chunk[pos+CANPACKET_HEAD_SIZE], data_len); - - pos += pckt_len; - } else { - // Keep partial CAN packet until next USB packet - tail_size = (chunk_len - pos); - assert(tail_size <= CANPACKET_MAX_SIZE); - memcpy(tail, &chunk[pos], tail_size); - break; - } + 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]); + } + + int pos = 0; + while (pos < recv_buf.size()) { + can_header header; + memcpy(&header, &recv_buf[pos], CANPACKET_HEAD_SIZE); + + can_frame &canData = out_vec.emplace_back(); + canData.busTime = 0; + canData.address = header.addr; + canData.src = header.bus + bus_offset; + if (header.rejected) { canData.src += CANPACKET_REJECTED; } + if (header.returned) { canData.src += CANPACKET_RETURNED; } + + const uint8_t data_len = dlc_to_len[header.data_len_code]; + canData.dat.assign((char *)&recv_buf[pos + CANPACKET_HEAD_SIZE], data_len); + + pos += CANPACKET_HEAD_SIZE + data_len; } return true; } diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 9be454a930..9ac75c968a 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -18,7 +18,7 @@ #define PANDA_BUS_CNT 4 #define RECV_SIZE (0x4000U) #define USB_TX_SOFT_LIMIT (0x100U) -#define USBPACKET_MAX_SIZE (0x40U) +#define USBPACKET_MAX_SIZE (0x40) #define CANPACKET_HEAD_SIZE 5U #define CANPACKET_MAX_SIZE 72U #define CANPACKET_REJECTED (0xC0U) @@ -70,6 +70,7 @@ class Panda { libusb_device_handle *dev_handle = NULL; std::mutex usb_lock; std::vector send; + std::vector recv_buf; void handle_usb_issue(int err, const char func[]); void cleanup(); diff --git a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc index f1fd2ef902..6a13cbd71f 100644 --- a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc +++ b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc @@ -64,7 +64,7 @@ void PandaTest::test_can_send() { 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); + 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; }