New usb protocol for panda (#22752)

* first try

* move dlc_to_len

* panda switch to mailbox branch

* add bitstruct to pipfile

* bump panda and remove bitstruct

* few cleanups and fixes

* sanity check data.size == dlc_to_len

* check problem with test

* Revert "check problem with test"

This reverts commit b4855b7c12188d36d5d510e729344dab2cf21be9.

* clean

* revert Pipfile.lock

* that was an issue???

* fix crash

* ...

* include dlc_to_len from panda

* alph

* define macros for returned and rejected

* cleanup

* bump panda to current master
old-commit-hash: e293ccf016

panda

@@ -1 +1 @@
-Subproject commit 73bbf5856769a0b16c22a185635dafbe6a4aa152
+Subproject commit ae26b75d7bac3d57b060f9fdc48b591fa23e0a25

selfdrive/boardd/panda.cc

@@ -7,6 +7,7 @@
 #include <vector>
 
 #include "cereal/messaging/messaging.h"
+#include "panda/board/dlc_to_len.h"
 #include "selfdrive/common/gpio.h"
 #include "selfdrive/common/swaglog.h"
 #include "selfdrive/common/util.h"
@@ -343,42 +344,70 @@ void Panda::send_heartbeat() {
   usb_write(0xf3, 1, 0);
 }
 
+uint8_t Panda::len_to_dlc(uint8_t len) {
+  if (len <= 8) {
+    return len;
+  }
+  if (len <= 24) {
+    return 8 + ((len - 8) / 4) + (len % 4) ? 1 : 0;
+  } else {
+    return 11 + (len / 16) + (len % 16) ? 1 : 0;
+  }
+}
+
 void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
-  send.resize(4 * can_data_list.size());
+  send.resize(72 * can_data_list.size()); // TODO: need to include 1 byte for each usb 64bytes frame
 
-  uint32_t msg_cnt = 0;
+  int msg_count = 0;
-  for (int i = 0; i < can_data_list.size(); i++) {
+  while (msg_count < can_data_list.size()) {
-    auto cmsg = can_data_list[i];
+    uint32_t pos = 0;
+    while (pos < 256) {
+      if (msg_count == can_data_list.size()) { break; }
+      auto cmsg = can_data_list[msg_count];
 
       // 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();
+      uint8_t data_len_code = len_to_dlc(can_data.size());
+      assert(can_data.size() <= (hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8);
+      assert(can_data.size() == dlc_to_len[data_len_code]);
 
-    if (cmsg.getAddress() >= 0x800) { // extended
+      *(uint32_t*)&send[pos+1] = (cmsg.getAddress() << 3);
-      send[msg_cnt*4] = (cmsg.getAddress() << 3) | 5;
+      if (cmsg.getAddress() >= 0x800) {
-    } else { // normal
+        *(uint32_t*)&send[pos+1] |= (1 << 2);
-      send[msg_cnt*4] = (cmsg.getAddress() << 21) | 1;
       }
-    auto can_data = cmsg.getDat();
+      send[pos] = data_len_code << 4 | ((bus - bus_offset) << 1);
-    assert(can_data.size() <= 8);
+      memcpy(&send[pos+5], can_data.begin(), can_data.size());
-    send[msg_cnt*4+1] = can_data.size() | ((bus - bus_offset) << 4);
-    memcpy(&send[msg_cnt*4+2], can_data.begin(), can_data.size());
 
-    msg_cnt++;
+      pos += CANPACKET_HEAD_SIZE + dlc_to_len[data_len_code];
+      msg_count++;
     }
 
-  usb_bulk_write(3, (unsigned char*)send.data(), msg_cnt * 0x10, 5);
+    if (pos > 0) { // Helps not to spam with ZLP
+      // insert counter
+      uint8_t counter = 0;
+      for (int i = 0; i < pos; i += 64) {
+        send.insert(send.begin() + i, counter);
+        counter++;
+        pos++;
+      }
+      usb_bulk_write(3, (uint8_t*)send.data(), pos, 5);
+    }
+  }
 }
 
 bool Panda::can_receive(std::vector<can_frame>& out_vec) {
-  uint32_t data[RECV_SIZE/4];
+  uint8_t data[RECV_SIZE];
-  int recv = usb_bulk_read(0x81, (unsigned char*)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;
 
+  // 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");
   }
@@ -387,30 +416,49 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
     return false;
   }
 
-  // Append to the end of the out_vec, such that we can pass it to multiple pandas
+  out_vec.reserve(out_vec.size() + (recv / CANPACKET_HEAD_SIZE));
-  // We already insert space for all the messages here for speed
-  size_t num_msg = recv / 0x10;
-  out_vec.reserve(out_vec.size() + num_msg);
 
-  // Populate messages
+  static uint8_t tail[72];
-  for (int i = 0; i < num_msg; i++) {
+  uint8_t tail_size = 0;
-    can_frame canData;
+  uint8_t counter = 0;
-    if (data[i*4] & 4) {
+  for (int i = 0; i < recv; i += 64) {
-      // extended
+    if (counter != data[i]) {
-      canData.address = data[i*4] >> 3;
+      LOGE("CAN: MALFORMED USB RECV PACKET");
-      //printf("got extended: %x\n", data[i*4] >> 3);
+      break;
-    } else {
-      // normal
-      canData.address = data[i*4] >> 21;
     }
-    canData.busTime = data[i*4+1] >> 16;
+    counter++;
-    int len = data[i*4+1] & 0xF;
+    uint8_t chunk_len = ((recv - i) > 64) ? 63 : (recv - i - 1); // as 1 is always reserved for counter
-    canData.dat.assign((char *)&data[i*4+2], len);
+    uint8_t chunk[72];
-    canData.src = ((data[i*4+1] >> 4) & 0xff) + bus_offset;
+    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_frame canData;
+        canData.busTime = 0;
+        canData.address = (*(uint32_t*)&chunk[pos+1]) >> 3;
+        canData.src = (chunk[pos] >> 1) & 0x7;
+
+        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);
+
+        pos += pckt_len;
 
         // add to vector
         out_vec.push_back(canData);
+      } else {
+        tail_size = (chunk_len - pos);
+        memcpy(tail, &chunk[pos], tail_size);
+        break;
+      }
+    }
   }
-
   return true;
 }

selfdrive/boardd/panda.h

@@ -14,9 +14,12 @@
 #include "cereal/gen/cpp/log.capnp.h"
 
 // double the FIFO size
-#define RECV_SIZE (0x1000)
+#define RECV_SIZE (0x4000)
 #define TIMEOUT 0
 #define PANDA_BUS_CNT 4
+#define CANPACKET_HEAD_SIZE (0x5U)
+#define CANPACKET_REJECTED  (0xC0U)
+#define CANPACKET_RETURNED  (0x80U)
 
 // copied from panda/board/main.c
 struct __attribute__((packed)) health_t {
@@ -53,7 +56,7 @@ class Panda {
   libusb_context *ctx = NULL;
   libusb_device_handle *dev_handle = NULL;
   std::mutex usb_lock;
-  std::vector<uint32_t> send;
+  std::vector<uint8_t> send;
   void handle_usb_issue(int err, const char func[]);
   void cleanup();
 
@@ -93,6 +96,7 @@ class Panda {
   void set_power_saving(bool power_saving);
   void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
   void send_heartbeat();
+  uint8_t len_to_dlc(uint8_t len);
   void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
   bool can_receive(std::vector<can_frame>& out_vec);
 };