From 70c77631240eeaa4f83c2cb5e23a58d13d299dfe Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 15 Jul 2023 12:26:24 -0700 Subject: [PATCH 1/5] SPI kernel driver (#1497) * kernel driver * fix checksum check * cleanup --------- Co-authored-by: Comma Device --- .gitignore | 1 + drivers/spi/.gitignore | 7 + drivers/spi/Makefile | 14 + drivers/spi/load.sh | 27 ++ drivers/spi/patch | 33 ++ drivers/spi/pull-src.sh | 12 + drivers/spi/spi_panda.h | 160 +++++++ drivers/spi/spidev_panda.c | 891 +++++++++++++++++++++++++++++++++++++ python/spi.py | 48 +- 9 files changed, 1192 insertions(+), 1 deletion(-) create mode 100644 drivers/spi/.gitignore create mode 100644 drivers/spi/Makefile create mode 100755 drivers/spi/load.sh create mode 100644 drivers/spi/patch create mode 100755 drivers/spi/pull-src.sh create mode 100644 drivers/spi/spi_panda.h create mode 100644 drivers/spi/spidev_panda.c diff --git a/.gitignore b/.gitignore index 403d034ee1..a3f6520bfd 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +*.tmp *.pyc .*.swp .*.swo diff --git a/drivers/spi/.gitignore b/drivers/spi/.gitignore new file mode 100644 index 0000000000..49dc09d89f --- /dev/null +++ b/drivers/spi/.gitignore @@ -0,0 +1,7 @@ +spidev.c +*.ko +*.cmd +*.mod +*.symvers +*.order +*.mod.c diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile new file mode 100644 index 0000000000..9a2a0abf67 --- /dev/null +++ b/drivers/spi/Makefile @@ -0,0 +1,14 @@ +obj-m += spidev_panda.o + +KDIR := /lib/modules/$(shell uname -r)/build +PWD := $(shell pwd) + +# GCC9 bug, apply kernel patch instead? +# https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/commit/?id=0b999ae3614d09d97a1575936bcee884f912b10e +ccflags-y := -Wno-missing-attributes + +default: + $(MAKE) -C $(KDIR) M=$(PWD) modules + +clean: + $(MAKE) -C $(KDIR) M=$(PWD) clean diff --git a/drivers/spi/load.sh b/drivers/spi/load.sh new file mode 100755 index 0000000000..b8efdbf708 --- /dev/null +++ b/drivers/spi/load.sh @@ -0,0 +1,27 @@ +#!/bin/bash +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +make -j8 + +sudo su -c "echo spi0.0 > /sys/bus/spi/drivers/spidev/unbind" || true + +sudo dmesg -C + +#sudo rmmod -f spidev_panda +sudo rmmod spidev_panda || true +sudo insmod spidev_panda.ko + +sudo su -c "echo 'file $DIR/spidev_panda.c +p' > /sys/kernel/debug/dynamic_debug/control" +sudo su -c "echo 'file $DIR/spi_panda.h +p' > /sys/kernel/debug/dynamic_debug/control" + +sudo lsmod + +echo "loaded" +ls -la /dev/spi* +sudo chmod 666 /dev/spi* +ipython -c "from panda import Panda; print(Panda.list())" +KERN=1 ipython -c "from panda import Panda; print(Panda.list())" +dmesg diff --git a/drivers/spi/patch b/drivers/spi/patch new file mode 100644 index 0000000000..a146303424 --- /dev/null +++ b/drivers/spi/patch @@ -0,0 +1,33 @@ +53c53,54 +< #define SPIDEV_MAJOR 153 /* assigned */ +--- +> int SPIDEV_MAJOR = 0; +> //#define SPIDEV_MAJOR 153 /* assigned */ +354a356,358 +> +> #include "spi_panda.h" +> +413,414c417,419 +< retval = __put_user((spi->mode & SPI_LSB_FIRST) ? 1 : 0, +< (__u8 __user *)arg); +--- +> retval = panda_transfer(spidev, spi, arg); +> //retval = __put_user((spi->mode & SPI_LSB_FIRST) ? 1 : 0, +> // (__u8 __user *)arg); +697,698d701 +< { .compatible = "rohm,dh2228fv" }, +< { .compatible = "lineartechnology,ltc2488" }, +831c834 +< .name = "spidev", +--- +> .name = "spidev_panda", +856c859 +< status = register_chrdev(SPIDEV_MAJOR, "spi", &spidev_fops); +--- +> status = register_chrdev(0, "spi", &spidev_fops); +860c863,865 +< spidev_class = class_create(THIS_MODULE, "spidev"); +--- +> SPIDEV_MAJOR = status; +> +> spidev_class = class_create(THIS_MODULE, "spidev_panda"); diff --git a/drivers/spi/pull-src.sh b/drivers/spi/pull-src.sh new file mode 100755 index 0000000000..f74b94b9e7 --- /dev/null +++ b/drivers/spi/pull-src.sh @@ -0,0 +1,12 @@ +#!/usr/bin/bash +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +rm -f spidev.c +wget https://raw.githubusercontent.com/commaai/agnos-kernel-sdm845/master/drivers/spi/spidev.c + +# diff spidev.c spidev_panda.c > patch +# git diff --no-index spidev.c spidev_panda.c +patch -o spidev_panda.c spidev.c -i patch diff --git a/drivers/spi/spi_panda.h b/drivers/spi/spi_panda.h new file mode 100644 index 0000000000..c7da681fc2 --- /dev/null +++ b/drivers/spi/spi_panda.h @@ -0,0 +1,160 @@ +#include +#include +#include + +#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 { + u8 sync; + u8 endpoint; + uint16_t tx_len; + uint16_t max_rx_len; +}; + +struct spi_panda_transfer { + __u64 rx_buf; + __u64 tx_buf; + __u32 tx_length; + __u32 rx_length_max; + __u32 timeout; + __u8 endpoint; + __u8 expect_disconnect; +}; + +static u8 panda_calc_checksum(u8 *buf, u16 length) { + int i; + u8 checksum = SPI_CHECKSUM_START; + for (i = 0U; i < length; i++) { + checksum ^= buf[i]; + } + return checksum; +} + +static long panda_wait_for_ack(struct spidev_data *spidev, u8 ack_val, u8 length) { + int i; + int ret; + for (i = 0; i < 1000; i++) { + ret = spidev_sync_read(spidev, length); + if (ret < 0) { + return ret; + } + + if (spidev->rx_buffer[0] == ack_val) { + return 0; + } else if (spidev->rx_buffer[0] == SPI_NACK) { + return -2; + } + if (i > 20) usleep_range(10, 20); + } + return -1; +} + +static long panda_transfer_raw(struct spidev_data *spidev, struct spi_device *spi, unsigned long arg) { + u16 rx_len; + long retval = -1; + struct spi_header header; + struct spi_panda_transfer pt; + + struct spi_transfer t = { + .len = 0, + .tx_buf = spidev->tx_buffer, + .rx_buf = spidev->rx_buffer, + .speed_hz = spidev->spi->max_speed_hz, + }; + + struct spi_message m; + spi_message_init(&m); + spi_message_add_tail(&t, &m); + + // read struct from user + if (!access_ok(VERIFY_WRITE, arg, sizeof(pt))) { + return -1; + } + if (copy_from_user(&pt, (void __user *)arg, sizeof(pt))) { + return -1; + } + dev_dbg(&spi->dev, "ep: %d, tx len: %d\n", pt.endpoint, pt.tx_length); + + // send header + header.sync = 0x5a; + header.endpoint = pt.endpoint; + header.tx_len = pt.tx_length; + header.max_rx_len = pt.rx_length_max; + memcpy(spidev->tx_buffer, &header, sizeof(header)); + spidev->tx_buffer[sizeof(header)] = panda_calc_checksum(spidev->tx_buffer, sizeof(header)); + + t.len = sizeof(header) + 1; + retval = spidev_sync(spidev, &m); + if (retval < 0) { + dev_dbg(&spi->dev, "spi xfer failed %ld\n", retval); + return retval; + } + + // wait for ACK + retval = panda_wait_for_ack(spidev, SPI_HACK, 1); + if (retval < 0) { + dev_dbg(&spi->dev, "no header ack %ld\n", retval); + return retval; + } + + // send data + dev_dbg(&spi->dev, "sending data\n"); + retval = copy_from_user(spidev->tx_buffer, (const u8 __user *)(uintptr_t)pt.tx_buf, pt.tx_length); + spidev->tx_buffer[pt.tx_length] = panda_calc_checksum(spidev->tx_buffer, pt.tx_length); + t.len = pt.tx_length + 1; + retval = spidev_sync(spidev, &m); + + if (pt.expect_disconnect) { + return 0; + } + + // wait for ACK + retval = panda_wait_for_ack(spidev, SPI_DACK, 3); + if (retval < 0) { + dev_dbg(&spi->dev, "no data ack\n"); + return retval; + } + + // get response + t.rx_buf = spidev->rx_buffer + 3; + rx_len = (spidev->rx_buffer[2] << 8) | (spidev->rx_buffer[1]); + dev_dbg(&spi->dev, "rx len %u\n", rx_len); + if (rx_len > pt.rx_length_max) { + dev_dbg(&spi->dev, "RX len greater than max\n"); + return -1; + } + + // do the read + t.len = rx_len + 1; + retval = spidev_sync(spidev, &m); + if (retval < 0) { + dev_dbg(&spi->dev, "spi xfer failed %ld\n", retval); + return retval; + } + if (panda_calc_checksum(spidev->rx_buffer, 3 + rx_len + 1) != 0) { + dev_dbg(&spi->dev, "bad checksum\n"); + return -1; + } + + retval = copy_to_user((u8 __user *)(uintptr_t)pt.rx_buf, spidev->rx_buffer + 3, rx_len); + + return rx_len; +} + +static long panda_transfer(struct spidev_data *spidev, struct spi_device *spi, unsigned long arg) { + int i; + int ret; + dev_dbg(&spi->dev, "=== XFER start ===\n"); + for (i = 0; i < 20; i++) { + ret = panda_transfer_raw(spidev, spi, arg); + if (ret >= 0) { + break; + } + } + dev_dbg(&spi->dev, "took %d tries\n", i+1); + return ret; +} diff --git a/drivers/spi/spidev_panda.c b/drivers/spi/spidev_panda.c new file mode 100644 index 0000000000..f21fe3387b --- /dev/null +++ b/drivers/spi/spidev_panda.c @@ -0,0 +1,891 @@ +/* + * Simple synchronous userspace interface to SPI devices + * + * Copyright (C) 2006 SWAPP + * Andrea Paterniani + * Copyright (C) 2007 David Brownell (simplification, cleanup) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + + +/* + * This supports access to SPI devices using normal userspace I/O calls. + * Note that while traditional UNIX/POSIX I/O semantics are half duplex, + * and often mask message boundaries, full SPI support requires full duplex + * transfers. There are several kinds of internal message boundaries to + * handle chipselect management and other protocol options. + * + * SPI has a character major number assigned. We allocate minor numbers + * dynamically using a bitmask. You must use hotplug tools, such as udev + * (or mdev with busybox) to create and destroy the /dev/spidevB.C device + * nodes, since there is no fixed association of minor numbers with any + * particular SPI bus or device. + */ +int SPIDEV_MAJOR = 0; +//#define SPIDEV_MAJOR 153 /* assigned */ +#define N_SPI_MINORS 32 /* ... up to 256 */ + +static DECLARE_BITMAP(minors, N_SPI_MINORS); + + +/* Bit masks for spi_device.mode management. Note that incorrect + * settings for some settings can cause *lots* of trouble for other + * devices on a shared bus: + * + * - CS_HIGH ... this device will be active when it shouldn't be + * - 3WIRE ... when active, it won't behave as it should + * - NO_CS ... there will be no explicit message boundaries; this + * is completely incompatible with the shared bus model + * - READY ... transfers may proceed when they shouldn't. + * + * REVISIT should changing those flags be privileged? + */ +#define SPI_MODE_MASK (SPI_CPHA | SPI_CPOL | SPI_CS_HIGH \ + | SPI_LSB_FIRST | SPI_3WIRE | SPI_LOOP \ + | SPI_NO_CS | SPI_READY | SPI_TX_DUAL \ + | SPI_TX_QUAD | SPI_RX_DUAL | SPI_RX_QUAD) + +struct spidev_data { + dev_t devt; + spinlock_t spi_lock; + struct spi_device *spi; + struct list_head device_entry; + + /* TX/RX buffers are NULL unless this device is open (users > 0) */ + struct mutex buf_lock; + unsigned users; + u8 *tx_buffer; + u8 *rx_buffer; + u32 speed_hz; +}; + +static LIST_HEAD(device_list); +static DEFINE_MUTEX(device_list_lock); + +static unsigned bufsiz = 4096; +module_param(bufsiz, uint, S_IRUGO); +MODULE_PARM_DESC(bufsiz, "data bytes in biggest supported SPI message"); + +/*-------------------------------------------------------------------------*/ + +static ssize_t +spidev_sync(struct spidev_data *spidev, struct spi_message *message) +{ + DECLARE_COMPLETION_ONSTACK(done); + int status; + struct spi_device *spi; + + spin_lock_irq(&spidev->spi_lock); + spi = spidev->spi; + spin_unlock_irq(&spidev->spi_lock); + + if (spi == NULL) + status = -ESHUTDOWN; + else + status = spi_sync(spi, message); + + if (status == 0) + status = message->actual_length; + + return status; +} + +static inline ssize_t +spidev_sync_write(struct spidev_data *spidev, size_t len) +{ + struct spi_transfer t = { + .tx_buf = spidev->tx_buffer, + .len = len, + .speed_hz = spidev->speed_hz, + }; + struct spi_message m; + + spi_message_init(&m); + spi_message_add_tail(&t, &m); + return spidev_sync(spidev, &m); +} + +static inline ssize_t +spidev_sync_read(struct spidev_data *spidev, size_t len) +{ + struct spi_transfer t = { + .rx_buf = spidev->rx_buffer, + .len = len, + .speed_hz = spidev->speed_hz, + }; + struct spi_message m; + + spi_message_init(&m); + spi_message_add_tail(&t, &m); + return spidev_sync(spidev, &m); +} + +/*-------------------------------------------------------------------------*/ + +/* Read-only message with current device setup */ +static ssize_t +spidev_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos) +{ + struct spidev_data *spidev; + ssize_t status = 0; + + /* chipselect only toggles at start or end of operation */ + if (count > bufsiz) + return -EMSGSIZE; + + spidev = filp->private_data; + + mutex_lock(&spidev->buf_lock); + status = spidev_sync_read(spidev, count); + if (status > 0) { + unsigned long missing; + + missing = copy_to_user(buf, spidev->rx_buffer, status); + if (missing == status) + status = -EFAULT; + else + status = status - missing; + } + mutex_unlock(&spidev->buf_lock); + + return status; +} + +/* Write-only message with current device setup */ +static ssize_t +spidev_write(struct file *filp, const char __user *buf, + size_t count, loff_t *f_pos) +{ + struct spidev_data *spidev; + ssize_t status = 0; + unsigned long missing; + + /* chipselect only toggles at start or end of operation */ + if (count > bufsiz) + return -EMSGSIZE; + + spidev = filp->private_data; + + mutex_lock(&spidev->buf_lock); + missing = copy_from_user(spidev->tx_buffer, buf, count); + if (missing == 0) + status = spidev_sync_write(spidev, count); + else + status = -EFAULT; + mutex_unlock(&spidev->buf_lock); + + return status; +} + +static int spidev_message(struct spidev_data *spidev, + struct spi_ioc_transfer *u_xfers, unsigned n_xfers) +{ + struct spi_message msg; + struct spi_transfer *k_xfers; + struct spi_transfer *k_tmp; + struct spi_ioc_transfer *u_tmp; + unsigned n, total, tx_total, rx_total; + u8 *tx_buf, *rx_buf; + int status = -EFAULT; + + spi_message_init(&msg); + k_xfers = kcalloc(n_xfers, sizeof(*k_tmp), GFP_KERNEL); + if (k_xfers == NULL) + return -ENOMEM; + + /* Construct spi_message, copying any tx data to bounce buffer. + * We walk the array of user-provided transfers, using each one + * to initialize a kernel version of the same transfer. + */ + tx_buf = spidev->tx_buffer; + rx_buf = spidev->rx_buffer; + total = 0; + tx_total = 0; + rx_total = 0; + for (n = n_xfers, k_tmp = k_xfers, u_tmp = u_xfers; + n; + n--, k_tmp++, u_tmp++) { + k_tmp->len = u_tmp->len; + + total += k_tmp->len; + /* Since the function returns the total length of transfers + * on success, restrict the total to positive int values to + * avoid the return value looking like an error. Also check + * each transfer length to avoid arithmetic overflow. + */ + if (total > INT_MAX || k_tmp->len > INT_MAX) { + status = -EMSGSIZE; + goto done; + } + + if (u_tmp->rx_buf) { + /* this transfer needs space in RX bounce buffer */ + rx_total += k_tmp->len; + if (rx_total > bufsiz) { + status = -EMSGSIZE; + goto done; + } + k_tmp->rx_buf = rx_buf; + if (!access_ok(VERIFY_WRITE, (u8 __user *) + (uintptr_t) u_tmp->rx_buf, + u_tmp->len)) + goto done; + rx_buf += k_tmp->len; + } + if (u_tmp->tx_buf) { + /* this transfer needs space in TX bounce buffer */ + tx_total += k_tmp->len; + if (tx_total > bufsiz) { + status = -EMSGSIZE; + goto done; + } + k_tmp->tx_buf = tx_buf; + if (copy_from_user(tx_buf, (const u8 __user *) + (uintptr_t) u_tmp->tx_buf, + u_tmp->len)) + goto done; + tx_buf += k_tmp->len; + } + + k_tmp->cs_change = !!u_tmp->cs_change; + k_tmp->tx_nbits = u_tmp->tx_nbits; + k_tmp->rx_nbits = u_tmp->rx_nbits; + k_tmp->bits_per_word = u_tmp->bits_per_word; + k_tmp->delay_usecs = u_tmp->delay_usecs; + k_tmp->speed_hz = u_tmp->speed_hz; + if (!k_tmp->speed_hz) + k_tmp->speed_hz = spidev->speed_hz; +#ifdef VERBOSE + dev_dbg(&spidev->spi->dev, + " xfer len %u %s%s%s%dbits %u usec %uHz\n", + u_tmp->len, + u_tmp->rx_buf ? "rx " : "", + u_tmp->tx_buf ? "tx " : "", + u_tmp->cs_change ? "cs " : "", + u_tmp->bits_per_word ? : spidev->spi->bits_per_word, + u_tmp->delay_usecs, + u_tmp->speed_hz ? : spidev->spi->max_speed_hz); +#endif + spi_message_add_tail(k_tmp, &msg); + } + + status = spidev_sync(spidev, &msg); + if (status < 0) + goto done; + + /* copy any rx data out of bounce buffer */ + rx_buf = spidev->rx_buffer; + for (n = n_xfers, u_tmp = u_xfers; n; n--, u_tmp++) { + if (u_tmp->rx_buf) { + if (__copy_to_user((u8 __user *) + (uintptr_t) u_tmp->rx_buf, rx_buf, + u_tmp->len)) { + status = -EFAULT; + goto done; + } + rx_buf += u_tmp->len; + } + } + status = total; + +done: + kfree(k_xfers); + return status; +} + +static struct spi_ioc_transfer * +spidev_get_ioc_message(unsigned int cmd, struct spi_ioc_transfer __user *u_ioc, + unsigned *n_ioc) +{ + struct spi_ioc_transfer *ioc; + u32 tmp; + + /* Check type, command number and direction */ + if (_IOC_TYPE(cmd) != SPI_IOC_MAGIC + || _IOC_NR(cmd) != _IOC_NR(SPI_IOC_MESSAGE(0)) + || _IOC_DIR(cmd) != _IOC_WRITE) + return ERR_PTR(-ENOTTY); + + tmp = _IOC_SIZE(cmd); + if ((tmp % sizeof(struct spi_ioc_transfer)) != 0) + return ERR_PTR(-EINVAL); + *n_ioc = tmp / sizeof(struct spi_ioc_transfer); + if (*n_ioc == 0) + return NULL; + + /* copy into scratch area */ + ioc = kmalloc(tmp, GFP_KERNEL); + if (!ioc) + return ERR_PTR(-ENOMEM); + if (__copy_from_user(ioc, u_ioc, tmp)) { + kfree(ioc); + return ERR_PTR(-EFAULT); + } + return ioc; +} + + +#include "spi_panda.h" + +static long +spidev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg) +{ + int err = 0; + int retval = 0; + struct spidev_data *spidev; + struct spi_device *spi; + u32 tmp; + unsigned n_ioc; + struct spi_ioc_transfer *ioc; + + /* Check type and command number */ + if (_IOC_TYPE(cmd) != SPI_IOC_MAGIC) + return -ENOTTY; + + /* Check access direction once here; don't repeat below. + * IOC_DIR is from the user perspective, while access_ok is + * from the kernel perspective; so they look reversed. + */ + if (_IOC_DIR(cmd) & _IOC_READ) + err = !access_ok(VERIFY_WRITE, + (void __user *)arg, _IOC_SIZE(cmd)); + if (err == 0 && _IOC_DIR(cmd) & _IOC_WRITE) + err = !access_ok(VERIFY_READ, + (void __user *)arg, _IOC_SIZE(cmd)); + if (err) + return -EFAULT; + + /* guard against device removal before, or while, + * we issue this ioctl. + */ + spidev = filp->private_data; + spin_lock_irq(&spidev->spi_lock); + spi = spi_dev_get(spidev->spi); + spin_unlock_irq(&spidev->spi_lock); + + if (spi == NULL) + return -ESHUTDOWN; + + /* use the buffer lock here for triple duty: + * - prevent I/O (from us) so calling spi_setup() is safe; + * - prevent concurrent SPI_IOC_WR_* from morphing + * data fields while SPI_IOC_RD_* reads them; + * - SPI_IOC_MESSAGE needs the buffer locked "normally". + */ + mutex_lock(&spidev->buf_lock); + + switch (cmd) { + /* read requests */ + case SPI_IOC_RD_MODE: + retval = __put_user(spi->mode & SPI_MODE_MASK, + (__u8 __user *)arg); + break; + case SPI_IOC_RD_MODE32: + retval = __put_user(spi->mode & SPI_MODE_MASK, + (__u32 __user *)arg); + break; + case SPI_IOC_RD_LSB_FIRST: + retval = panda_transfer(spidev, spi, arg); + //retval = __put_user((spi->mode & SPI_LSB_FIRST) ? 1 : 0, + // (__u8 __user *)arg); + break; + case SPI_IOC_RD_BITS_PER_WORD: + retval = __put_user(spi->bits_per_word, (__u8 __user *)arg); + break; + case SPI_IOC_RD_MAX_SPEED_HZ: + retval = __put_user(spidev->speed_hz, (__u32 __user *)arg); + break; + + /* write requests */ + case SPI_IOC_WR_MODE: + case SPI_IOC_WR_MODE32: + if (cmd == SPI_IOC_WR_MODE) + retval = __get_user(tmp, (u8 __user *)arg); + else + retval = __get_user(tmp, (u32 __user *)arg); + if (retval == 0) { + u32 save = spi->mode; + + if (tmp & ~SPI_MODE_MASK) { + retval = -EINVAL; + break; + } + + tmp |= spi->mode & ~SPI_MODE_MASK; + spi->mode = (u16)tmp; + retval = spi_setup(spi); + if (retval < 0) + spi->mode = save; + else + dev_dbg(&spi->dev, "spi mode %x\n", tmp); + } + break; + case SPI_IOC_WR_LSB_FIRST: + retval = __get_user(tmp, (__u8 __user *)arg); + if (retval == 0) { + u32 save = spi->mode; + + if (tmp) + spi->mode |= SPI_LSB_FIRST; + else + spi->mode &= ~SPI_LSB_FIRST; + retval = spi_setup(spi); + if (retval < 0) + spi->mode = save; + else + dev_dbg(&spi->dev, "%csb first\n", + tmp ? 'l' : 'm'); + } + break; + case SPI_IOC_WR_BITS_PER_WORD: + retval = __get_user(tmp, (__u8 __user *)arg); + if (retval == 0) { + u8 save = spi->bits_per_word; + + spi->bits_per_word = tmp; + retval = spi_setup(spi); + if (retval < 0) + spi->bits_per_word = save; + else + dev_dbg(&spi->dev, "%d bits per word\n", tmp); + } + break; + case SPI_IOC_WR_MAX_SPEED_HZ: + retval = __get_user(tmp, (__u32 __user *)arg); + if (retval == 0) { + u32 save = spi->max_speed_hz; + + spi->max_speed_hz = tmp; + retval = spi_setup(spi); + if (retval >= 0) + spidev->speed_hz = tmp; + else + dev_dbg(&spi->dev, "%d Hz (max)\n", tmp); + spi->max_speed_hz = save; + } + break; + + default: + /* segmented and/or full-duplex I/O request */ + /* Check message and copy into scratch area */ + ioc = spidev_get_ioc_message(cmd, + (struct spi_ioc_transfer __user *)arg, &n_ioc); + if (IS_ERR(ioc)) { + retval = PTR_ERR(ioc); + break; + } + if (!ioc) + break; /* n_ioc is also 0 */ + + /* translate to spi_message, execute */ + retval = spidev_message(spidev, ioc, n_ioc); + kfree(ioc); + break; + } + + mutex_unlock(&spidev->buf_lock); + spi_dev_put(spi); + return retval; +} + +#ifdef CONFIG_COMPAT +static long +spidev_compat_ioc_message(struct file *filp, unsigned int cmd, + unsigned long arg) +{ + struct spi_ioc_transfer __user *u_ioc; + int retval = 0; + struct spidev_data *spidev; + struct spi_device *spi; + unsigned n_ioc, n; + struct spi_ioc_transfer *ioc; + + u_ioc = (struct spi_ioc_transfer __user *) compat_ptr(arg); + if (!access_ok(VERIFY_READ, u_ioc, _IOC_SIZE(cmd))) + return -EFAULT; + + /* guard against device removal before, or while, + * we issue this ioctl. + */ + spidev = filp->private_data; + spin_lock_irq(&spidev->spi_lock); + spi = spi_dev_get(spidev->spi); + spin_unlock_irq(&spidev->spi_lock); + + if (spi == NULL) + return -ESHUTDOWN; + + /* SPI_IOC_MESSAGE needs the buffer locked "normally" */ + mutex_lock(&spidev->buf_lock); + + /* Check message and copy into scratch area */ + ioc = spidev_get_ioc_message(cmd, u_ioc, &n_ioc); + if (IS_ERR(ioc)) { + retval = PTR_ERR(ioc); + goto done; + } + if (!ioc) + goto done; /* n_ioc is also 0 */ + + /* Convert buffer pointers */ + for (n = 0; n < n_ioc; n++) { + ioc[n].rx_buf = (uintptr_t) compat_ptr(ioc[n].rx_buf); + ioc[n].tx_buf = (uintptr_t) compat_ptr(ioc[n].tx_buf); + } + + /* translate to spi_message, execute */ + retval = spidev_message(spidev, ioc, n_ioc); + kfree(ioc); + +done: + mutex_unlock(&spidev->buf_lock); + spi_dev_put(spi); + return retval; +} + +static long +spidev_compat_ioctl(struct file *filp, unsigned int cmd, unsigned long arg) +{ + if (_IOC_TYPE(cmd) == SPI_IOC_MAGIC + && _IOC_NR(cmd) == _IOC_NR(SPI_IOC_MESSAGE(0)) + && _IOC_DIR(cmd) == _IOC_WRITE) + return spidev_compat_ioc_message(filp, cmd, arg); + + return spidev_ioctl(filp, cmd, (unsigned long)compat_ptr(arg)); +} +#else +#define spidev_compat_ioctl NULL +#endif /* CONFIG_COMPAT */ + +static int spidev_open(struct inode *inode, struct file *filp) +{ + struct spidev_data *spidev; + int status = -ENXIO; + + mutex_lock(&device_list_lock); + + list_for_each_entry(spidev, &device_list, device_entry) { + if (spidev->devt == inode->i_rdev) { + status = 0; + break; + } + } + + if (status) { + pr_debug("spidev: nothing for minor %d\n", iminor(inode)); + goto err_find_dev; + } + + if (!spidev->tx_buffer) { + spidev->tx_buffer = kmalloc(bufsiz, GFP_KERNEL); + if (!spidev->tx_buffer) { + dev_dbg(&spidev->spi->dev, "open/ENOMEM\n"); + status = -ENOMEM; + goto err_find_dev; + } + } + + if (!spidev->rx_buffer) { + spidev->rx_buffer = kmalloc(bufsiz, GFP_KERNEL); + if (!spidev->rx_buffer) { + dev_dbg(&spidev->spi->dev, "open/ENOMEM\n"); + status = -ENOMEM; + goto err_alloc_rx_buf; + } + } + + spidev->users++; + filp->private_data = spidev; + nonseekable_open(inode, filp); + + mutex_unlock(&device_list_lock); + return 0; + +err_alloc_rx_buf: + kfree(spidev->tx_buffer); + spidev->tx_buffer = NULL; +err_find_dev: + mutex_unlock(&device_list_lock); + return status; +} + +static int spidev_release(struct inode *inode, struct file *filp) +{ + struct spidev_data *spidev; + + mutex_lock(&device_list_lock); + spidev = filp->private_data; + filp->private_data = NULL; + + /* last close? */ + spidev->users--; + if (!spidev->users) { + int dofree; + + kfree(spidev->tx_buffer); + spidev->tx_buffer = NULL; + + kfree(spidev->rx_buffer); + spidev->rx_buffer = NULL; + + spin_lock_irq(&spidev->spi_lock); + if (spidev->spi) + spidev->speed_hz = spidev->spi->max_speed_hz; + + /* ... after we unbound from the underlying device? */ + dofree = (spidev->spi == NULL); + spin_unlock_irq(&spidev->spi_lock); + + if (dofree) + kfree(spidev); + } + mutex_unlock(&device_list_lock); + + return 0; +} + +static const struct file_operations spidev_fops = { + .owner = THIS_MODULE, + /* REVISIT switch to aio primitives, so that userspace + * gets more complete API coverage. It'll simplify things + * too, except for the locking. + */ + .write = spidev_write, + .read = spidev_read, + .unlocked_ioctl = spidev_ioctl, + .compat_ioctl = spidev_compat_ioctl, + .open = spidev_open, + .release = spidev_release, + .llseek = no_llseek, +}; + +/*-------------------------------------------------------------------------*/ + +/* The main reason to have this class is to make mdev/udev create the + * /dev/spidevB.C character device nodes exposing our userspace API. + * It also simplifies memory management. + */ + +static struct class *spidev_class; + +#ifdef CONFIG_OF +static const struct of_device_id spidev_dt_ids[] = { + { .compatible = "commaai,panda" }, + {}, +}; +MODULE_DEVICE_TABLE(of, spidev_dt_ids); +#endif + +#ifdef CONFIG_ACPI + +/* Dummy SPI devices not to be used in production systems */ +#define SPIDEV_ACPI_DUMMY 1 + +static const struct acpi_device_id spidev_acpi_ids[] = { + /* + * The ACPI SPT000* devices are only meant for development and + * testing. Systems used in production should have a proper ACPI + * description of the connected peripheral and they should also use + * a proper driver instead of poking directly to the SPI bus. + */ + { "SPT0001", SPIDEV_ACPI_DUMMY }, + { "SPT0002", SPIDEV_ACPI_DUMMY }, + { "SPT0003", SPIDEV_ACPI_DUMMY }, + {}, +}; +MODULE_DEVICE_TABLE(acpi, spidev_acpi_ids); + +static void spidev_probe_acpi(struct spi_device *spi) +{ + const struct acpi_device_id *id; + + if (!has_acpi_companion(&spi->dev)) + return; + + id = acpi_match_device(spidev_acpi_ids, &spi->dev); + if (WARN_ON(!id)) + return; + + if (id->driver_data == SPIDEV_ACPI_DUMMY) + dev_warn(&spi->dev, "do not use this driver in production systems!\n"); +} +#else +static inline void spidev_probe_acpi(struct spi_device *spi) {} +#endif + +/*-------------------------------------------------------------------------*/ + +static int spidev_probe(struct spi_device *spi) +{ + struct spidev_data *spidev; + int status; + unsigned long minor; + + /* + * spidev should never be referenced in DT without a specific + * compatible string, it is a Linux implementation thing + * rather than a description of the hardware. + */ + if (spi->dev.of_node && !of_match_device(spidev_dt_ids, &spi->dev)) { + dev_err(&spi->dev, "buggy DT: spidev listed directly in DT\n"); + WARN_ON(spi->dev.of_node && + !of_match_device(spidev_dt_ids, &spi->dev)); + } + + spidev_probe_acpi(spi); + + /* Allocate driver data */ + spidev = kzalloc(sizeof(*spidev), GFP_KERNEL); + if (!spidev) + return -ENOMEM; + + /* Initialize the driver data */ + spidev->spi = spi; + spin_lock_init(&spidev->spi_lock); + mutex_init(&spidev->buf_lock); + + INIT_LIST_HEAD(&spidev->device_entry); + + /* If we can allocate a minor number, hook up this device. + * Reusing minors is fine so long as udev or mdev is working. + */ + mutex_lock(&device_list_lock); + minor = find_first_zero_bit(minors, N_SPI_MINORS); + if (minor < N_SPI_MINORS) { + struct device *dev; + + spidev->devt = MKDEV(SPIDEV_MAJOR, minor); + dev = device_create(spidev_class, &spi->dev, spidev->devt, + spidev, "spidev%d.%d", + spi->master->bus_num, spi->chip_select); + status = PTR_ERR_OR_ZERO(dev); + } else { + dev_dbg(&spi->dev, "no minor number available!\n"); + status = -ENODEV; + } + if (status == 0) { + set_bit(minor, minors); + list_add(&spidev->device_entry, &device_list); + } + mutex_unlock(&device_list_lock); + + spidev->speed_hz = spi->max_speed_hz; + + if (status == 0) + spi_set_drvdata(spi, spidev); + else + kfree(spidev); + + return status; +} + +static int spidev_remove(struct spi_device *spi) +{ + struct spidev_data *spidev = spi_get_drvdata(spi); + + /* make sure ops on existing fds can abort cleanly */ + spin_lock_irq(&spidev->spi_lock); + spidev->spi = NULL; + spin_unlock_irq(&spidev->spi_lock); + + /* prevent new opens */ + mutex_lock(&device_list_lock); + list_del(&spidev->device_entry); + device_destroy(spidev_class, spidev->devt); + clear_bit(MINOR(spidev->devt), minors); + if (spidev->users == 0) + kfree(spidev); + mutex_unlock(&device_list_lock); + + return 0; +} + +static struct spi_driver spidev_spi_driver = { + .driver = { + .name = "spidev_panda", + .of_match_table = of_match_ptr(spidev_dt_ids), + .acpi_match_table = ACPI_PTR(spidev_acpi_ids), + }, + .probe = spidev_probe, + .remove = spidev_remove, + + /* NOTE: suspend/resume methods are not necessary here. + * We don't do anything except pass the requests to/from + * the underlying controller. The refrigerator handles + * most issues; the controller driver handles the rest. + */ +}; + +/*-------------------------------------------------------------------------*/ + +static int __init spidev_init(void) +{ + int status; + + /* Claim our 256 reserved device numbers. Then register a class + * that will key udev/mdev to add/remove /dev nodes. Last, register + * the driver which manages those device numbers. + */ + BUILD_BUG_ON(N_SPI_MINORS > 256); + status = register_chrdev(0, "spi", &spidev_fops); + if (status < 0) + return status; + + SPIDEV_MAJOR = status; + + spidev_class = class_create(THIS_MODULE, "spidev_panda"); + if (IS_ERR(spidev_class)) { + unregister_chrdev(SPIDEV_MAJOR, spidev_spi_driver.driver.name); + return PTR_ERR(spidev_class); + } + + status = spi_register_driver(&spidev_spi_driver); + if (status < 0) { + class_destroy(spidev_class); + unregister_chrdev(SPIDEV_MAJOR, spidev_spi_driver.driver.name); + } + return status; +} +module_init(spidev_init); + +static void __exit spidev_exit(void) +{ + spi_unregister_driver(&spidev_spi_driver); + class_destroy(spidev_class); + unregister_chrdev(SPIDEV_MAJOR, spidev_spi_driver.driver.name); +} +module_exit(spidev_exit); + +MODULE_AUTHOR("Andrea Paterniani, "); +MODULE_DESCRIPTION("User mode SPI device interface"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("spi:spidev"); diff --git a/python/spi.py b/python/spi.py index 26b188439e..b73aea1dba 100644 --- a/python/spi.py +++ b/python/spi.py @@ -1,4 +1,5 @@ import binascii +import ctypes import os import fcntl import math @@ -16,8 +17,10 @@ try: import spidev + import spidev2 except ImportError: spidev = None + spidev2 = None # Constants SYNC = 0x5A @@ -55,6 +58,17 @@ class PandaSpiTransferFailed(PandaSpiException): SPI_LOCK = threading.Lock() +class PandaSpiTransfer(ctypes.Structure): + _fields_ = [ + ('rx_buf', ctypes.c_uint64), + ('tx_buf', ctypes.c_uint64), + ('tx_length', ctypes.c_uint32), + ('rx_length_max', ctypes.c_uint32), + ('timeout', ctypes.c_uint32), + ('endpoint', ctypes.c_uint8), + ('expect_disconnect', ctypes.c_uint8), + ] + class SpiDevice: """ Provides locked, thread-safe access to a panda's SPI interface. @@ -90,6 +104,7 @@ def close(self): self._spidev.close() + class PandaSpiHandle(BaseHandle): """ A class that mimics a libusb1 handle for panda SPI communications. @@ -97,6 +112,21 @@ class PandaSpiHandle(BaseHandle): def __init__(self): self.dev = SpiDevice() + self._transfer = self._transfer_spidev + + if "KERN" in os.environ: + self._transfer = self._transfer_kernel_driver + + self.tx_buf = bytearray(1024) + self.rx_buf = bytearray(1024) + tx_buf_raw = ctypes.c_char.from_buffer(self.tx_buf) + rx_buf_raw = ctypes.c_char.from_buffer(self.rx_buf) + + self.ioctl_data = PandaSpiTransfer() + self.ioctl_data.tx_buf = ctypes.addressof(tx_buf_raw) + self.ioctl_data.rx_buf = ctypes.addressof(rx_buf_raw) + self.fileno = self.dev._spidev.fileno() + # helpers def _calc_checksum(self, data: List[int]) -> int: cksum = CHECKSUM_START @@ -117,7 +147,23 @@ def _wait_for_ack(self, spi, ack_val: int, timeout: int, tx: int) -> None: raise PandaSpiMissingAck - def _transfer(self, spi, endpoint: int, data, timeout: int, max_rx_len: int = 1000, expect_disconnect: bool = False) -> bytes: + def _transfer_kernel_driver(self, spi, endpoint: int, data, timeout: int, max_rx_len: int = 1000, expect_disconnect: bool = False) -> bytes: + self.tx_buf[:len(data)] = data + self.ioctl_data.endpoint = endpoint + self.ioctl_data.tx_length = len(data) + self.ioctl_data.rx_length_max = max_rx_len + self.ioctl_data.expect_disconnect = int(expect_disconnect) + + # TODO: use our own ioctl request + try: + ret = fcntl.ioctl(self.fileno, spidev2.SPI_IOC_RD_LSB_FIRST, self.ioctl_data) + except OSError as e: + raise PandaSpiException from e + if ret < 0: + raise PandaSpiException(f"ioctl returned {ret}") + return bytes(self.rx_buf[:ret]) + + def _transfer_spidev(self, spi, endpoint: int, data, timeout: int, max_rx_len: int = 1000, expect_disconnect: bool = False) -> bytes: logging.debug("starting transfer: endpoint=%d, max_rx_len=%d", endpoint, max_rx_len) logging.debug("==============================================") From 6e5009125a37b31d0d1a6c16a975e5616c4a5c0b Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Sun, 16 Jul 2023 12:13:09 +0100 Subject: [PATCH 2/5] Ford: namespaced constants (#1516) --- board/safety/safety_ford.h | 130 ++++++++++++++++++------------------- 1 file changed, 65 insertions(+), 65 deletions(-) diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 98cf204d99..5fcdcdc444 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -1,53 +1,53 @@ // Safety-relevant CAN messages for Ford vehicles. -#define MSG_EngBrakeData 0x165 // RX from PCM, for driver brake pedal and cruise state -#define MSG_EngVehicleSpThrottle 0x204 // RX from PCM, for driver throttle input -#define MSG_DesiredTorqBrk 0x213 // RX from ABS, for standstill state -#define MSG_BrakeSysFeatures 0x415 // RX from ABS, for vehicle speed -#define MSG_EngVehicleSpThrottle2 0x202 // RX from PCM, for second vehicle speed -#define MSG_Yaw_Data_FD1 0x91 // RX from RCM, for yaw rate -#define MSG_Steering_Data_FD1 0x083 // TX by OP, various driver switches and LKAS/CC buttons -#define MSG_ACCDATA 0x186 // TX by OP, ACC controls -#define MSG_ACCDATA_3 0x18A // TX by OP, ACC/TJA user interface -#define MSG_Lane_Assist_Data1 0x3CA // TX by OP, Lane Keep Assist -#define MSG_LateralMotionControl 0x3D3 // TX by OP, Traffic Jam Assist -#define MSG_IPMA_Data 0x3D8 // TX by OP, IPMA and LKAS user interface +#define FORD_EngBrakeData 0x165 // RX from PCM, for driver brake pedal and cruise state +#define FORD_EngVehicleSpThrottle 0x204 // RX from PCM, for driver throttle input +#define FORD_DesiredTorqBrk 0x213 // RX from ABS, for standstill state +#define FORD_BrakeSysFeatures 0x415 // RX from ABS, for vehicle speed +#define FORD_EngVehicleSpThrottle2 0x202 // RX from PCM, for second vehicle speed +#define FORD_Yaw_Data_FD1 0x91 // RX from RCM, for yaw rate +#define FORD_Steering_Data_FD1 0x083 // TX by OP, various driver switches and LKAS/CC buttons +#define FORD_ACCDATA 0x186 // TX by OP, ACC controls +#define FORD_ACCDATA_3 0x18A // TX by OP, ACC/TJA user interface +#define FORD_Lane_Assist_Data1 0x3CA // TX by OP, Lane Keep Assist +#define FORD_LateralMotionControl 0x3D3 // TX by OP, Traffic Jam Assist +#define FORD_IPMA_Data 0x3D8 // TX by OP, IPMA and LKAS user interface // CAN bus numbers. #define FORD_MAIN_BUS 0U #define FORD_CAM_BUS 2U const CanMsg FORD_STOCK_TX_MSGS[] = { - {MSG_Steering_Data_FD1, 0, 8}, - {MSG_Steering_Data_FD1, 2, 8}, - {MSG_ACCDATA_3, 0, 8}, - {MSG_Lane_Assist_Data1, 0, 8}, - {MSG_LateralMotionControl, 0, 8}, - {MSG_IPMA_Data, 0, 8}, + {FORD_Steering_Data_FD1, 0, 8}, + {FORD_Steering_Data_FD1, 2, 8}, + {FORD_ACCDATA_3, 0, 8}, + {FORD_Lane_Assist_Data1, 0, 8}, + {FORD_LateralMotionControl, 0, 8}, + {FORD_IPMA_Data, 0, 8}, }; #define FORD_STOCK_TX_LEN (sizeof(FORD_STOCK_TX_MSGS) / sizeof(FORD_STOCK_TX_MSGS[0])) const CanMsg FORD_LONG_TX_MSGS[] = { - {MSG_Steering_Data_FD1, 0, 8}, - {MSG_Steering_Data_FD1, 2, 8}, - {MSG_ACCDATA, 0, 8}, - {MSG_ACCDATA_3, 0, 8}, - {MSG_Lane_Assist_Data1, 0, 8}, - {MSG_LateralMotionControl, 0, 8}, - {MSG_IPMA_Data, 0, 8}, + {FORD_Steering_Data_FD1, 0, 8}, + {FORD_Steering_Data_FD1, 2, 8}, + {FORD_ACCDATA, 0, 8}, + {FORD_ACCDATA_3, 0, 8}, + {FORD_Lane_Assist_Data1, 0, 8}, + {FORD_LateralMotionControl, 0, 8}, + {FORD_IPMA_Data, 0, 8}, }; #define FORD_LONG_TX_LEN (sizeof(FORD_LONG_TX_MSGS) / sizeof(FORD_LONG_TX_MSGS[0])) // warning: quality flags are not yet checked in openpilot's CAN parser, // this may be the cause of blocked messages AddrCheckStruct ford_addr_checks[] = { - {.msg = {{MSG_BrakeSysFeatures, 0, 8, .check_checksum = true, .max_counter = 15U, .quality_flag=true, .expected_timestep = 20000U}, { 0 }, { 0 }}}, - // TODO: MSG_EngVehicleSpThrottle2 has a counter that skips by 2, understand and enable counter check - {.msg = {{MSG_EngVehicleSpThrottle2, 0, 8, .check_checksum = true, .quality_flag=true, .expected_timestep = 20000U}, { 0 }, { 0 }}}, - {.msg = {{MSG_Yaw_Data_FD1, 0, 8, .check_checksum = true, .max_counter = 255U, .quality_flag=true, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{FORD_BrakeSysFeatures, 0, 8, .check_checksum = true, .max_counter = 15U, .quality_flag=true, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + // TODO: FORD_EngVehicleSpThrottle2 has a counter that skips by 2, understand and enable counter check + {.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, .check_checksum = true, .quality_flag=true, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{FORD_Yaw_Data_FD1, 0, 8, .check_checksum = true, .max_counter = 255U, .quality_flag=true, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // These messages have no counter or checksum - {.msg = {{MSG_EngBrakeData, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, - {.msg = {{MSG_EngVehicleSpThrottle, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, - {.msg = {{MSG_DesiredTorqBrk, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{FORD_EngBrakeData, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, + {.msg = {{FORD_EngVehicleSpThrottle, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{FORD_DesiredTorqBrk, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, }; #define FORD_ADDR_CHECK_LEN (sizeof(ford_addr_checks) / sizeof(ford_addr_checks[0])) addr_checks ford_rx_checks = {ford_addr_checks, FORD_ADDR_CHECK_LEN}; @@ -56,13 +56,13 @@ static uint8_t ford_get_counter(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); uint8_t cnt; - if (addr == MSG_BrakeSysFeatures) { + if (addr == FORD_BrakeSysFeatures) { // Signal: VehVActlBrk_No_Cnt cnt = (GET_BYTE(to_push, 2) >> 2) & 0xFU; - } else if (addr == MSG_EngVehicleSpThrottle2) { + } else if (addr == FORD_EngVehicleSpThrottle2) { // Signal: VehVActlEng_No_Cnt cnt = (GET_BYTE(to_push, 2) >> 3) & 0xFU; - } else if (addr == MSG_Yaw_Data_FD1) { + } else if (addr == FORD_Yaw_Data_FD1) { // Signal: VehRollYaw_No_Cnt cnt = GET_BYTE(to_push, 5); } else { @@ -75,13 +75,13 @@ static uint32_t ford_get_checksum(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); uint8_t chksum; - if (addr == MSG_BrakeSysFeatures) { + if (addr == FORD_BrakeSysFeatures) { // Signal: VehVActlBrk_No_Cs chksum = GET_BYTE(to_push, 3); - } else if (addr == MSG_EngVehicleSpThrottle2) { + } else if (addr == FORD_EngVehicleSpThrottle2) { // Signal: VehVActlEng_No_Cs chksum = GET_BYTE(to_push, 1); - } else if (addr == MSG_Yaw_Data_FD1) { + } else if (addr == FORD_Yaw_Data_FD1) { // Signal: VehRollYawW_No_Cs chksum = GET_BYTE(to_push, 4); } else { @@ -94,17 +94,17 @@ static uint32_t ford_compute_checksum(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); uint8_t chksum = 0; - if (addr == MSG_BrakeSysFeatures) { + if (addr == FORD_BrakeSysFeatures) { chksum += GET_BYTE(to_push, 0) + GET_BYTE(to_push, 1); // Veh_V_ActlBrk chksum += GET_BYTE(to_push, 2) >> 6; // VehVActlBrk_D_Qf chksum += (GET_BYTE(to_push, 2) >> 2) & 0xFU; // VehVActlBrk_No_Cnt chksum = 0xFFU - chksum; - } else if (addr == MSG_EngVehicleSpThrottle2) { + } else if (addr == FORD_EngVehicleSpThrottle2) { chksum += (GET_BYTE(to_push, 2) >> 3) & 0xFU; // VehVActlEng_No_Cnt chksum += (GET_BYTE(to_push, 4) >> 5) & 0x3U; // VehVActlEng_D_Qf chksum += GET_BYTE(to_push, 6) + GET_BYTE(to_push, 7); // Veh_V_ActlEng chksum = 0xFFU - chksum; - } else if (addr == MSG_Yaw_Data_FD1) { + } else if (addr == FORD_Yaw_Data_FD1) { chksum += GET_BYTE(to_push, 0) + GET_BYTE(to_push, 1); // VehRol_W_Actl chksum += GET_BYTE(to_push, 2) + GET_BYTE(to_push, 3); // VehYaw_W_Actl chksum += GET_BYTE(to_push, 5); // VehRollYaw_No_Cnt @@ -121,11 +121,11 @@ static bool ford_get_quality_flag_valid(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); bool valid = false; - if (addr == MSG_BrakeSysFeatures) { + if (addr == FORD_BrakeSysFeatures) { valid = (GET_BYTE(to_push, 2) >> 6) == 0x3U; // VehVActlBrk_D_Qf - } else if (addr == MSG_EngVehicleSpThrottle2) { + } else if (addr == FORD_EngVehicleSpThrottle2) { valid = ((GET_BYTE(to_push, 4) >> 5) & 0x3U) == 0x3U; // VehVActlEng_D_Qf - } else if (addr == MSG_Yaw_Data_FD1) { + } else if (addr == FORD_Yaw_Data_FD1) { valid = ((GET_BYTE(to_push, 6) >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf } else { } @@ -150,17 +150,17 @@ const LongitudinalLimits FORD_LONG_LIMITS = { .inactive_gas = 0, // -5.0 m/s^2 }; -#define INACTIVE_CURVATURE 1000U -#define INACTIVE_CURVATURE_RATE 4096U -#define INACTIVE_PATH_OFFSET 512U -#define INACTIVE_PATH_ANGLE 1000U +#define FORD_INACTIVE_CURVATURE 1000U +#define FORD_INACTIVE_CURVATURE_RATE 4096U +#define FORD_INACTIVE_PATH_OFFSET 512U +#define FORD_INACTIVE_PATH_ANGLE 1000U #define FORD_MAX_SPEED_DELTA 2.0 // m/s static bool ford_lkas_msg_check(int addr) { - return (addr == MSG_ACCDATA_3) - || (addr == MSG_Lane_Assist_Data1) - || (addr == MSG_LateralMotionControl) - || (addr == MSG_IPMA_Data); + return (addr == FORD_ACCDATA_3) + || (addr == FORD_Lane_Assist_Data1) + || (addr == FORD_LateralMotionControl) + || (addr == FORD_IPMA_Data); } // Curvature rate limits @@ -192,19 +192,19 @@ static int ford_rx_hook(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); // Update in motion state from standstill signal - if (addr == MSG_DesiredTorqBrk) { + if (addr == FORD_DesiredTorqBrk) { // Signal: VehStop_D_Stat vehicle_moving = ((GET_BYTE(to_push, 3) >> 3) & 0x3U) == 0U; } // Update vehicle speed - if (addr == MSG_BrakeSysFeatures) { + if (addr == FORD_BrakeSysFeatures) { // Signal: Veh_V_ActlBrk update_sample(&vehicle_speed, ROUND(((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6 * VEHICLE_SPEED_FACTOR)); } // Check vehicle speed against a second source - if (addr == MSG_EngVehicleSpThrottle2) { + if (addr == FORD_EngVehicleSpThrottle2) { // Disable controls if speeds from ABS and PCM ECUs are too far apart. // Signal: Veh_V_ActlEng float filtered_pcm_speed = ((GET_BYTE(to_push, 6) << 8) | GET_BYTE(to_push, 7)) * 0.01 / 3.6; @@ -214,7 +214,7 @@ static int ford_rx_hook(CANPacket_t *to_push) { } // Update vehicle yaw rate - if (addr == MSG_Yaw_Data_FD1) { + if (addr == FORD_Yaw_Data_FD1) { // Signal: VehYaw_W_Actl float ford_yaw_rate = (((GET_BYTE(to_push, 2) << 8U) | GET_BYTE(to_push, 3)) * 0.0002) - 6.5; float current_curvature = ford_yaw_rate / MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1); @@ -223,14 +223,14 @@ static int ford_rx_hook(CANPacket_t *to_push) { } // Update gas pedal - if (addr == MSG_EngVehicleSpThrottle) { + if (addr == FORD_EngVehicleSpThrottle) { // Pedal position: (0.1 * val) in percent // Signal: ApedPos_Pc_ActlArb gas_pressed = (((GET_BYTE(to_push, 0) & 0x03U) << 8) | GET_BYTE(to_push, 1)) > 0U; } // Update brake pedal and cruise state - if (addr == MSG_EngBrakeData) { + if (addr == FORD_EngBrakeData) { // Signal: BpedDrvAppl_D_Actl brake_pressed = ((GET_BYTE(to_push, 0) >> 4) & 0x3U) == 2U; @@ -259,7 +259,7 @@ static int ford_tx_hook(CANPacket_t *to_send) { } // Safety check for ACCDATA accel and brake requests - if (addr == MSG_ACCDATA) { + if (addr == FORD_ACCDATA) { // Signal: AccPrpl_A_Rq int gas = ((GET_BYTE(to_send, 6) & 0x3U) << 8) | GET_BYTE(to_send, 7); // Signal: AccBrkTot_A_Rq @@ -282,7 +282,7 @@ static int ford_tx_hook(CANPacket_t *to_send) { // Safety check for Steering_Data_FD1 button signals // Note: Many other signals in this message are not relevant to safety (e.g. blinkers, wiper switches, high beam) // which we passthru in OP. - if (addr == MSG_Steering_Data_FD1) { + if (addr == FORD_Steering_Data_FD1) { // Violation if resume button is pressed while controls not allowed, or // if cancel button is pressed when cruise isn't engaged. bool violation = false; @@ -295,7 +295,7 @@ static int ford_tx_hook(CANPacket_t *to_send) { } // Safety check for Lane_Assist_Data1 action - if (addr == MSG_Lane_Assist_Data1) { + if (addr == FORD_Lane_Assist_Data1) { // Do not allow steering using Lane_Assist_Data1 (Lane-Departure Aid). // This message must be sent for Lane Centering to work, and can include // values such as the steering angle or lane curvature for debugging, @@ -307,7 +307,7 @@ static int ford_tx_hook(CANPacket_t *to_send) { } // Safety check for LateralMotionControl action - if (addr == MSG_LateralMotionControl) { + if (addr == FORD_LateralMotionControl) { // Signal: LatCtl_D_Rq bool steer_control_enabled = ((GET_BYTE(to_send, 4) >> 2) & 0x7U) != 0U; unsigned int raw_curvature = (GET_BYTE(to_send, 0) << 3) | (GET_BYTE(to_send, 1) >> 5); @@ -316,10 +316,10 @@ static int ford_tx_hook(CANPacket_t *to_send) { unsigned int raw_path_offset = (GET_BYTE(to_send, 5) << 2) | (GET_BYTE(to_send, 6) >> 6); // These signals are not yet tested with the current safety limits - bool violation = (raw_curvature_rate != INACTIVE_CURVATURE_RATE) || (raw_path_angle != INACTIVE_PATH_ANGLE) || (raw_path_offset != INACTIVE_PATH_OFFSET); + bool violation = (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); // Check angle error and steer_control_enabled - int desired_curvature = raw_curvature - INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature + int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); if (violation) { @@ -344,7 +344,7 @@ static int ford_fwd_hook(int bus_num, int addr) { if (ford_lkas_msg_check(addr)) { // Block stock LKAS and UI messages bus_fwd = -1; - } else if (ford_longitudinal && (addr == MSG_ACCDATA)) { + } else if (ford_longitudinal && (addr == FORD_ACCDATA)) { // Block stock ACC message bus_fwd = -1; } else { From 0c3355462483cf3150b7d481c76cbf0659c16c7a Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Sun, 16 Jul 2023 15:09:37 +0100 Subject: [PATCH 3/5] Ford CAN FD safety (#1455) --- board/safety/safety_ford.h | 69 ++++++++++++++++++++-- python/__init__.py | 1 + tests/safety/common.py | 2 +- tests/safety/test_ford.py | 117 ++++++++++++++++++++++++++++--------- 4 files changed, 153 insertions(+), 36 deletions(-) diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 5fcdcdc444..4f71650c90 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -9,7 +9,8 @@ #define FORD_ACCDATA 0x186 // TX by OP, ACC controls #define FORD_ACCDATA_3 0x18A // TX by OP, ACC/TJA user interface #define FORD_Lane_Assist_Data1 0x3CA // TX by OP, Lane Keep Assist -#define FORD_LateralMotionControl 0x3D3 // TX by OP, Traffic Jam Assist +#define FORD_LateralMotionControl 0x3D3 // TX by OP, Lateral Control message +#define FORD_LateralMotionControl2 0x3D6 // TX by OP, alternate Lateral Control message #define FORD_IPMA_Data 0x3D8 // TX by OP, IPMA and LKAS user interface // CAN bus numbers. @@ -37,6 +38,27 @@ const CanMsg FORD_LONG_TX_MSGS[] = { }; #define FORD_LONG_TX_LEN (sizeof(FORD_LONG_TX_MSGS) / sizeof(FORD_LONG_TX_MSGS[0])) +const CanMsg FORD_CANFD_STOCK_TX_MSGS[] = { + {FORD_Steering_Data_FD1, 0, 8}, + {FORD_Steering_Data_FD1, 2, 8}, + {FORD_ACCDATA_3, 0, 8}, + {FORD_Lane_Assist_Data1, 0, 8}, + {FORD_LateralMotionControl2, 0, 8}, + {FORD_IPMA_Data, 0, 8}, +}; +#define FORD_CANFD_STOCK_TX_LEN (sizeof(FORD_CANFD_STOCK_TX_MSGS) / sizeof(FORD_CANFD_STOCK_TX_MSGS[0])) + +const CanMsg FORD_CANFD_LONG_TX_MSGS[] = { + {FORD_Steering_Data_FD1, 0, 8}, + {FORD_Steering_Data_FD1, 2, 8}, + {FORD_ACCDATA, 0, 8}, + {FORD_ACCDATA_3, 0, 8}, + {FORD_Lane_Assist_Data1, 0, 8}, + {FORD_LateralMotionControl2, 0, 8}, + {FORD_IPMA_Data, 0, 8}, +}; +#define FORD_CANFD_LONG_TX_LEN (sizeof(FORD_CANFD_LONG_TX_MSGS) / sizeof(FORD_CANFD_LONG_TX_MSGS[0])) + // warning: quality flags are not yet checked in openpilot's CAN parser, // this may be the cause of blocked messages AddrCheckStruct ford_addr_checks[] = { @@ -133,8 +155,10 @@ static bool ford_get_quality_flag_valid(CANPacket_t *to_push) { } const uint16_t FORD_PARAM_LONGITUDINAL = 1; +const uint16_t FORD_PARAM_CANFD = 2; bool ford_longitudinal = false; +bool ford_canfd = false; const LongitudinalLimits FORD_LONG_LIMITS = { // acceleration cmd limits (used for brakes) @@ -154,12 +178,16 @@ const LongitudinalLimits FORD_LONG_LIMITS = { #define FORD_INACTIVE_CURVATURE_RATE 4096U #define FORD_INACTIVE_PATH_OFFSET 512U #define FORD_INACTIVE_PATH_ANGLE 1000U + +#define FORD_CANFD_INACTIVE_CURVATURE_RATE 1024U + #define FORD_MAX_SPEED_DELTA 2.0 // m/s static bool ford_lkas_msg_check(int addr) { return (addr == FORD_ACCDATA_3) || (addr == FORD_Lane_Assist_Data1) || (addr == FORD_LateralMotionControl) + || (addr == FORD_LateralMotionControl2) || (addr == FORD_IPMA_Data); } @@ -249,13 +277,20 @@ static int ford_rx_hook(CANPacket_t *to_push) { } static int ford_tx_hook(CANPacket_t *to_send) { - int tx = 1; int addr = GET_ADDR(to_send); - - if (ford_longitudinal) { - tx = msg_allowed(to_send, FORD_LONG_TX_MSGS, FORD_LONG_TX_LEN); + int tx; + if (ford_canfd) { + if (ford_longitudinal) { + tx = msg_allowed(to_send, FORD_CANFD_LONG_TX_MSGS, FORD_CANFD_LONG_TX_LEN); + } else { + tx = msg_allowed(to_send, FORD_CANFD_STOCK_TX_MSGS, FORD_CANFD_STOCK_TX_LEN); + } } else { - tx = msg_allowed(to_send, FORD_STOCK_TX_MSGS, FORD_STOCK_TX_LEN); + if (ford_longitudinal) { + tx = msg_allowed(to_send, FORD_LONG_TX_MSGS, FORD_LONG_TX_LEN); + } else { + tx = msg_allowed(to_send, FORD_STOCK_TX_MSGS, FORD_STOCK_TX_LEN); + } } // Safety check for ACCDATA accel and brake requests @@ -327,6 +362,27 @@ static int ford_tx_hook(CANPacket_t *to_send) { } } + // Safety check for LateralMotionControl2 action + if (addr == FORD_LateralMotionControl2) { + // Signal: LatCtl_D2_Rq + bool steer_control_enabled = ((GET_BYTE(to_send, 0) >> 4) & 0x7U) != 0U; + unsigned int raw_curvature = (GET_BYTE(to_send, 2) << 3) | (GET_BYTE(to_send, 3) >> 5); + unsigned int raw_curvature_rate = (GET_BYTE(to_send, 6) << 3) | (GET_BYTE(to_send, 7) >> 5); + unsigned int raw_path_angle = ((GET_BYTE(to_send, 3) & 0x1FU) << 6) | (GET_BYTE(to_send, 4) >> 2); + unsigned int raw_path_offset = ((GET_BYTE(to_send, 4) & 0x3U) << 8) | GET_BYTE(to_send, 5); + + // These signals are not yet tested with the current safety limits + bool violation = (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); + + // Check angle error and steer_control_enabled + int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature + violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); + + if (violation) { + tx = 0; + } + } + // 1 allows the message through return tx; } @@ -367,6 +423,7 @@ static const addr_checks* ford_init(uint16_t param) { UNUSED(param); #ifdef ALLOW_DEBUG ford_longitudinal = GET_FLAG(param, FORD_PARAM_LONGITUDINAL); + ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD); #endif return &ford_rx_checks; } diff --git a/python/__init__.py b/python/__init__.py index ffa0b1bec0..26a70ff260 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -254,6 +254,7 @@ class Panda: FLAG_GM_HW_CAM_LONG = 2 FLAG_FORD_LONG_CONTROL = 1 + FLAG_FORD_CANFD = 2 def __init__(self, serial: Optional[str] = None, claim: bool = True, disable_checks: bool = True): self._connect_serial = serial diff --git a/tests/safety/common.py b/tests/safety/common.py index 24b3f9b726..1b141d85e6 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -901,7 +901,7 @@ def test_tx_hook_on_wrong_safety_mode(self): continue if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety'}): continue - if {attr, current_test}.issubset({'TestFordStockSafety', 'TestFordLongitudinalSafety'}): + if attr.startswith('TestFord') and current_test.startswith('TestFord'): continue if attr.startswith('TestHyundaiCanfd') and current_test.startswith('TestHyundaiCanfd'): continue diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py index dfc317fa1d..7d2a128f53 100755 --- a/tests/safety/test_ford.py +++ b/tests/safety/test_ford.py @@ -17,7 +17,8 @@ MSG_ACCDATA = 0x186 # TX by OP, ACC controls MSG_ACCDATA_3 = 0x18A # TX by OP, ACC/TJA user interface MSG_Lane_Assist_Data1 = 0x3CA # TX by OP, Lane Keep Assist -MSG_LateralMotionControl = 0x3D3 # TX by OP, Traffic Jam Assist +MSG_LateralMotionControl = 0x3D3 # TX by OP, Lateral Control message +MSG_LateralMotionControl2 = 0x3D6 # TX by OP, alternate Lateral Control message MSG_IPMA_Data = 0x3D8 # TX by OP, IPMA and LKAS user interface @@ -57,18 +58,26 @@ class Buttons: TJA_TOGGLE = 2 -# Ford safety has two different configurations tested here: -# * stock longitudinal -# * openpilot longitudinal +# Ford safety has four different configurations tested here: +# * CAN with stock longitudinal +# * CAN with openpilot longitudinal +# * CAN FD with stock longitudinal +# * CAN FD with openpilot longitudinal class TestFordSafetyBase(common.PandaSafetyTest): STANDSTILL_THRESHOLD = 1 RELAY_MALFUNCTION_ADDR = MSG_IPMA_Data RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, + MSG_LateralMotionControl2, MSG_IPMA_Data]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + # Max allowed delta between car speeds MAX_SPEED_DELTA = 2.0 # m/s + STEER_MESSAGE = 0 + # Curvature control limits DEG_TO_CAN = 50000 # 1 / (2e-5) rad to can MAX_CURVATURE = 0.02 @@ -158,16 +167,26 @@ def _lkas_command_msg(self, action: int): } return self.packer.make_can_msg_panda("Lane_Assist_Data1", 0, values) - # TJA command - def _tja_command_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float): - values = { - "LatCtl_D_Rq": 1 if enabled else 0, - "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter - "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians - "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 - "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter - } - return self.packer.make_can_msg_panda("LateralMotionControl", 0, values) + # LCA command + def _lat_ctl_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float): + if self.STEER_MESSAGE == MSG_LateralMotionControl: + values = { + "LatCtl_D_Rq": 1 if enabled else 0, + "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians + "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter + } + return self.packer.make_can_msg_panda("LateralMotionControl", 0, values) + elif self.STEER_MESSAGE == MSG_LateralMotionControl2: + values = { + "LatCtl_D2_Rq": 1 if enabled else 0, + "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians + "LatCtlCrv_NoRate2_Actl": curvature_rate, # Curvature rate [-0.001024|0.001023] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter + } + return self.packer.make_can_msg_panda("LateralMotionControl2", 0, values) # Cruise control buttons def _acc_button_msg(self, button: int, bus: int): @@ -258,7 +277,7 @@ def test_steer_allowed(self): with self.subTest(controls_allowed=controls_allowed, steer_control_enabled=steer_control_enabled, path_offset=path_offset, path_angle=path_angle, curvature_rate=curvature_rate, curvature=curvature): - self.assertEqual(should_tx, self._tx(self._tja_command_msg(steer_control_enabled, path_offset, path_angle, curvature, curvature_rate))) + self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(steer_control_enabled, path_offset, path_angle, curvature, curvature_rate))) def test_curvature_rate_limit_up(self): """ @@ -285,7 +304,7 @@ def test_curvature_rate_limit_up(self): self._reset_curvature_measurement(sign * (self.MAX_CURVATURE_ERROR + 1e-3), speed) for should_tx, curvature in cases: self._set_prev_desired_angle(sign * small_curvature) - self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, sign * (small_curvature + curvature), 0))) + self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(True, 0, 0, sign * (small_curvature + curvature), 0))) def test_curvature_rate_limit_down(self): self.safety.set_controls_allowed(True) @@ -308,7 +327,7 @@ def test_curvature_rate_limit_down(self): self._reset_curvature_measurement(sign * (self.MAX_CURVATURE - self.MAX_CURVATURE_ERROR - 1e-3), speed) for should_tx, curvature in cases: self._set_prev_desired_angle(sign * self.MAX_CURVATURE) - self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, sign * curvature, 0))) + self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(True, 0, 0, sign * curvature, 0))) def test_prevent_lkas_action(self): self.safety.set_controls_allowed(1) @@ -336,12 +355,12 @@ def test_acc_buttons(self): class TestFordStockSafety(TestFordSafetyBase): + STEER_MESSAGE = MSG_LateralMotionControl + TX_MSGS = [ [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], [MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0], ] - FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_IPMA_Data]} - FWD_BUS_LOOKUP = {0: 2, 2: 0} def setUp(self): self.packer = CANPackerPanda("ford_lincoln_base_pt") @@ -350,13 +369,24 @@ def setUp(self): self.safety.init_tests() -class TestFordLongitudinalSafety(TestFordSafetyBase): +class TestFordCANFDStockSafety(TestFordSafetyBase): + STEER_MESSAGE = MSG_LateralMotionControl2 + TX_MSGS = [ - [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA, 0], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], - [MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0], + [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], + [MSG_LateralMotionControl2, 0], [MSG_IPMA_Data, 0], ] - FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA, MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_IPMA_Data]} - FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.packer = CANPackerPanda("ford_lincoln_base_pt") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_FORD, Panda.FLAG_FORD_CANFD) + self.safety.init_tests() + + +class TestFordLongitudinalSafetyBase(TestFordSafetyBase): + FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA, MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, + MSG_LateralMotionControl2, MSG_IPMA_Data]} MAX_ACCEL = 2.0 # accel is used for brakes, but openpilot can set positive values MIN_ACCEL = -3.5 @@ -366,11 +396,10 @@ class TestFordLongitudinalSafety(TestFordSafetyBase): MIN_GAS = -0.5 INACTIVE_GAS = -5.0 - def setUp(self): - self.packer = CANPackerPanda("ford_lincoln_base_pt") - self.safety = libpanda_py.libpanda - self.safety.set_safety_hooks(Panda.SAFETY_FORD, Panda.FLAG_FORD_LONG_CONTROL) - self.safety.init_tests() + @classmethod + def setUpClass(cls): + if cls.__name__ == "TestFordLongitudinalSafetyBase": + raise unittest.SkipTest # ACC command def _acc_command_msg(self, gas: float, brake: float, cmbb_deny: bool = False): @@ -408,5 +437,35 @@ def test_brake_safety_check(self): self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, brake))) +class TestFordLongitudinalSafety(TestFordLongitudinalSafetyBase): + STEER_MESSAGE = MSG_LateralMotionControl + + TX_MSGS = [ + [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA, 0], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], + [MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0], + ] + + def setUp(self): + self.packer = CANPackerPanda("ford_lincoln_base_pt") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_FORD, Panda.FLAG_FORD_LONG_CONTROL) + self.safety.init_tests() + + +class TestFordCANFDLongitudinalSafety(TestFordLongitudinalSafetyBase): + STEER_MESSAGE = MSG_LateralMotionControl2 + + TX_MSGS = [ + [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA, 0], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], + [MSG_LateralMotionControl2, 0], [MSG_IPMA_Data, 0], + ] + + def setUp(self): + self.packer = CANPackerPanda("ford_lincoln_base_pt") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_FORD, Panda.FLAG_FORD_LONG_CONTROL | Panda.FLAG_FORD_CANFD) + self.safety.init_tests() + + if __name__ == "__main__": unittest.main() From e8da4eab2e5d1ce5219a587e8c5909989d72907b Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 15 Jul 2023 22:53:34 -0700 Subject: [PATCH 4/5] SPI: increase buffer size on H7 --- board/drivers/spi.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/board/drivers/spi.h b/board/drivers/spi.h index 2c85184871..5f1711100f 100644 --- a/board/drivers/spi.h +++ b/board/drivers/spi.h @@ -2,7 +2,6 @@ #include "crc.h" -#define SPI_BUF_SIZE 1024U #define SPI_TIMEOUT_US 10000U // got max rate from hitting a non-existent endpoint @@ -10,9 +9,11 @@ #define SPI_IRQ_RATE 16000U #ifdef STM32H7 +#define SPI_BUF_SIZE 2048U __attribute__((section(".ram_d1"))) uint8_t spi_buf_rx[SPI_BUF_SIZE]; -__attribute__((section(".ram_d1"))) uint8_t spi_buf_tx[SPI_BUF_SIZE]; +__attribute__((section(".ram_d2"))) uint8_t spi_buf_tx[SPI_BUF_SIZE]; #else +#define SPI_BUF_SIZE 1024U uint8_t spi_buf_rx[SPI_BUF_SIZE]; uint8_t spi_buf_tx[SPI_BUF_SIZE]; #endif From 5d873444b2cf801ba73f4a457993260df3a412b8 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 16 Jul 2023 19:33:18 -0700 Subject: [PATCH 5/5] SPI: connect by VERSION command (#1495) * SPI: connect by VERSION command * shorter timeout * add exception * simple test * fallback * bootstub check * update comments --------- Co-authored-by: Comma Device --- __init__.py | 1 + python/__init__.py | 32 ++++++++++++++++++++++++++------ python/spi.py | 8 +++++++- tests/hitl/8_spi.py | 16 ++++++++++++++-- 4 files changed, 48 insertions(+), 9 deletions(-) diff --git a/__init__.py b/__init__.py index 21e35b9e98..19cbee3e4f 100644 --- a/__init__.py +++ b/__init__.py @@ -1,4 +1,5 @@ from .python.constants import McuType, BASEDIR, FW_PATH # noqa: F401 +from .python.spi import PandaSpiException, PandaProtocolMismatch # noqa: F401 from .python.serial import PandaSerial # noqa: F401 from .python import (Panda, PandaDFU, # noqa: F401 pack_can_buffer, unpack_can_buffer, calculate_checksum, unpack_log, diff --git a/python/__init__.py b/python/__init__.py index 26a70ff260..0349d161b7 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -17,7 +17,7 @@ from .constants import FW_PATH, McuType from .dfu import PandaDFU from .isotp import isotp_send, isotp_recv -from .spi import PandaSpiHandle, PandaSpiException +from .spi import PandaSpiHandle, PandaSpiException, PandaProtocolMismatch from .usb import PandaUsbHandle __version__ = '0.0.10' @@ -326,16 +326,30 @@ def connect(self, claim=True, wait=False): self.set_power_save(0) @staticmethod - def spi_connect(serial): + def spi_connect(serial, ignore_version=False): # get UID to confirm slave is present and up handle = None spi_serial = None bootstub = None + spi_version = None try: handle = PandaSpiHandle() - dat = handle.controlRead(Panda.REQUEST_IN, 0xc3, 0, 0, 12, timeout=100) - spi_serial = binascii.hexlify(dat).decode() - bootstub = Panda.flasher_present(handle) + + # connect by protcol version + try: + dat = handle.get_protocol_version() + spi_serial = binascii.hexlify(dat[:12]).decode() + pid = dat[13] + if pid not in (0xcc, 0xee): + raise PandaSpiException("invalid bootstub status") + bootstub = pid == 0xee + spi_version = dat[14] + except PandaSpiException: + # fallback, we'll raise a protocol mismatch below + dat = handle.controlRead(Panda.REQUEST_IN, 0xc3, 0, 0, 12, timeout=100) + spi_serial = binascii.hexlify(dat).decode() + bootstub = Panda.flasher_present(handle) + spi_version = 0 except PandaSpiException: pass @@ -345,6 +359,12 @@ def spi_connect(serial): spi_serial = None bootstub = False + # ensure our protocol version matches the panda + if handle is not None and not ignore_version: + if spi_version != handle.PROTOCOL_VERSION: + err = f"panda protocol mismatch: expected {handle.PROTOCOL_VERSION}, got {spi_version}. reflash panda" + raise PandaProtocolMismatch(err) + return handle, spi_serial, bootstub, None @staticmethod @@ -416,7 +436,7 @@ def usb_list(): @staticmethod def spi_list(): - _, serial, _, _ = Panda.spi_connect(None) + _, serial, _, _ = Panda.spi_connect(None, ignore_version=True) if serial is not None: return [serial, ] return [] diff --git a/python/spi.py b/python/spi.py index b73aea1dba..55bfa6d2e4 100644 --- a/python/spi.py +++ b/python/spi.py @@ -40,6 +40,9 @@ class PandaSpiException(Exception): pass +class PandaProtocolMismatch(PandaSpiException): + pass + class PandaSpiUnavailable(PandaSpiException): pass @@ -109,6 +112,9 @@ class PandaSpiHandle(BaseHandle): """ A class that mimics a libusb1 handle for panda SPI communications. """ + + PROTOCOL_VERSION = 1 + def __init__(self): self.dev = SpiDevice() @@ -225,7 +231,7 @@ def _get_version(spi) -> bytes: version_bytes = spi.readbytes(len(vers_str) + 2) if bytes(version_bytes).startswith(vers_str): break - if (time.monotonic() - start) > 0.5: + if (time.monotonic() - start) > 0.01: raise PandaSpiMissingAck rlen = struct.unpack("