From 8844b560d0c4db62a2694214793631c0163978c6 Mon Sep 17 00:00:00 2001 From: Igor Biletskyy Date: Wed, 17 Nov 2021 16:00:18 -0800 Subject: [PATCH] panda.cc: struct for CAN header, counter complexity, cleanup for readability (#22956) * try struct * can_send refactor * cleanups * Resize vector only when it is needed * ... * more cleanup old-commit-hash: 47d0d717ebdef8a0b3c472bfc17b5cd1b401c5ed --- selfdrive/boardd/panda.cc | 54 ++++++++++++++++++++++----------------- selfdrive/boardd/panda.h | 17 +++++++++--- 2 files changed, 45 insertions(+), 26 deletions(-) diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 546a7904f2..f101b2e3b2 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -364,12 +364,14 @@ uint8_t Panda::len_to_dlc(uint8_t len) { } void Panda::can_send(capnp::List::Reader can_data_list) { - send.resize(72 * can_data_list.size()); // TODO: need to include 1 byte for each usb 64bytes frame + if (send.size() < (can_data_list.size() * CANPACKET_MAX_SIZE)) { + send.resize(can_data_list.size() * CANPACKET_MAX_SIZE); + } int msg_count = 0; while (msg_count < can_data_list.size()) { uint32_t pos = 0; - while (pos < 256) { + while (pos < USB_TX_SOFT_LIMIT) { if (msg_count == can_data_list.size()) { break; } auto cmsg = can_data_list[msg_count]; @@ -384,26 +386,31 @@ void Panda::can_send(capnp::List::Reader can_data_list) { assert(can_data.size() <= (hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8); assert(can_data.size() == dlc_to_len[data_len_code]); - *(uint32_t*)&send[pos+1] = (cmsg.getAddress() << 3); - if (cmsg.getAddress() >= 0x800) { - *(uint32_t*)&send[pos+1] |= (1 << 2); - } - send[pos] = data_len_code << 4 | ((bus - bus_offset) << 1); - memcpy(&send[pos+5], can_data.begin(), can_data.size()); + can_header header; + header.addr = cmsg.getAddress(); + header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0; + header.data_len_code = data_len_code; + header.bus = bus - bus_offset; + memcpy(&send[pos], &header, CANPACKET_HEAD_SIZE); + memcpy(&send[pos+CANPACKET_HEAD_SIZE], can_data.begin(), can_data.size()); pos += CANPACKET_HEAD_SIZE + dlc_to_len[data_len_code]; msg_count++; } if (pos > 0) { // Helps not to spam with ZLP - // insert counter + // Counter needs to be inserted every 64 bytes (first byte of 64 bytes USB packet) uint8_t counter = 0; - for (int i = 0; i < pos; i += 64) { - send.insert(send.begin() + i, counter); + uint8_t to_write[USB_TX_SOFT_LIMIT+128]; + int ptr = 0; + for (int i = 0; i < pos; i += 63) { + to_write[ptr] = counter; + int copy_size = ((pos - i) < 63) ? (pos - i) : 63; + memcpy(&to_write[ptr+1], &(send.data()[i]) , copy_size); + ptr += copy_size + 1; counter++; - pos++; } - usb_bulk_write(3, (uint8_t*)send.data(), pos, 5); + usb_bulk_write(3, to_write, ptr, 5); } } } @@ -415,7 +422,6 @@ bool Panda::can_receive(std::vector& out_vec) { // Not sure if this can happen if (recv < 0) recv = 0; - // TODO: Might change from full to overloaded? if > some threshold that is lower than RECV_SIZE, let's say 80-90% if (recv == RECV_SIZE) { LOGW("Receive buffer full"); } @@ -426,17 +432,18 @@ bool Panda::can_receive(std::vector& out_vec) { out_vec.reserve(out_vec.size() + (recv / CANPACKET_HEAD_SIZE)); - static uint8_t tail[72]; + static uint8_t tail[CANPACKET_MAX_SIZE]; uint8_t tail_size = 0; uint8_t counter = 0; for (int i = 0; i < recv; i += 64) { + // 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) > 64) ? 63 : (recv - i - 1); // as 1 is always reserved for counter - uint8_t chunk[72]; + uint8_t chunk[CANPACKET_MAX_SIZE]; memcpy(chunk, tail, tail_size); memcpy(&chunk[tail_size], &data[i+1], chunk_len); chunk_len += tail_size; @@ -447,21 +454,22 @@ bool Panda::can_receive(std::vector& out_vec) { uint8_t pckt_len = CANPACKET_HEAD_SIZE + data_len; if (pckt_len <= (chunk_len - pos)) { can_frame canData; + can_header header; + memcpy(&header, &chunk[pos], CANPACKET_HEAD_SIZE); canData.busTime = 0; - canData.address = (*(uint32_t*)&chunk[pos+1]) >> 3; - canData.src = ((chunk[pos] >> 1) & 0x7) + bus_offset; + canData.address = header.addr; + canData.src = header.bus + bus_offset; - bool rejected = chunk[pos+1] & 0x1; - bool returned = (chunk[pos+1] >> 1) & 0x1; - if (rejected) { canData.src += CANPACKET_REJECTED; } - if (returned) { canData.src += CANPACKET_RETURNED; } - canData.dat.assign((char*)&chunk[pos+5], data_len); + 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; // add to vector out_vec.push_back(canData); } else { + // Keep partial CAN packet until next USB packet tail_size = (chunk_len - pos); memcpy(tail, &chunk[pos], tail_size); break; diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index e42955d2b2..415bb30103 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -13,11 +13,12 @@ #include "cereal/gen/cpp/car.capnp.h" #include "cereal/gen/cpp/log.capnp.h" -// double the FIFO size -#define RECV_SIZE (0x4000) #define TIMEOUT 0 #define PANDA_BUS_CNT 4 -#define CANPACKET_HEAD_SIZE (0x5U) +#define RECV_SIZE (0x4000U) +#define USB_TX_SOFT_LIMIT (0x100U) +#define CANPACKET_HEAD_SIZE 5U +#define CANPACKET_MAX_SIZE 72U #define CANPACKET_REJECTED (0xC0U) #define CANPACKET_RETURNED (0x80U) @@ -44,6 +45,16 @@ struct __attribute__((packed)) health_t { uint8_t heartbeat_lost; }; +struct __attribute__((packed)) can_header { + uint8_t reserved : 1; + uint8_t bus : 3; + uint8_t data_len_code : 4; + uint8_t rejected : 1; + uint8_t returned : 1; + uint8_t extended : 1; + uint32_t addr : 29; +}; + struct can_frame { long address; std::string dat;