panda: refactor can_send (#22981)

* refactor

* merge master

* fix

* move  down

* space

* merge master

* remove struct PacketWriter, use function

* Update selfdrive/boardd/panda.cc

less violations

* Update selfdrive/boardd/panda.cc

comment

* Update selfdrive/boardd/panda.cc

add comment to counter

Co-authored-by: Igor Biletskyy <briskspirit@users.noreply.github.com>

selfdrive/boardd/panda.cc

@@ -363,23 +363,26 @@ static uint8_t len_to_dlc(uint8_t len) {
   }
 }
 
-void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
+static void write_packet(uint8_t *dest, int *write_pos, const uint8_t *src, size_t size) {
-                         std::function<void(uint8_t *, size_t)> write_func) {
+  for (int i = 0, &pos = *write_pos; i < size; ++i, ++pos) {
-  if (send.size() < (can_data_list.size() * CANPACKET_MAX_SIZE)) {
+    // Insert counter every 64 bytes (first byte of 64 bytes USB packet)
-    send.resize(can_data_list.size() * CANPACKET_MAX_SIZE);
+    if (pos % USBPACKET_MAX_SIZE == 0) {
+      dest[pos] = pos / USBPACKET_MAX_SIZE;
+      pos++;
+    }
+    dest[pos] = src[i];
   }
+}
 
-  int msg_count = 0;
+void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
-  while (msg_count < can_data_list.size()) {
+                            std::function<void(uint8_t *, size_t)> write_func) {
-    uint32_t pos = 0;
+  int32_t pos = 0;
-    while (pos < USB_TX_SOFT_LIMIT) {
+  uint8_t send_buf[2 * USB_TX_SOFT_LIMIT];
-      if (msg_count == can_data_list.size()) { break; }
-      auto cmsg = can_data_list[msg_count];
 
+  for (auto cmsg : can_data_list) {
     // check if the message is intended for this panda
     uint8_t bus = cmsg.getSrc();
     if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_CNT)) {
-        msg_count++;
       continue;
     }
     auto can_data = cmsg.getDat();
@@ -392,28 +395,17 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
     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
+    write_packet(send_buf, &pos, (uint8_t *)&header, sizeof(can_header));
-      // Counter needs to be inserted every 64 bytes (first byte of 64 bytes USB packet)
+    write_packet(send_buf, &pos, (uint8_t *)can_data.begin(), can_data.size());
-      uint8_t counter = 0;
+    if (pos >= USB_TX_SOFT_LIMIT) {
-      uint8_t to_write[USB_TX_SOFT_LIMIT+128];
+      write_func(send_buf, pos);
-      int ptr = 0;
+      pos = 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++;
-      }
-      write_func(to_write, ptr);
     }
   }
+
+  // send remaining packets
+  if (pos > 0) write_func(send_buf, pos);
 }
 
 void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {

selfdrive/boardd/panda.h

@@ -69,7 +69,6 @@ class Panda {
   libusb_context *ctx = NULL;
   libusb_device_handle *dev_handle = NULL;
   std::mutex usb_lock;
-  std::vector<uint8_t> send;
   std::vector<uint8_t> recv_buf;
   void handle_usb_issue(int err, const char func[]);
   void cleanup();