boardd: SPI support (#26374)

* spi handle

* put usb back

* handle eintr

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

common/util.cc

@@ -1,5 +1,6 @@
 #include "common/util.h"
 
+#include <sys/ioctl.h>
 #include <sys/stat.h>
 #include <dirent.h>
 
@@ -149,6 +150,14 @@ int safe_fflush(FILE *stream) {
   return ret;
 }
 
+int safe_ioctl(int fd, unsigned long request, void *argp) {
+  int ret;
+  do {
+    ret = ioctl(fd, request, argp);
+  } while ((ret == -1) && (errno == EINTR));
+  return ret;
+}
+
 std::string readlink(const std::string &path) {
   char buff[4096];
   ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1);

common/util.h

@@ -86,6 +86,7 @@ int write_file(const char* path, const void* data, size_t size, int flags = O_WR
 FILE* safe_fopen(const char* filename, const char* mode);
 size_t safe_fwrite(const void * ptr, size_t size, size_t count, FILE * stream);
 int safe_fflush(FILE *stream);
+int safe_ioctl(int fd, unsigned long request, void *argp);
 
 std::string readlink(const std::string& path);
 bool file_exists(const std::string& fn);

release/files_common

@@ -90,6 +90,7 @@ selfdrive/boardd/boardd_api_impl.pyx
 selfdrive/boardd/can_list_to_can_capnp.cc
 selfdrive/boardd/panda.cc
 selfdrive/boardd/panda.h
+selfdrive/boardd/spi.cc
 selfdrive/boardd/panda_comms.h
 selfdrive/boardd/panda_comms.cc
 selfdrive/boardd/set_time.py

selfdrive/boardd/SConscript

@@ -1,9 +1,9 @@
 Import('env', 'envCython', 'common', 'cereal', 'messaging')
 
 libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']
-env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'panda_comms.cc'], LIBS=libs)
+env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'panda_comms.cc', 'spi.cc'], LIBS=libs)
 env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
 
 envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
 if GetOption('test'):
-  env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc', 'panda_comms.cc'], LIBS=libs)
+  env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc', 'panda_comms.cc', 'spi.cc'], LIBS=libs)

selfdrive/boardd/panda.cc

@@ -12,7 +12,11 @@
 
 Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
   // TODO: support SPI here one day...
-  handle = std::make_unique<PandaUsbHandle>(serial);
+  if (serial.find("spi") != std::string::npos) {
+    handle = std::make_unique<PandaSpiHandle>(serial);
+  } else {
+    handle = std::make_unique<PandaUsbHandle>(serial);
+  }
 
   hw_type = get_hw_type();
 

selfdrive/boardd/panda_comms.h

@@ -8,6 +8,7 @@
 #include <libusb-1.0/libusb.h>
 
 #define TIMEOUT 0
+#define SPI_BUF_SIZE 1024
 
 
 // comms base class
@@ -49,3 +50,24 @@ private:
   std::vector<uint8_t> recv_buf;
   void handle_usb_issue(int err, const char func[]);
 };
+
+class PandaSpiHandle : public PandaCommsHandle {
+public:
+  PandaSpiHandle(std::string serial);
+  ~PandaSpiHandle();
+  int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT);
+  int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT);
+  int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
+  int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
+  void cleanup();
+
+  static std::vector<std::string> list();
+
+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];
+};

selfdrive/boardd/spi.cc

@@ -0,0 +1,252 @@
+#include <sys/ioctl.h>
+#include <linux/spi/spidev.h>
+
+#include <cassert>
+#include <cstring>
+
+#include "common/util.h"
+#include "common/swaglog.h"
+#include "selfdrive/boardd/panda_comms.h"
+
+
+#define SPI_SYNC 0x5AU
+#define SPI_HACK 0x79U
+#define SPI_DACK 0x85U
+#define SPI_NACK 0x1FU
+#define SPI_CHECKSUM_START 0xABU
+
+struct __attribute__((packed)) spi_header {
+  uint8_t sync;
+  uint8_t endpoint;
+  uint16_t tx_len;
+  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());
+
+  int err;
+  uint32_t spi_mode = SPI_MODE_0;
+  uint32_t spi_speed = 30000000;
+  uint8_t spi_bits_per_word = 8;
+
+  spi_fd = open(serial.c_str(), O_RDWR);
+  if (spi_fd < 0) {
+    LOGE("failed setting SPI mode %d", err);
+    goto fail;
+  }
+
+  // SPI settings
+  err = util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode);
+  if (err < 0) {
+    LOGE("failed setting SPI mode %d", err);
+    goto fail;
+  }
+
+  err = util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+  if (err < 0) {
+    LOGE("failed setting SPI speed");
+    goto fail;
+  }
+
+  err = util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word);
+  if (err < 0) {
+    LOGE("failed setting SPI bits per word");
+    goto fail;
+  }
+
+  return;
+
+fail:
+  cleanup();
+  throw std::runtime_error("Error connecting to panda");
+}
+
+PandaSpiHandle::~PandaSpiHandle() {
+  std::lock_guard lk(hw_lock);
+  cleanup();
+}
+
+void PandaSpiHandle::cleanup() {
+  if (spi_fd != -1) {
+    close(spi_fd);
+    spi_fd = -1;
+  }
+}
+
+
+
+int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout) {
+  int err;
+
+  std::lock_guard lk(hw_lock);
+  do {
+    spi_control_packet packet = {
+      .request = request,
+      .param1 = param1,
+      .param2 = param2,
+      .length = 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;
+
+  std::lock_guard lk(hw_lock);
+  do {
+    spi_control_packet packet = {
+      .request = request,
+      .param1 = param1,
+      .param2 = param2,
+      .length = 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) {
+  return 0;
+}
+
+int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
+  return 0;
+}
+
+std::vector<std::string> PandaSpiHandle::list() {
+  // TODO: list all pandas available over SPI
+  return {};
+}
+
+
+
+void add_checksum(uint8_t *data, int data_len) {
+  data[data_len] = SPI_CHECKSUM_START;
+  for (int i=0; i < data_len; i++) {
+    data[data_len] ^= data[i];
+  }
+}
+
+
+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;
+  uint16_t rx_data_len;
+
+  // needs to be less, since we need to have space for the checksum
+  assert(tx_len < SPI_BUF_SIZE);
+  assert(max_rx_len < SPI_BUF_SIZE);
+
+  spi_header header = {
+    .sync = SPI_SYNC,
+    .endpoint = endpoint,
+    .tx_len = tx_len,
+    .max_rx_len = max_rx_len
+  };
+
+  spi_ioc_transfer transfer = {
+    .tx_buf = (uint64_t)tx_buf,
+    .rx_buf = (uint64_t)rx_buf
+  };
+
+  // Send header
+  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);
+  if (ret < 0) {
+    LOGE("SPI: failed to send header");
+    goto transfer_fail;
+  }
+
+  // Wait for (N)ACK
+  tx_buf[0] = 0x12;
+  transfer.len = 1;
+  while (true) {
+    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) {
+    memcpy(tx_buf, tx_data, tx_len);
+  }
+  add_checksum(tx_buf, tx_len);
+  transfer.len = tx_len + 1;
+  ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
+  if (ret < 0) {
+    LOGE("SPI: failed to send data");
+    goto transfer_fail;
+  }
+
+  // Wait for (N)ACK
+  tx_buf[0] = 0xab;
+  transfer.len = 1;
+  while (true) {
+    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;
+  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;
+  assert(rx_data_len < SPI_BUF_SIZE);
+
+  // Read data
+  transfer.len = rx_data_len + 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 (rx_data != NULL) {
+    memcpy(rx_data, rx_buf, rx_data_len);
+  }
+  ret = rx_data_len;
+
+transfer_fail:
+  return ret;
+}

system/hardware/tici/hardware.py

@@ -431,6 +431,9 @@ class Tici(HardwareBase):
   def initialize_hardware(self):
     self.amplifier.initialize_configuration()
 
+    # TODO: this should go in AGNOS
+    os.system("sudo chmod 666 /dev/spidev0.0")
+
     # Allow thermald to write engagement status to kmsg
     os.system("sudo chmod a+w /dev/kmsg")