From 8e88116dba3b0d06726b331c801dfe5a51011afd Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Fri, 11 Nov 2022 12:51:14 -0800
Subject: [PATCH] boardd: SPI bulk read + write (#26462)

* bulk read

* write

* write

* fix write

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 714ab491b0ff90170353df99f43f09bbae1ab00d
---
 selfdrive/boardd/panda_comms.h |  3 ++-
 selfdrive/boardd/spi.cc        | 49 ++++++++++++++++++++++++++++------
 2 files changed, 43 insertions(+), 9 deletions(-)

diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h
index aef7b41d07..f42eadc5b2 100644
--- a/selfdrive/boardd/panda_comms.h
+++ b/selfdrive/boardd/panda_comms.h
@@ -34,7 +34,7 @@ public:
   virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0;
 
 protected:
-  std::mutex hw_lock;
+  std::recursive_mutex hw_lock;
 };
 
 class PandaUsbHandle : public PandaCommsHandle {
@@ -74,6 +74,7 @@ private:
   uint8_t rx_buf[SPI_BUF_SIZE];
 
   int wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack);
+  int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len);
   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);
 };
diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc
index 3969b313f0..2803f58db0 100644
--- a/selfdrive/boardd/spi.cc
+++ b/selfdrive/boardd/spi.cc
@@ -2,6 +2,7 @@
 #include <linux/spi/spidev.h>
 
 #include <cassert>
+#include <cmath>
 #include <cstring>
 
 #include "common/util.h"
@@ -99,13 +100,45 @@ int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t para
 }
 
 int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
-  return 0;
+  return bulk_transfer(endpoint, data, length, NULL, 0);
 }
-
 int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
-  return 0;
+  return bulk_transfer(endpoint, NULL, 0, data, length);
+}
+
+int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len) {
+  std::lock_guard lk(hw_lock);
+
+  const int xfer_size = 0x40;
+
+  int ret = 0;
+  uint16_t length = (tx_data != NULL) ? tx_len : rx_len;
+  for (int i = 0; i < (int)std::ceil((float)length / xfer_size); i++) {
+    int d;
+    if (tx_data != NULL) {
+      int len = std::min(xfer_size, tx_len - (xfer_size * i));
+      d = spi_transfer_retry(endpoint, tx_data + (xfer_size * i), len, NULL, 0);
+    } else {
+      d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), xfer_size);
+    }
+
+    if (d < 0) {
+      LOGE("SPI: bulk transfer failed with %d", d);
+      comms_healthy = false;
+      return -1;
+    }
+
+    ret += d;
+    if ((rx_data != NULL) && d < xfer_size) {
+      break;
+    }
+  }
+
+  return ret;
 }
 
+
+
 std::vector<std::string> PandaSpiHandle::list() {
   // TODO: list all pandas available over SPI
   return {};
@@ -130,15 +163,15 @@ bool check_checksum(uint8_t *data, int data_len) {
 
 
 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;
+  int ret;
 
   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);
+    ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len);
+  } while (ret < 0 && connected && !PANDA_NO_RETRY);
 
-  return err;
+  return ret;
 }
 
 int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) {
@@ -153,7 +186,7 @@ int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) {
     if (rx_buf[0] == ack) {
       break;
     } else if (rx_buf[0] == SPI_NACK) {
-      LOGW("SPI: got header NACK");
+      LOGW("SPI: got NACK");
       return -1;
     }
   }