boardd: verify SPI checksum (#26454)

* verify checksum

* import from panda

Co-authored-by: Comma Device <device@comma.ai>

selfdrive/boardd/panda_comms.h

@@ -5,11 +5,16 @@
 #include <cstdint>
 #include <vector>
 
+#include <linux/spi/spidev.h>
+
 #include <libusb-1.0/libusb.h>
 
+
 #define TIMEOUT 0
 #define SPI_BUF_SIZE 1024
 
+const bool PANDA_NO_RETRY = getenv("PANDA_NO_RETRY");
+
 
 // comms base class
 class PandaCommsHandle {
@@ -65,9 +70,10 @@ public:
 
 private:
   int spi_fd = -1;
-  int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len);
-  int wait_for_ack();
-
   uint8_t tx_buf[SPI_BUF_SIZE];
   uint8_t rx_buf[SPI_BUF_SIZE];
+
+  int wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack);
+  int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len);
+  int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len);
 };

selfdrive/boardd/spi.cc

@@ -6,6 +6,7 @@
 
 #include "common/util.h"
 #include "common/swaglog.h"
+#include "panda/board/comms_definitions.h"
 #include "selfdrive/boardd/panda_comms.h"
 
 
@@ -22,13 +23,6 @@ struct __attribute__((packed)) spi_header {
   uint16_t max_rx_len;
 };
 
-struct __attribute__((packed)) spi_control_packet {
-  uint16_t request;
-  uint16_t param1;
-  uint16_t param2;
-  uint16_t length;
-};
-
 
 PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) {
   LOGD("opening SPI panda: %s", serial.c_str());
@@ -40,7 +34,7 @@ PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) {
 
   spi_fd = open(serial.c_str(), O_RDWR);
   if (spi_fd < 0) {
-    LOGE("failed setting SPI mode %d", err);
+    LOGE("failed opening SPI device %d", err);
     goto fail;
   }
 
@@ -85,41 +79,23 @@ void PandaSpiHandle::cleanup() {
 
 
 int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout) {
-  int err;
+  ControlPacket_t packet = {
-
-  std::lock_guard lk(hw_lock);
-  do {
-    spi_control_packet packet = {
     .request = request,
     .param1 = param1,
     .param2 = param2,
     .length = 0
   };
-
+  return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), NULL, 0);
-    // TODO: handle error
-    err = spi_transfer(0, (uint8_t *) &packet, sizeof(packet), NULL, 0);
-  } while (err < 0 && connected);
-
-  return err;
 }
 
 int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout) {
-  int err;
+  ControlPacket_t packet = {
-
-  std::lock_guard lk(hw_lock);
-  do {
-    spi_control_packet packet = {
     .request = request,
     .param1 = param1,
     .param2 = param2,
     .length = length
   };
-
+  return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), data, length);
-    // TODO: handle error
-    err = spi_transfer(0, (uint8_t *) &packet, sizeof(packet), data, length);
-  } while (err < 0 && connected);
-
-  return err;
 }
 
 int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
@@ -144,6 +120,46 @@ void add_checksum(uint8_t *data, int data_len) {
   }
 }
 
+bool check_checksum(uint8_t *data, int data_len) {
+  uint8_t checksum = SPI_CHECKSUM_START;
+  for (uint16_t i = 0U; i < data_len; i++) {
+    checksum ^= data[i];
+  }
+  return checksum == 0U;
+}
+
+
+int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len) {
+  int err;
+
+  std::lock_guard lk(hw_lock);
+  do {
+    // TODO: handle error
+    err = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len);
+  } while (err < 0 && connected && !PANDA_NO_RETRY);
+
+  return err;
+}
+
+int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) {
+  // TODO: add timeout?
+  while (true) {
+    int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
+    if (ret < 0) {
+      LOGE("SPI: failed to send ACK request");
+      return ret;
+    }
+
+    if (rx_buf[0] == ack) {
+      break;
+    } else if (rx_buf[0] == SPI_NACK) {
+      LOGW("SPI: got header NACK");
+      return -1;
+    }
+  }
+
+  return 0;
+}
 
 int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len) {
   int ret;
@@ -178,20 +194,10 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
   // Wait for (N)ACK
   tx_buf[0] = 0x12;
   transfer.len = 1;
-  while (true) {
+  ret = wait_for_ack(transfer, SPI_HACK);
-    ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
   if (ret < 0) {
-      LOGE("SPI: failed to send ACK request");
-      goto transfer_fail;
-    }
-
-    if (rx_buf[0] == SPI_HACK) {
-      break;
-    } else if (rx_buf[0] == SPI_NACK) {
-      LOGW("SPI: got header NACK");
     goto transfer_fail;
   }
-  }
 
   // Send data
   if (tx_data != NULL) {
@@ -208,44 +214,40 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
   // Wait for (N)ACK
   tx_buf[0] = 0xab;
   transfer.len = 1;
-  while (true) {
+  ret = wait_for_ack(transfer, SPI_DACK);
-    ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
   if (ret < 0) {
-      LOGE("SPI: failed to send ACK request");
     goto transfer_fail;
   }
 
-    if (rx_buf[0] == SPI_DACK) {
-      break;
-    } else if (rx_buf[0] == SPI_NACK) {
-      LOGE("SPI: got data NACK");
-      goto transfer_fail;
-    }
-  }
-
   // Read data len
   transfer.len = 2;
+  transfer.rx_buf = (uint64_t)(rx_buf + 1);
   ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
   if (ret < 0) {
     LOGE("SPI: failed to read rx data len");
     goto transfer_fail;
   }
-  rx_data_len = *(uint16_t *)rx_buf;
+  rx_data_len = *(uint16_t *)(rx_buf+1);
   assert(rx_data_len < SPI_BUF_SIZE);
 
   // Read data
   transfer.len = rx_data_len + 1;
+  transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1);
   ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
   if (ret < 0) {
     LOGE("SPI: failed to read rx data");
     goto transfer_fail;
   }
-  // TODO: check checksum
+  if (!check_checksum(rx_buf, rx_data_len + 4)) {
+    LOGE("SPI: bad checksum");
+    goto transfer_fail;
+  }
 
   if (rx_data != NULL) {
-    memcpy(rx_data, rx_buf, rx_data_len);
+    memcpy(rx_data, rx_buf + 3, rx_data_len);
   }
-  ret = rx_data_len;
+
+  return rx_data_len;
 
 transfer_fail:
   return ret;