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

selfdrive/boardd/panda.cc

@@ -364,12 +364,14 @@ uint8_t Panda::len_to_dlc(uint8_t len) {
 }
 
 void Panda::can_send(capnp::List<cereal::CanData>::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<cereal::CanData>::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<can_frame>& 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<can_frame>& 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<can_frame>& 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;

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;