From 2d838f95dabb0229dba01cd585d852e7ca5d47ff Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Tue, 7 May 2024 21:36:04 -0700
Subject: [PATCH] boardd: add debug flag for injecting SPI errors (#32346)

* pull out ll first

* errors

---------

Co-authored-by: Comma Device <device@comma.ai>
---
 selfdrive/boardd/panda_comms.h   |  1 +
 selfdrive/boardd/spi.cc          | 34 ++++++++++++++++++++++++++++----
 selfdrive/debug/check_timings.py |  2 +-
 3 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h
index e61d25402f..6b64768c17 100644
--- a/selfdrive/boardd/panda_comms.h
+++ b/selfdrive/boardd/panda_comms.h
@@ -78,5 +78,6 @@ private:
   int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout);
   int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
   int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
+  int lltransfer(spi_ioc_transfer &t);
 };
 #endif
diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc
index d11e955c49..e02252018f 100644
--- a/selfdrive/boardd/spi.cc
+++ b/selfdrive/boardd/spi.cc
@@ -268,7 +268,7 @@ int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout,
   tx_buf[0] = tx;
 
   while (true) {
-    int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
+    int ret = lltransfer(transfer);
     if (ret < 0) {
       LOGE("SPI: failed to send ACK request");
       return ret;
@@ -291,6 +291,32 @@ int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout,
   return 0;
 }
 
+int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) {
+  static const double err_prob = std::stod(util::getenv("SPI_ERR_PROB", "-1"));
+
+  if (err_prob > 0) {
+    if ((static_cast<double>(rand()) / RAND_MAX) < err_prob) {
+      printf("transfer len error\n");
+      t.len = rand() % SPI_BUF_SIZE;
+    }
+    if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.tx_buf != (uint64_t)NULL) {
+      printf("corrupting TX\n");
+      memset((uint8_t*)t.tx_buf, (uint8_t)(rand() % 256), rand() % (t.len+1));
+    }
+  }
+
+  int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &t);
+
+  if (err_prob > 0) {
+    if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.rx_buf != (uint64_t)NULL) {
+      printf("corrupting RX\n");
+      memset((uint8_t*)t.rx_buf, (uint8_t)(rand() % 256), rand() % (t.len+1));
+    }
+  }
+
+  return ret;
+}
+
 int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) {
   int ret;
   uint16_t rx_data_len;
@@ -316,7 +342,7 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
   memcpy(tx_buf, &header, sizeof(header));
   add_checksum(tx_buf, sizeof(header));
   transfer.len = sizeof(header) + 1;
-  ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
+  ret = lltransfer(transfer);
   if (ret < 0) {
     LOGE("SPI: failed to send header");
     goto transfer_fail;
@@ -334,7 +360,7 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
   }
   add_checksum(tx_buf, tx_len);
   transfer.len = tx_len + 1;
-  ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
+  ret = lltransfer(transfer);
   if (ret < 0) {
     LOGE("SPI: failed to send data");
     goto transfer_fail;
@@ -355,7 +381,7 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
 
   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);
+  ret = lltransfer(transfer);
   if (ret < 0) {
     LOGE("SPI: failed to read rx data");
     goto transfer_fail;
diff --git a/selfdrive/debug/check_timings.py b/selfdrive/debug/check_timings.py
index 3de6b8eb66..24c467659d 100755
--- a/selfdrive/debug/check_timings.py
+++ b/selfdrive/debug/check_timings.py
@@ -21,5 +21,5 @@ if __name__ == "__main__":
 
       if len(ts[s]) > 2:
         d = np.diff(ts[s])
-        print(f"{s:25} {np.mean(d):.2f} {np.std(d):.2f} {np.max(d):.2f} {np.min(d):.2f}")
+        print(f"{s:25} {np.mean(d):7.2f} {np.std(d):7.2f} {np.max(d):7.2f} {np.min(d):7.2f}")
     time.sleep(1)