From 50319fa702f95ee8a677b466bc66c55a9a680598 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 14 Oct 2026 08:19:41 -0700 Subject: [PATCH] simple SPI (#39076) --- openpilot/selfdrive/pandad/panda_comms.h | 15 +- openpilot/selfdrive/pandad/spi.cc | 406 +++++++++--------- .../selfdrive/pandad/tests/test_pandad_spi.py | 12 +- panda | 2 +- 4 files changed, 209 insertions(+), 226 deletions(-) diff --git a/openpilot/selfdrive/pandad/panda_comms.h b/openpilot/selfdrive/pandad/panda_comms.h index cdfb5019b..77c085fdc 100644 --- a/openpilot/selfdrive/pandad/panda_comms.h +++ b/openpilot/selfdrive/pandad/panda_comms.h @@ -8,7 +8,7 @@ #define TIMEOUT 0 -#define SPI_BUF_SIZE 2048 +#define SPI_BUF_SIZE 4096 class PandaSpiHandle { @@ -35,15 +35,18 @@ private: inline static std::recursive_mutex hw_lock; struct __attribute__((packed)) spi_header { - uint8_t sync; uint8_t endpoint; + uint32_t id; uint16_t tx_len; - uint16_t max_rx_len; + uint16_t rx_len; + }; + struct __attribute__((packed)) spi_resp_header { + uint8_t status; + uint32_t id; + uint16_t len; }; - int wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length); - 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(uint8_t endpoint, uint32_t id, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, double deadline); 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(struct spi_ioc_transfer &t); diff --git a/openpilot/selfdrive/pandad/spi.cc b/openpilot/selfdrive/pandad/spi.cc index f54c26e50..8636861cb 100644 --- a/openpilot/selfdrive/pandad/spi.cc +++ b/openpilot/selfdrive/pandad/spi.cc @@ -2,10 +2,12 @@ #include #include +#include #include #include #include #include +#include #include #include "common/util.h" @@ -15,19 +17,25 @@ #include "selfdrive/pandad/panda_comms.h" -#define SPI_SYNC 0x5AU -#define SPI_HACK 0x79U -#define SPI_DACK 0x85U -#define SPI_NACK 0x1FU -#define SPI_CHECKSUM_START 0xABU +// protocol, see panda/board/drivers/spi.h +#define SPI_SLAVE_READY 0x3CU +#define SPI_ACK 0xC3U +#define SPI_NACK 0x5AU +#define SPI_POLL 0xFFU // filler while reading the response, never a valid request + +#define SPI_CRC_SIZE 4U +#define SPI_XFER_SIZE (SPI_BUF_SIZE - 0x40) // bulk chunk size, leaves room for the protocol overhead +#define SPI_FIRST_READ_SIZE (1024 - sizeof(spi_resp_header) - SPI_CRC_SIZE) // reads are usually small, start small enum SpiError { NACK = -2, ACK_TIMEOUT = -3, + SILENT = -4, }; -const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds +const unsigned int SPI_MIN_TIMEOUT = 100; // milliseconds +const double SPI_SILENT_TIMEOUT = 1000; // milliseconds, longer than the slowest request takes on the panda const std::string SPI_DEVICE = "/dev/spidev0.0"; class LockEx { @@ -50,8 +58,8 @@ private: #define SPILOG(fn, fmt, ...) do { \ fn(fmt, ## __VA_ARGS__); \ fn(" %d / 0x%x / %d / %d / tx: %s", \ - xfer_count, header.endpoint, header.tx_len, header.max_rx_len, \ - util::hexdump(tx_buf, std::min((int)header.tx_len, 8)).c_str()); \ + xfer_count, header.endpoint, header.tx_len, header.rx_len, \ + util::hexdump(tx_buf + sizeof(header), std::min((int)header.tx_len, 8)).c_str()); \ } while (0) PandaSpiHandle::PandaSpiHandle(std::string serial) { @@ -82,7 +90,7 @@ PandaSpiHandle::PandaSpiHandle(std::string serial) { util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word, "failed setting SPI bits per word"); // get hw UID/serial - ret = control_read(0xc3, 0, 0, uid, uid_len, 100); + ret = control_read(0xc3, 0, 0, uid, uid_len, 500); if (ret == uid_len) { std::stringstream stream; for (int i = 0; i < uid_len; i++) { @@ -140,40 +148,31 @@ 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 bulk_transfer(endpoint, data, length, NULL, 0, timeout); -} -int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - return bulk_transfer(endpoint, NULL, 0, data, length, timeout); -} - -int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout) { - const int xfer_size = SPI_BUF_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, timeout); - } else { - uint16_t to_read = std::min(xfer_size, rx_len - ret); - d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), to_read, timeout); - } - - if (d < 0) { - SPILOG(LOGE, "SPI: bulk transfer failed with %d", d); + for (int pos = 0; pos < length; pos += SPI_XFER_SIZE) { + int ret = spi_transfer_retry(endpoint, data + pos, std::min(SPI_XFER_SIZE, length - pos), NULL, 0, timeout); + if (ret < 0) { comms_healthy = false; - return d; + return ret; } + } + return length; +} - ret += d; - if ((rx_data != NULL) && d < xfer_size) { +int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int pos = 0; + while (pos < length) { + int to_read = std::min(pos == 0 ? (int)SPI_FIRST_READ_SIZE : SPI_XFER_SIZE, length - pos); + int ret = spi_transfer_retry(endpoint, NULL, 0, data + pos, to_read, timeout); + if (ret < 0) { + comms_healthy = false; + return ret; + } + pos += ret; + if (ret < to_read) { break; } } - - return ret; + return pos; } std::vector PandaSpiHandle::list() { @@ -186,223 +185,202 @@ std::vector PandaSpiHandle::list() { 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]; - } +// random, so it's never the same as another request the panda just handled, even from another client +static uint32_t random_id() { + static thread_local std::mt19937 rng{std::random_device{}()}; + return rng(); } -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]; +// standard CRC-32, same as zlib's crc32() +static uint32_t crc32(const uint8_t *data, uint32_t len) { + static const auto table = [] { + std::array t{}; + for (uint32_t i = 0; i < 256; i++) { + uint32_t c = i; + for (int j = 0; j < 8; j++) { + c = (c & 1) ? (0xEDB88320U ^ (c >> 1)) : (c >> 1); + } + t[i] = c; + } + return t; + }(); + + uint32_t crc = 0xFFFFFFFFU; + for (uint32_t i = 0; i < len; i++) { + crc = table[(crc ^ data[i]) & 0xFFU] ^ (crc >> 8); } - return checksum == 0U; + return crc ^ 0xFFFFFFFFU; +} + +static void put_crc(uint8_t *data, uint32_t len) { + uint32_t crc = crc32(data, len); + memcpy(&data[len], &crc, sizeof(crc)); +} + +static bool check_crc(const uint8_t *data, uint32_t len) { + uint32_t crc; + memcpy(&crc, &data[len], sizeof(crc)); + return crc == crc32(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, unsigned int timeout) { + // timeout of 0 means forever + const double deadline = (timeout == 0) ? INFINITY : millis_since_boot() + std::max(timeout, SPI_MIN_TIMEOUT); + + // the id stays the same when we send the request again, so the panda never runs it twice + uint32_t id = random_id(); int ret; int nack_count = 0; - int timeout_count = 0; - bool timed_out = false; - double start_time = millis_since_boot(); - - do { - ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len, timeout); - - if (ret < 0) { - timed_out = (timeout != 0) && (timeout_count > 5); - timeout_count += ret == SpiError::ACK_TIMEOUT; - - // give other threads a chance to run - std::this_thread::yield(); - - if (ret == SpiError::NACK) { - // prevent busy waiting while the panda is NACK'ing - // due to full TX buffers - nack_count += 1; - if (nack_count > 3) { - SPILOG(LOGD, "NACK sleep %d", nack_count); - usleep(std::clamp(nack_count*10, 200, 2000)); - } - } + while (true) { + ret = spi_transfer(endpoint, id, tx_data, tx_len, rx_data, max_rx_len, deadline); + if (ret >= 0 || !connected) { + break; } - } while (ret < 0 && connected && !timed_out); + + if (ret == SpiError::NACK) { + // the panda NACKs while its CAN TX buffers are full. that's not a comms + // failure, so keep trying regardless of the timeout, without busy waiting. + // a new id, since the panda would answer the same id with the same NACK + id = random_id(); + nack_count += 1; + if (nack_count > 3) { + usleep(std::clamp(nack_count*10, 200, 2000)); + } + } else if (ret == SpiError::SILENT) { + // the panda went quiet. let go of the bus for a bit, someone else might need it + LOGW("SPI: panda isn't responding"); + usleep(10000); + } else if (millis_since_boot() > deadline) { + break; + } + + // give other threads a chance to run + std::this_thread::yield(); + } if (ret < 0) { - SPILOG(LOGE, "transfer failed, after %d tries, %.2fms", timeout_count, millis_since_boot() - start_time); + SPILOG(LOGE, "SPI: transfer failed with %d", ret); } - return ret; } -int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length) { - double start_millis = millis_since_boot(); - if (timeout == 0) { - timeout = SPI_ACK_TIMEOUT; - } - timeout = std::clamp(timeout, 20U, SPI_ACK_TIMEOUT); - - spi_ioc_transfer transfer = { - .tx_buf = (uint64_t)tx_buf, - .rx_buf = (uint64_t)rx_buf, - .len = length, - }; - memset(tx_buf, tx, length); - - while (true) { - int ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to send ACK request"); - return ret; - } - - if (rx_buf[0] == ack) { - break; - } else if (rx_buf[0] == SPI_NACK) { - SPILOG(LOGD, "SPI: got NACK, waiting for 0x%x", ack); - return SpiError::NACK; - } - - // handle timeout - if (millis_since_boot() - start_millis > timeout) { - SPILOG(LOGW, "SPI: timed out waiting for ACK, waiting for 0x%x", ack); - return SpiError::ACK_TIMEOUT; - } - } - - return 0; -} - int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) { static const double err_prob = std::stod(util::getenv("SPI_ERR_PROB", "-1")); + auto chance = [](double p) { return (static_cast(rand()) / RAND_MAX) < p; }; + auto corrupt = [&](uint64_t buf) { + for (uint32_t i = 0; i < t.len; i++) { + if (chance(0.1)) ((uint8_t*)buf)[i] = (uint8_t)(rand() % 256); + } + }; - if (err_prob > 0) { - if ((static_cast(rand()) / RAND_MAX) < err_prob) { - printf("transfer len error\n"); - t.len = rand() % SPI_BUF_SIZE; - } - if ((static_cast(rand()) / RAND_MAX) < err_prob && t.tx_buf != (uint64_t)NULL) { - printf("corrupting TX\n"); - for (int i = 0; i < t.len; i++) { - if ((static_cast(rand()) / RAND_MAX) > 0.9) { - ((uint8_t*)t.tx_buf)[i] = (uint8_t)(rand() % 256); - } - } - } + if (err_prob > 0 && chance(err_prob)) { + printf("transfer len error\n"); + t.len = rand() % SPI_BUF_SIZE; + } + if (err_prob > 0 && chance(err_prob) && t.tx_buf == (uint64_t)tx_buf) { + printf("corrupting TX\n"); + corrupt(t.tx_buf); } int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &t); - - if (err_prob > 0) { - if ((static_cast(rand()) / RAND_MAX) < err_prob && t.rx_buf != (uint64_t)NULL) { - printf("corrupting RX\n"); - for (int i = 0; i < t.len; i++) { - if ((static_cast(rand()) / RAND_MAX) > 0.9) { - ((uint8_t*)t.rx_buf)[i] = (uint8_t)(rand() % 256); - } - } - } + if (err_prob > 0 && chance(err_prob)) { + printf("ioctl error\n"); + ret = -1; } + if (err_prob > 0 && chance(err_prob)) { + printf("corrupting RX\n"); + corrupt(t.rx_buf); + } 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; +// One request and its response. Resends until the panda got the request and we got its response, +// or the deadline. Every transfer either sees all of what the panda queued from its first byte, or +// nothing it can mistake for a response. Returns the response length, or a negative SpiError +int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint32_t id, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, double deadline) { + static const auto poll = [] { + std::array p; + p.fill(SPI_POLL); + return p; + }(); + LockEx lock(spi_fd, hw_lock); - // 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); + // the response is always read in one transfer of rx_len + const uint32_t rx_len = sizeof(spi_resp_header) + max_rx_len + SPI_CRC_SIZE; + const uint32_t req_len = sizeof(header) + tx_len; + assert(req_len + SPI_CRC_SIZE <= SPI_BUF_SIZE); + assert(rx_len <= SPI_BUF_SIZE); xfer_count++; header = { - .sync = SPI_SYNC, .endpoint = endpoint, + .id = id, .tx_len = tx_len, - .max_rx_len = max_rx_len + .rx_len = (uint16_t)rx_len, }; - spi_ioc_transfer transfer = { - .tx_buf = (uint64_t)tx_buf, - .rx_buf = (uint64_t)rx_buf + auto xfer = [&](const uint8_t *tx, uint32_t len) { + spi_ioc_transfer t = {.tx_buf = (uint64_t)tx, .rx_buf = (uint64_t)rx_buf, .len = len}; + int ret = lltransfer(t); + if (ret < 0) { + SPILOG(LOGE, "SPI: transfer failed"); + } + return ret; }; - // Send header - memcpy(tx_buf, &header, sizeof(header)); - add_checksum(tx_buf, sizeof(header)); - transfer.len = sizeof(header) + 1; - ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to send header"); - goto fail; - } + // when the panda last showed it's there. while it's quiet, we let go of the bus + double alive = millis_since_boot(); + while (true) { + // send the request, until the panda was ready for it. it's rebuilt every time, + // since SPI_ERR_PROB corrupts tx_buf + memcpy(tx_buf, &header, sizeof(header)); + if (tx_data != NULL) { + memcpy(tx_buf + sizeof(header), tx_data, tx_len); + } + put_crc(tx_buf, req_len); + // a failed transfer is sent again like any other, keeping the bus, so nothing can run in between + if (xfer(tx_buf, req_len + SPI_CRC_SIZE) >= 0 && rx_buf[0] == SPI_SLAVE_READY) { + alive = millis_since_boot(); + // read until the response. if the panda is ready again, it never got the request + do { + if (xfer(poll.data(), rx_len) < 0) { + break; + } + if (rx_buf[0] == SPI_SLAVE_READY) { + alive = millis_since_boot(); + break; + } - // Wait for (N)ACK - ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1); - if (ret < 0) { - goto fail; - } + // a response that got cut off or corrupted isn't one, keep reading + spi_resp_header resp; + memcpy(&resp, rx_buf, sizeof(resp)); + if ((resp.status == SPI_ACK || resp.status == SPI_NACK) && resp.len <= max_rx_len && check_crc(rx_buf, sizeof(resp) + resp.len)) { + // otherwise it's the response to an old request, and we send ours again + if (resp.id == header.id) { + if (resp.status == SPI_NACK) { + SPILOG(LOGD, "SPI: got NACK"); + return SpiError::NACK; + } + if (rx_data != NULL) { + memcpy(rx_data, rx_buf + sizeof(resp), resp.len); + } + return resp.len; + } + alive = millis_since_boot(); + break; + } + } while (millis_since_boot() < std::min(deadline, alive + SPI_SILENT_TIMEOUT)); + } - // 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 = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to send data"); - goto fail; - } - - // Wait for (N)ACK - ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3); - if (ret < 0) { - goto fail; - } - - // Read data - rx_data_len = *(uint16_t *)(rx_buf+1); - if (rx_data_len >= SPI_BUF_SIZE) { - SPILOG(LOGE, "SPI: RX data len larger than buf size %d", rx_data_len); - goto fail; - } - - transfer.len = rx_data_len + 1; - transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1); - ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to read rx data"); - goto fail; - } - if (!check_checksum(rx_buf, rx_data_len + 4)) { - SPILOG(LOGE, "SPI: bad checksum"); - goto fail; - } - - if (rx_data != NULL) { - memcpy(rx_data, rx_buf + 3, rx_data_len); - } - - return rx_data_len; - -fail: - // ensure slave is in a consistent state - // and ready for the next transfer - int nack_cnt = 0; - while (nack_cnt < 3) { - if (wait_for_ack(SPI_NACK, 0x14, 1, SPI_BUF_SIZE/2) == 0) { - nack_cnt += 1; - } else { - nack_cnt = 0; + if (millis_since_boot() > deadline) { + SPILOG(LOGW, "SPI: timed out"); + return SpiError::ACK_TIMEOUT; + } + if (millis_since_boot() > alive + SPI_SILENT_TIMEOUT) { + return SpiError::SILENT; } } - - if (ret >= 0) ret = -1; - return ret; } diff --git a/openpilot/selfdrive/pandad/tests/test_pandad_spi.py b/openpilot/selfdrive/pandad/tests/test_pandad_spi.py index 8ca39445a..281865428 100755 --- a/openpilot/selfdrive/pandad/tests/test_pandad_spi.py +++ b/openpilot/selfdrive/pandad/tests/test_pandad_spi.py @@ -45,9 +45,10 @@ class TestBoarddSpi(OpenpilotTestCase): st = time.monotonic() ts = {s: [] for s in socks.keys()} - for _ in range(int(os.getenv("TEST_TIME", "20"))): - # send some CAN messages - if not JUNGLE_SPAM: + test_time = int(os.getenv("TEST_TIME", "20")) + for i in range(test_time + 2): + # send some CAN messages, then give the last ones time to come back + if not JUNGLE_SPAM and i < test_time: sent = send_random_can_messages(sendcan, random.randrange(2, 20)) for k, v in sent.items(): sent_msgs[k].extend(list(v)) @@ -74,8 +75,7 @@ class TestBoarddSpi(OpenpilotTestCase): continue key = (msg.address, msg.dat) assert key in sent_msgs[msg.src], f"got unexpected msg: {msg.src=} {msg.address=} {msg.dat=}" - # TODO: enable this - #sent_msgs[msg.src].remove(key) + sent_msgs[msg.src].remove(key) total_recv_count += 1 elif service == "pandaStates": assert len(m.pandaStates) == 1 @@ -107,6 +107,8 @@ class TestBoarddSpi(OpenpilotTestCase): with subtests.test(msg="CAN traffic"): print(f"Sent {total_sent_count} CAN messages, got {total_recv_count} back. {total_recv_count/(total_sent_count+1e-4):.2%} received") assert total_recv_count > 20 + # every message came back exactly once, despite the injected errors + assert all(len(v) == 0 for v in sent_msgs.values()) if __name__ == "__main__": diff --git a/panda b/panda index 92eb56516..835e4c31d 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 92eb565169fd553f4dfaf508c1a3f8dbc14fbfbf +Subproject commit 835e4c31d9514f9d06c341479237c35349e75136