diff --git a/common/util.cc b/common/util.cc index b6a8322a27..010fe8a11a 100644 --- a/common/util.cc +++ b/common/util.cc @@ -1,5 +1,6 @@ #include "common/util.h" +#include #include #include @@ -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); diff --git a/common/util.h b/common/util.h index e13f4dc130..b46f7bde4a 100644 --- a/common/util.h +++ b/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); diff --git a/release/files_common b/release/files_common index 61d16a2088..26662f1ef1 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index 356b5de663..d99e67a9f0 100644 --- a/selfdrive/boardd/SConscript +++ b/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) diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index e68558632e..deccee3e76 100644 --- a/selfdrive/boardd/panda.cc +++ b/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(serial); + if (serial.find("spi") != std::string::npos) { + handle = std::make_unique(serial); + } else { + handle = std::make_unique(serial); + } hw_type = get_hw_type(); diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h index c5143b16b3..08d0c1a2af 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -8,6 +8,7 @@ #include #define TIMEOUT 0 +#define SPI_BUF_SIZE 1024 // comms base class @@ -49,3 +50,24 @@ private: std::vector 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 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]; +}; diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc new file mode 100644 index 0000000000..1ec5e89c71 --- /dev/null +++ b/selfdrive/boardd/spi.cc @@ -0,0 +1,252 @@ +#include +#include + +#include +#include + +#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 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; +} diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index e2fd20c1be..c5b931ddae 100644 --- a/system/hardware/tici/hardware.py +++ b/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")