diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h index 08d0c1a2af..aef7b41d07 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -5,11 +5,16 @@ #include #include +#include + #include + #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); }; diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index 1ec5e89c71..3969b313f0 100644 --- a/selfdrive/boardd/spi.cc +++ b/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; - - 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; + ControlPacket_t packet = { + .request = request, + .param1 = param1, + .param2 = param2, + .length = 0 + }; + return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), NULL, 0); } 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; + ControlPacket_t packet = { + .request = request, + .param1 = param1, + .param2 = param2, + .length = length + }; + return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), data, length); } 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,19 +194,9 @@ 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 = 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; - } + ret = wait_for_ack(transfer, SPI_HACK); + if (ret < 0) { + goto transfer_fail; } // Send data @@ -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 = 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; - } + ret = wait_for_ack(transfer, SPI_DACK); + if (ret < 0) { + 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;