mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-10-28 01:03:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

simple SPI (#39076)

Browse Source
This commit is contained in:
Adeeb Shihadeh
2026-10-14 08:19:41 -07:00
committed by GitHub
parent 1623441a9f
commit 50319fa702
4 changed files with 209 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/pandad/panda_comms.h
+9 -6
View File
@@ -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);
openpilot/selfdrive/pandad/spi.cc
+192 -214
View File
@@ -2,10 +2,12 @@
#include <sys/ioctl.h>
#include <linux/spi/spidev.h>
#include <array>
#include <cassert>
#include <cmath>
#include <cstring>
#include <iomanip>
#include <random>
#include <sstream>
#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<std::string> PandaSpiHandle::list() {
@@ -186,223 +185,202 @@ std::vector<std::string> 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<uint32_t, 256> 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<double>(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<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");
for (int i = 0; i < t.len; i++) {
if ((static_cast<double>(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<double>(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<double>(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<uint8_t, SPI_BUF_SIZE> 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;
}
openpilot/selfdrive/pandad/tests/test_pandad_spi.py
+7 -5
View File
@@ -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__":
panda
+1 -1
Submodule panda updated: 92eb565169...835e4c31d9