diff --git a/drivers/sensor/icm42688/Kconfig b/drivers/sensor/icm42688/Kconfig index a4ee90efb70c8..bb336e440e9d9 100644 --- a/drivers/sensor/icm42688/Kconfig +++ b/drivers/sensor/icm42688/Kconfig @@ -4,6 +4,8 @@ # # SPDX-License-Identifier: Apache-2.0 +DT_COMPAT_ICM42688 := invensense,icm42688 + config ICM42688 bool "ICM42688 Six-Axis Motion Tracking Device" default y @@ -58,4 +60,10 @@ config ICM42688_THREAD_STACK_SIZE help The thread stack size. +config ICM42688_RTIO + bool "ICM42688 RTIO Support" + depends on RTIO + help + Enable RTIO support for ICM42688 allowing for scatter gather like requests against the fifo. + endif # ICM42688 diff --git a/drivers/sensor/icm42688/icm42688.c b/drivers/sensor/icm42688/icm42688.c index 329907a010d93..0cebfe72ebbf8 100644 --- a/drivers/sensor/icm42688/icm42688.c +++ b/drivers/sensor/icm42688/icm42688.c @@ -8,10 +8,14 @@ #define DT_DRV_COMPAT invensense_icm42688 +#include #include +#include #include #include - +#include +#include +#include #include "icm42688.h" #include "icm42688_reg.h" #include "icm42688_spi.h" @@ -24,8 +28,37 @@ struct icm42688_sensor_data { struct icm42688_dev_data dev_data; int16_t readings[7]; + +#ifdef CONFIG_ICM42688_RTIO + const struct device *dev; + struct rtio *r; + struct rtio_iodev *fifo_iodev; + struct rtio_iodev_sqe *iodev_sqe; + bool checked_out; + uint32_t overflows; + struct gpio_callback gpio_cb; +#ifdef CONFIG_SPI_RTIO + struct rtio_iodev *spi_iodev; + uint8_t int_status; + uint16_t fifo_count; + atomic_t reading_fifo; +#endif /* CONFIG_SPI_RTIO */ +#endif /* CONFIG_ICM42688_RTIO */ + +#if CONFIG_ICM42688_TRIGGER_OWN_THREAD + struct k_thread thread; + + K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_ICM42688_THREAD_STACK_SIZE); +#endif }; + +#if defined(CONFIG_SPI_RTIO) && defined(CONFIG_ICM42688_RTIO) +#define ICM42688_USE_SPI_RTIO 1 +#else +#define ICM42688_USE_SPI_RTIO 0 +#endif + struct icm42688_sensor_config { struct icm42688_dev_cfg dev_cfg; }; @@ -93,21 +126,9 @@ static int icm42688_channel_get(const struct device *dev, enum sensor_channel ch static int icm42688_sample_fetch(const struct device *dev, enum sensor_channel chan) { - uint8_t status; struct icm42688_sensor_data *data = dev->data; - const struct icm42688_sensor_config *cfg = dev->config; - - int res = icm42688_spi_read(&cfg->dev_cfg.spi, REG_INT_STATUS, &status, 1); - - if (res) { - return res; - } - - if (!FIELD_GET(BIT_INT_STATUS_DATA_RDY, status)) { - return -EBUSY; - } - uint8_t readings[14]; + int res; res = icm42688_read_all(dev, readings); @@ -215,6 +236,430 @@ static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan, return res; } +#ifdef CONFIG_ICM42688_RTIO + + +static int icm42688_fifo_iodev(const struct device *dev, struct rtio_iodev **iodev) +{ + struct icm42688_sensor_data *data = dev->data; + + *iodev = data->fifo_iodev; + + return 0; +} + +static int icm42688_fifo_read(const struct device *dev) +{ + /** TODO initiate a fifo read without checking int status */ + + return 0; +} + + +static int icm42688_fifo_start(const struct device *dev) +{ + struct icm42688_dev_data *data = dev->data; + const struct icm42688_dev_cfg *cfg = dev->config; + int res; + + /* Disable gpio interrupt */ + res = gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_DISABLE); + if (res != 0) { + goto out; + } + + /* Enable FIFO on chip */ + struct icm42688_cfg sensor_cfg = data->cfg; + + sensor_cfg.fifo_en = true; + + /* TODO figure out a nice way of configuring these with attr set e.g. + * fifo_odr, fifo_wm (in bytes or records?) attrs? + */ + sensor_cfg.fifo_wm = 1024; /* wm in bytes */ + sensor_cfg.accel_odr = ICM42688_ACCEL_ODR_32000; + sensor_cfg.gyro_odr = ICM42688_GYRO_ODR_32000; + + res = icm42688_safely_configure(dev, &sensor_cfg); + if (res != 0) { + goto out; + } + + /* Enable the gpio interrupt */ + res = gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE); + +out: + return res; +} + +static int icm42688_fifo_stop(const struct device *dev) +{ + struct icm42688_sensor_data *data = dev->data; + const struct icm42688_sensor_config *cfg = dev->config; + int res; + + /* Disable gpio interrupt */ + res = gpio_pin_interrupt_configure_dt(&cfg->dev_cfg.gpio_int1, GPIO_INT_DISABLE); + + + /* Disable FIFO on chip */ + struct icm42688_cfg sensor_cfg = data->dev_data.cfg; + + sensor_cfg.fifo_en = false; + /* TODO figure out a nice way of configuring these with saved attrs e.g. xyz odrs attrs? */ + sensor_cfg.accel_odr = ICM42688_ACCEL_ODR_1000; + sensor_cfg.gyro_odr = ICM42688_GYRO_ODR_1000; + + res = icm42688_safely_configure(dev, &sensor_cfg); + if (res != 0) { + goto out; + } + + /* Clear request queue */ + rtio_iodev_cancel_all(data->fifo_iodev); + + /* TODO Possibly re-enable gpio interrupt if trigger_set has been setup */ + +out: + return 0; +} +/* Define a simple minimum buffer size of 1 fifo packet plus a copy of the config */ +#define ICM42688_MAX_FIFO_PKT_SIZE 20 +#define ICM42688_MIN_BUF_SIZE (ICM42688_MAX_FIFO_PKT_SIZE + sizeof(struct icm42688_cfg)) + +/* Accepts read requests with buffers long enough to store at least a single FIFO packet + * and appends them to a pending request queue. This is then pulled from and read into + * when sensor_fifo_read is called. + */ +void icm42688_fifo_submit(struct rtio_iodev_sqe *iodev_sqe) +{ + const struct rtio_sqe *sqe = iodev_sqe->sqe; + + if (sqe->op != RTIO_OP_RX || sqe->buf_len < ICM42688_MIN_BUF_SIZE) { + rtio_iodev_sqe_err(iodev_sqe, -EINVAL); + return; + } + + rtio_mpsc_push((struct rtio_mpsc *)&iodev_sqe->sqe->iodev->iodev_sq, &iodev_sqe->q); +} + + +static const struct rtio_iodev_api icm42688_fifo_iodev_api = { + .submit = icm42688_fifo_submit +}; + +struct fifo_header { + uint8_t int_status; + uint16_t gyro_odr: 4; + uint16_t accel_odr: 4; + uint16_t gyro_fs: 3; + uint16_t accel_fs: 3; + uint16_t packet_format: 2; +} __attribute__((__packed__)); + +BUILD_ASSERT(sizeof(struct fifo_header) == 3); + + +#if CONFIG_SPI_RTIO + +static void icm42688_complete_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg) +{ + const struct device *dev = arg; + struct icm42688_sensor_data *drv_data = dev->data; + const struct icm42688_sensor_config *drv_cfg = dev->config; + + /* TODO report number of frames or bytes read here */ + rtio_iodev_sqe_ok(drv_data->iodev_sqe, drv_data->fifo_count); + drv_data->iodev_sqe = NULL; + + gpio_pin_interrupt_configure_dt(&drv_cfg->dev_cfg.gpio_int1, GPIO_INT_EDGE_TO_ACTIVE); +} + +static void icm42688_fifo_count_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg) +{ + const struct device *dev = arg; + struct icm42688_sensor_data *drv_data = dev->data; + const struct icm42688_sensor_config *drv_cfg = dev->config; + uint8_t *fifo_count_buf = (uint8_t *)&drv_data->fifo_count; + uint16_t fifo_count = ((fifo_count_buf[0] << 8) | fifo_count_buf[1]); + + drv_data->fifo_count = fifo_count; + + /* Pull a operation from our device iodev queue, validated to only be reads */ + struct rtio_mpsc_node *next = rtio_mpsc_pop(&drv_data->fifo_iodev->iodev_sq); + + /* Not inherently an underrun/overrun as we may have a buffer to fill next time */ + if (next == NULL) { + gpio_pin_interrupt_configure_dt(&drv_cfg->dev_cfg.gpio_int1, + GPIO_INT_EDGE_TO_ACTIVE); + return; + } + + struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(next, struct rtio_iodev_sqe, q); + + drv_data->iodev_sqe = iodev_sqe; + + const size_t packet_size = drv_data->dev_data.cfg.fifo_hires ? 20 : 16; + + /* Check buffer is big enough for configuration + * and at least 1 packet + * + * TODO determine len to be aligned down to the nearest packet + */ + if (iodev_sqe->sqe->buf_len < (sizeof(struct fifo_header) + packet_size)) { + LOG_WRN("Buffer minimum size not met"); + rtio_iodev_sqe_err(iodev_sqe, -ENOMEM); + return; + } + + /* Read FIFO and call back to rtio with rtio_sqe completion */ + /* TODO is packet format even needed? the fifo has a header per packet + * already + */ + struct fifo_header hdr = { + .int_status = drv_data->int_status, + .gyro_odr = drv_data->dev_data.cfg.gyro_odr, + .gyro_fs = drv_data->dev_data.cfg.gyro_fs, + .accel_odr = drv_data->dev_data.cfg.accel_odr, + .accel_fs = drv_data->dev_data.cfg.accel_fs, + .packet_format = 0, + }; + + uint32_t buf_avail = iodev_sqe->sqe->buf_len; + + memcpy(iodev_sqe->sqe->buf, &hdr, sizeof(hdr)); + buf_avail -= sizeof(hdr); + + /* TODO account for other packet sizes */ + const uint32_t pkt_size = 16; + + uint32_t read_len = fifo_count > buf_avail ? buf_avail : fifo_count; + uint32_t pkts = read_len / pkt_size; + + read_len = pkts*pkt_size; + + __ASSERT_NO_MSG(read_len % pkt_size == 0); + + uint8_t *read_buf = &iodev_sqe->sqe->buf[sizeof(hdr)]; + + /* Flush out completions */ + struct rtio_cqe *cqe = rtio_cqe_consume(drv_data->r); + + while (cqe != NULL) { + cqe = rtio_cqe_consume(drv_data->r); + } + rtio_cqe_release_all(r); + + /* Setup new rtio chain to read the fifo data and report then check the + * result + */ + struct rtio_sqe *write_fifo_addr = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *read_fifo_data = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *complete_op = rtio_sqe_acquire(drv_data->r); + const uint8_t reg_addr = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_FIFO_DATA); + + rtio_sqe_prep_tiny_write(write_fifo_addr, drv_data->spi_iodev, + RTIO_PRIO_NORM, ®_addr, 1, NULL); + write_fifo_addr->flags = RTIO_SQE_TRANSACTION; + rtio_sqe_prep_read(read_fifo_data, drv_data->spi_iodev, RTIO_PRIO_NORM, + read_buf, read_len, iodev_sqe); + /* TODO add flush op here, works without because we are using simple executor */ + rtio_sqe_prep_callback(complete_op, icm42688_complete_cb, + (void *)dev, NULL); + + rtio_submit(drv_data->r, 0); +} + +static void icm42688_int_status_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg) +{ + const struct device *dev = arg; + struct icm42688_sensor_data *drv_data = dev->data; + const struct icm42688_sensor_config *drv_cfg = dev->config; + + if (!(drv_data->int_status & BIT_INT_STATUS_FIFO_THS + || drv_data->int_status & BIT_INT_STATUS_FIFO_FULL)) { + gpio_pin_interrupt_configure_dt(&drv_cfg->dev_cfg.gpio_int1, + GPIO_INT_EDGE_TO_ACTIVE); + return; + } + + /* In effect a overrun, as the sensor is producing faster than we are consuming */ + if (drv_data->int_status & BIT_INT_STATUS_FIFO_FULL) { + drv_data->overflows++; + } + + /* Flush out completions */ + struct rtio_cqe *cqe = rtio_cqe_consume(drv_data->r); + + while (cqe != NULL) { + cqe = rtio_cqe_consume(drv_data->r); + } + rtio_cqe_release_all(r); + + struct rtio_sqe *write_fifo_count_reg = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *read_fifo_count = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *check_fifo_count = rtio_sqe_acquire(drv_data->r); + uint8_t reg = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_FIFO_COUNTH); + uint8_t *read_buf = (uint8_t *)&drv_data->fifo_count; + + rtio_sqe_prep_tiny_write(write_fifo_count_reg, drv_data->spi_iodev, RTIO_PRIO_NORM, + ®, 1, NULL); + write_fifo_count_reg->flags = RTIO_SQE_TRANSACTION; + rtio_sqe_prep_read(read_fifo_count, drv_data->spi_iodev, RTIO_PRIO_NORM, + read_buf, 2, NULL); + /* TODO add flush op here, works without because we are using simple executor */ + rtio_sqe_prep_callback(check_fifo_count, icm42688_fifo_count_cb, arg, NULL); + + rtio_submit(drv_data->r, 0); +} + +void icm42688_rtio_fifo_event(const struct device *dev) +{ + struct icm42688_sensor_data *drv_data = dev->data; + + /* + * Setup rtio chain of ops with inline calls to make decisions + * 1. read int status + * 2. call to check int status and get pending RX operation + * 4. read fifo len + * 5. call to determine read len + * 6. read fifo + * 7. call to report completion + */ + struct rtio_sqe *write_int_reg = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *read_int_reg = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *check_int_status = rtio_sqe_acquire(drv_data->r); + uint8_t reg = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_INT_STATUS); + + rtio_sqe_prep_tiny_write(write_int_reg, drv_data->spi_iodev, RTIO_PRIO_NORM, ®, 1, NULL); + write_int_reg->flags = RTIO_SQE_TRANSACTION; + rtio_sqe_prep_read(read_int_reg, drv_data->spi_iodev, RTIO_PRIO_NORM, + &drv_data->int_status, 1, NULL); + /* TODO add flush op here, works without because we are using simple executor */ + rtio_sqe_prep_callback(check_int_status, icm42688_int_status_cb, (void *)dev, NULL); + + rtio_submit(drv_data->r, 0); +} + +#else + +void icm42688_rtio_fifo_event(const struct device *dev) +{ + struct icm42688_sensor_data *data = dev->data; + const struct icm42688_sensor_config *cfg = dev->config; + + int res = 0; + uint16_t fifo_count, fifo_lost; + uint8_t int_status; + uint8_t read_buf[2]; + + res = icm42688_spi_read(&cfg->dev_cfg.spi, REG_INT_STATUS, &int_status, 1); + if (res) { + goto out; + } + + if (data->dev_data.cfg.fifo_en != true) { + goto out; + } + + if (!(int_status & BIT_INT_STATUS_FIFO_THS || int_status & BIT_INT_STATUS_FIFO_FULL)) { + goto out; + } + + /* In effect a overrun, as the sensor is producing faster than we are consuming */ + if (int_status & BIT_INT_STATUS_FIFO_FULL) { + data->overflows++; + } + + res = icm42688_spi_read(&cfg->dev_cfg.spi, REG_FIFO_COUNTH, read_buf, 2); + + if (res) { + goto out; + } + fifo_count = ((read_buf[0] << 8) | read_buf[1]); + + if (fifo_count < data->dev_data.cfg.fifo_wm) { + goto out; + } + + /* Get a buffer to read into, if one exists */ + struct rtio_mpsc_node *next = rtio_mpsc_pop(&data->fifo_iodev->iodev_sq); + + /* Not inherently an underrun/overrun as we may have a buffer to fill + * next time + */ + if (next == NULL) { + /* In effect we want to punish this thread (ugh) for now so + * others may run. Without this its very possible all time is + * spent jumping from the interrupt to this thread not allowing + * other threads to run (like the one adding the buffers to + * read into) + */ + k_msleep(1); + goto out; + } + + struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(next, struct rtio_iodev_sqe, q); + + /* Check buffer is big enough for configuration + * and at least 1 packet + * + * TODO determine len to be aligned down to the nearest packet + */ + if (iodev_sqe->sqe->buf_len < (sizeof(struct fifo_header) + 20)) { + LOG_WRN("Buffer minimum size not met"); + rtio_iodev_sqe_err(iodev_sqe, -ENOMEM); + goto out; + } + + /* Read FIFO and call back to rtio with rtio_sqe completion */ + /* TODO is packet format even needed? the fifo has a header per packet + * already + */ + struct fifo_header hdr = { + .int_status = int_status, + .gyro_odr = data->dev_data.cfg.gyro_odr, + .gyro_fs = data->dev_data.cfg.gyro_fs, + .accel_odr = data->dev_data.cfg.accel_odr, + .accel_fs = data->dev_data.cfg.accel_fs, + .packet_format = 0, + }; + + uint32_t buf_avail = iodev_sqe->sqe->buf_len; + + memcpy(iodev_sqe->sqe->buf, &hdr, sizeof(hdr)); + buf_avail -= sizeof(hdr); + + /* TODO account for other packet sizes */ + const uint32_t pkt_size = 16; + uint32_t read_len = fifo_count > buf_avail ? buf_avail : fifo_count; + uint32_t pkts = read_len / pkt_size; + + read_len = pkts*pkt_size; + + __ASSERT_NO_MSG(read_len % pkt_size == 0); + + uint8_t *read_addr = &iodev_sqe->sqe->buf[sizeof(hdr)]; + + res = icm42688_spi_read(&cfg->dev_cfg.spi, REG_FIFO_DATA, read_addr, + read_len); + + if (res != 0) { + LOG_ERR("Error reading fifo"); + rtio_iodev_sqe_err(iodev_sqe, -EIO); + goto out; + } + + rtio_iodev_sqe_ok(iodev_sqe, read_len + sizeof(hdr)); + +out: + return; +} + +#endif /* CONFIG_SPI_RTIO */ +#endif /* CONFIG_ICM42688_RTIO */ + + static const struct sensor_driver_api icm42688_driver_api = { .sample_fetch = icm42688_sample_fetch, .channel_get = icm42688_channel_get, @@ -222,7 +667,13 @@ static const struct sensor_driver_api icm42688_driver_api = { .attr_get = icm42688_attr_get, #ifdef CONFIG_ICM42688_TRIGGER .trigger_set = icm42688_trigger_set, -#endif +#endif /* CONFIG_ICM42688_TRIGGER */ +#ifdef CONFIG_ICM42688_RTIO + .fifo_iodev = icm42688_fifo_iodev, + .fifo_read = icm42688_fifo_read, + .fifo_start = icm42688_fifo_start, + .fifo_stop = icm42688_fifo_stop, +#endif /* CONFIG_ICM42688_RTIO */ }; int icm42688_init(const struct device *dev) @@ -268,7 +719,7 @@ int icm42688_init(const struct device *dev) return res; } - return 0; + return res; } #ifndef CONFIG_ICM42688_TRIGGER @@ -280,26 +731,48 @@ void icm42688_unlock(const struct device *dev) { ARG_UNUSED(dev); } -#endif +#endif /* CONFIG_ICM42688_TRIGGER */ /* device defaults to spi mode 0/3 support */ -#define ICM42688_SPI_CFG \ +#define ICM42688_SPI_CFG \ SPI_OP_MODE_MASTER | SPI_MODE_CPOL | SPI_MODE_CPHA | SPI_WORD_SET(8) | SPI_TRANSFER_MSB +#define ICM42688_RTIO_DEFINE(inst) \ + IF_ENABLED(CONFIG_SPI_RTIO, \ + (SPI_DT_IODEV_DEFINE(icm42688_spi_iodev_##inst, \ + DT_DRV_INST(inst), ICM42688_SPI_CFG, 0U));) \ + RTIO_EXECUTOR_SIMPLE_DEFINE(icm42688_rtio_exec_##inst); \ + RTIO_DEFINE(icm42688_rtio_##inst, \ + (struct rtio_executor *)&icm42688_rtio_exec_##inst, 8, 4); \ + RTIO_IODEV_DEFINE(icm42688_fifo_iodev_##inst, &icm42688_fifo_iodev_api, NULL); + +#define ICM42688_DEFINE_CONFIG(inst) \ + static const struct icm42688_sensor_config icm42688_cfg_##inst = { \ + .dev_cfg = \ + { \ + .spi = SPI_DT_SPEC_INST_GET(inst, ICM42688_SPI_CFG, 0U), \ + .gpio_int1 = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \ + }, \ + } + +#define ICM42688_DEFINE_DATA(inst) \ + IF_ENABLED(CONFIG_ICM42688_RTIO, (ICM42688_RTIO_DEFINE(inst))); \ + static struct icm42688_sensor_data icm42688_driver_##inst = { \ + IF_ENABLED(CONFIG_ICM42688_RTIO, ( \ + .r = &icm42688_rtio_##inst, \ + .fifo_iodev = &icm42688_fifo_iodev_##inst, \ + )) \ + IF_ENABLED(ICM42688_USE_SPI_RTIO, ( \ + .spi_iodev = &icm42688_spi_iodev_##inst, \ + )) \ + } -#define ICM42688_INIT(inst) \ - static struct icm42688_sensor_data icm42688_driver_##inst = { 0 }; \ - \ - static const struct icm42688_sensor_config icm42688_cfg_##inst = { \ - .dev_cfg = \ - { \ - .spi = SPI_DT_SPEC_INST_GET(inst, ICM42688_SPI_CFG, 0U), \ - .gpio_int1 = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \ - }, \ - }; \ - \ - SENSOR_DEVICE_DT_INST_DEFINE(inst, icm42688_init, NULL, &icm42688_driver_##inst, \ - &icm42688_cfg_##inst, POST_KERNEL, \ +#define ICM42688_INIT(inst) \ + ICM42688_DEFINE_CONFIG(inst); \ + ICM42688_DEFINE_DATA(inst); \ + \ + SENSOR_DEVICE_DT_INST_DEFINE(inst, icm42688_init, NULL, &icm42688_driver_##inst, \ + &icm42688_cfg_##inst, POST_KERNEL, \ CONFIG_SENSOR_INIT_PRIORITY, &icm42688_driver_api); DT_INST_FOREACH_STATUS_OKAY(ICM42688_INIT) diff --git a/drivers/sensor/icm42688/icm42688_trigger.c b/drivers/sensor/icm42688/icm42688_trigger.c index 01e555a3f82f0..8c20deb323ec0 100644 --- a/drivers/sensor/icm42688/icm42688_trigger.c +++ b/drivers/sensor/icm42688/icm42688_trigger.c @@ -17,18 +17,32 @@ LOG_MODULE_DECLARE(ICM42688, CONFIG_SENSOR_LOG_LEVEL); + +void icm42688_rtio_fifo_event(const struct device *dev); + static void icm42688_gpio_callback(const struct device *dev, struct gpio_callback *cb, uint32_t pins) { struct icm42688_dev_data *data = CONTAINER_OF(cb, struct icm42688_dev_data, gpio_cb); + const struct icm42688_dev_cfg *cfg = data->dev->config; ARG_UNUSED(dev); ARG_UNUSED(pins); + gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_DISABLE); + +#if defined(CONFIG_SPI_RTIO) && defined(CONFIG_ICM42688_RTIO) + if (data->cfg.fifo_en) { + icm42688_rtio_fifo_event(data->dev); + } else { +#endif #if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD) - k_sem_give(&data->gpio_sem); + k_sem_give(&data->gpio_sem); #elif defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD) - k_work_submit(&data->work); + k_work_submit(&data->work); +#endif +#if defined(CONFIG_SPI_RTIO) && defined(CONFIG_ICM42688_RTIO) + } #endif } @@ -39,7 +53,9 @@ static void icm42688_thread_cb(const struct device *dev) icm42688_lock(dev); - if (data->data_ready_handler != NULL) { + if (data->cfg.fifo_en && IS_ENABLED(CONFIG_ICM42688_RTIO) && !IS_ENABLED(CONFIG_SPI_RTIO)) { + icm42688_rtio_fifo_event(dev); + } else if (data->data_ready_handler != NULL) { data->data_ready_handler(dev, data->data_ready_trigger); } else { uint8_t status; @@ -47,6 +63,7 @@ static void icm42688_thread_cb(const struct device *dev) icm42688_spi_read(&cfg->spi, REG_INT_STATUS, &status, 1); } + gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE); icm42688_unlock(dev); } diff --git a/include/zephyr/drivers/sensor.h b/include/zephyr/drivers/sensor.h index e7f5ceba5bfe9..f6101797ef2fc 100644 --- a/include/zephyr/drivers/sensor.h +++ b/include/zephyr/drivers/sensor.h @@ -21,6 +21,7 @@ #include #include +#include #include #ifdef __cplusplus @@ -318,6 +319,10 @@ enum sensor_attribute { * to the new sampling frequency. */ SENSOR_ATTR_FF_DUR, + /** FIFO watermark attribute */ + SENSOR_ATTR_FIFO_WATERMARK, + /** FIFO enable/disable fifo related interrupts */ + SENSOR_ATTR_FIFO_INTERRUPT, /** * Number of all common sensor attributes. */ @@ -394,12 +399,48 @@ typedef int (*sensor_channel_get_t)(const struct device *dev, enum sensor_channel chan, struct sensor_value *val); +#ifdef CONFIG_RTIO + +/** + * @type sensor_fifo_iodev_t + * @brief Callback API for obtaining a reference to a sensors FIFO iodev. + */ +typedef int (*sensor_fifo_iodev_t)(const struct device *dev, struct rtio_iodev **iodev_sqe); + +/** + * @type sensor_fifo_read_t + * @brief Callback API for immediately triggering a FIFO read. + */ +typedef int (*sensor_fifo_read_t)(const struct device *dev); + +/** + * @type sensor_fifo_start_t + * @brief Callback API to start FIFO streaming. + */ +typedef int (*sensor_fifo_start_t)(const struct device *dev); + +/** + * @type sensor_fifo_stop_t + * @brief Callback API to stop FIFO streaming. + */ +typedef int (*sensor_fifo_stop_t)(const struct device *dev); + + +#endif /* CONFIG_RTIO */ + __subsystem struct sensor_driver_api { sensor_attr_set_t attr_set; sensor_attr_get_t attr_get; sensor_trigger_set_t trigger_set; sensor_sample_fetch_t sample_fetch; sensor_channel_get_t channel_get; + +#ifdef CONFIG_RTIO + sensor_fifo_iodev_t fifo_iodev; + sensor_fifo_read_t fifo_read; + sensor_fifo_start_t fifo_start; + sensor_fifo_stop_t fifo_stop; +#endif /* CONFIG_RTIO */ }; /** @@ -598,6 +639,70 @@ static inline int z_impl_sensor_channel_get(const struct device *dev, return api->channel_get(dev, chan, val); } +/** + * @brief Submit a request for the fifo stream (iodev) for this sensor. + */ +static inline int sensor_fifo_iodev(const struct device *dev, struct rtio_iodev **iodev) +{ +#ifdef CONFIG_RTIO + + const struct sensor_driver_api *api = + (const struct sensor_driver_api *)dev->api; + + return api->fifo_iodev(dev, iodev); +#else + return -ENOSYS; +#endif +} + +/** + * @brief Checkin a rtio_iodev pointer for this sensor, only one per device. + */ +static inline int sensor_fifo_read(const struct device *dev) +{ +#ifdef CONFIG_RTIO + + const struct sensor_driver_api *api = + (const struct sensor_driver_api *)dev->api; + + return api->fifo_read(dev); +#else + return -ENOSYS; +#endif +} + +/** + * @brief Start streaming the fifo for this sensor. + */ +static inline int sensor_fifo_start(const struct device *dev) +{ +#ifdef CONFIG_RTIO + + const struct sensor_driver_api *api = + (const struct sensor_driver_api *)dev->api; + + return api->fifo_start(dev); +#else + return -ENOSYS; +#endif +} + +/** + * @brief Stop streaming the fifo for this sensor. + */ +static inline int sensor_fifo_stop(const struct device *dev) +{ +#ifdef CONFIG_RTIO + + const struct sensor_driver_api *api = + (const struct sensor_driver_api *)dev->api; + + return api->fifo_stop(dev); +#else + return -ENOSYS; +#endif +} + /** * @brief The value of gravitational constant in micro m/s^2. */ diff --git a/samples/boards/tdk_robokit1/CMakeLists.txt b/samples/boards/tdk_robokit1/CMakeLists.txt new file mode 100644 index 0000000000000..a4b5a2fcf7550 --- /dev/null +++ b/samples/boards/tdk_robokit1/CMakeLists.txt @@ -0,0 +1,11 @@ +# Copyright (c) 2022 Intel Corporation +# +# SPDX-License-Identifier: Apache-2.0 +# +cmake_minimum_required(VERSION 3.20.0) + +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(tdk_robokit1) + +target_include_directories(app PRIVATE $ENV{ZEPHYR_BASE}/drivers/sensor/icm42688) +target_sources(app PRIVATE src/main.c) diff --git a/samples/boards/tdk_robokit1/README.rst b/samples/boards/tdk_robokit1/README.rst new file mode 100644 index 0000000000000..1af271d0cdb9f --- /dev/null +++ b/samples/boards/tdk_robokit1/README.rst @@ -0,0 +1,22 @@ +.. _tdk_robokit1 sensors: + +TDK Robokit1 +############ + +Overview +******** + +This sample provides an example of how to read sensors data +from the TDK RoboKit1 board using polling and RTIO. + +This sample enables all sensors of TDK RoboKit1 board, and then +periodically reads and displays data on the console from the following +sensors: + +- ICM42688: 6-Axis acceleration and angular velocity + +Requirements +************ + +The application requires a TDK RoboKit1 board connected to the PC +through USB. diff --git a/samples/boards/tdk_robokit1/prj.conf b/samples/boards/tdk_robokit1/prj.conf new file mode 100644 index 0000000000000..49a9bcdae9333 --- /dev/null +++ b/samples/boards/tdk_robokit1/prj.conf @@ -0,0 +1,46 @@ +CONFIG_ARM_MPU=y # Seemingly not working +CONFIG_FPU=n +CONFIG_CBPRINTF_FP_SUPPORT=n +CONFIG_ASSERT=y +CONFIG_STACK_SENTINEL=y +CONFIG_ENTROPY_GENERATOR=y +CONFIG_STACK_CANARIES=y +CONFIG_LOG=y # causes mpu fault? +CONFIG_LOG_PRINTK=y +CONFIG_LOG_MODE_DEFERRED=y +CONFIG_SPI=y +CONFIG_I2C=y +CONFIG_GPIO=y +CONFIG_THREAD_NAME=y +CONFIG_DEBUG_THREAD_INFO=y + +CONFIG_LOG_PROCESS_THREAD_STACK_SIZE=8192 +CONFIG_MAIN_STACK_SIZE=2048 +CONFIG_ISR_STACK_SIZE=2048 + +# config sensors +CONFIG_DMA=y +CONFIG_DMA_LOG_LEVEL_INF=y +CONFIG_SENSOR=y +CONFIG_SENSOR_LOG_LEVEL_DBG=y +CONFIG_SPI_LOG_LEVEL_DBG=y +CONFIG_ICM42688_RTIO=y +CONFIG_RTIO=y +CONFIG_SPI_SAM_DMA=y +CONFIG_RTIO_SUBMIT_SEM=n +CONFIG_RTIO_CONSUME_SEM=y + + +# Tracing +CONFIG_TRACING=y +CONFIG_TRACING_TIMER=y +CONFIG_TRACING_SYSCALL=n + +# SystemView +#CONFIG_SEGGER_SYSTEMVIEW=y +#CONFIG_SEGGER_SYSVIEW_RTT_BUFFER_SIZE=65536 + +# Threadalyzer Tracing + +# Percepio +CONFIG_PERCEPIO_TRACERECORDER=y diff --git a/samples/boards/tdk_robokit1/sample.yaml b/samples/boards/tdk_robokit1/sample.yaml new file mode 100644 index 0000000000000..f0886b70705d5 --- /dev/null +++ b/samples/boards/tdk_robokit1/sample.yaml @@ -0,0 +1,8 @@ +sample: + description: TDK RoboKit1 Test App for Sensors + name: tdk_robokit1 + +tests: + sample.boards.tdk_robokit1: + platform_allow: tdk_robokit1 + tags: rtio sensor diff --git a/samples/boards/tdk_robokit1/src/main.c b/samples/boards/tdk_robokit1/src/main.c new file mode 100644 index 0000000000000..ecdb794e38c64 --- /dev/null +++ b/samples/boards/tdk_robokit1/src/main.c @@ -0,0 +1,228 @@ +/* + * Copyright (c) 2022 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include + +#include +#include + +#include +LOG_MODULE_REGISTER(TDK_ROBOKIT1, CONFIG_SENSOR_LOG_LEVEL); + +/* Power of 2 number of stream buffers */ +#define N_BUFS 4 + +RTIO_EXECUTOR_SIMPLE_DEFINE(r_exec); + +/* Show no over/under flows with a 4 buffer queue */ +RTIO_DEFINE(r4, (struct rtio_executor *)&r_exec, N_BUFS, N_BUFS); + +/* Show over/underflows with a single buffer queue */ +RTIO_DEFINE(r1, (struct rtio_executor *)&r_exec, 1, 1); + +#define FIFO_ITERS 4096 + +struct fifo_header { + uint8_t int_status; + uint16_t gyro_odr: 4; + uint16_t accel_odr: 4; + uint16_t gyro_fs: 3; + uint16_t accel_fs: 3; + uint16_t packet_format: 2; +} __packed; + +/* The fifo size (2048) bytes plus 3 for the header */ +#define ICM42688_FIFO_BUF_LEN 2051 +static uint8_t icm42688_fifo_bufs[N_BUFS][ICM42688_FIFO_BUF_LEN]; + +const struct device *icm42688 = DEVICE_DT_GET_ONE(invensense_icm42688); +const struct device *akm09918 = DEVICE_DT_GET_ONE(asahi_kasei_akm09918c); + +void poll_imu(void) +{ + uint32_t cycle_start, cycle_end; + struct sensor_value accel[3]; + struct sensor_value gyro[3]; + struct sensor_value imu_temp; + + /* Fetch everything */ + cycle_start = k_cycle_get_32(); + sensor_sample_fetch_chan(icm42688, SENSOR_CHAN_ALL); + cycle_end = k_cycle_get_32(); + + sensor_channel_get(icm42688, SENSOR_CHAN_ACCEL_XYZ, accel); + sensor_channel_get(icm42688, SENSOR_CHAN_GYRO_XYZ, gyro); + sensor_channel_get(icm42688, SENSOR_CHAN_DIE_TEMP, &imu_temp); + + LOG_INF("ICM42688: Fetch took %u cycles", cycle_end - cycle_start); + LOG_INF("ICM42688: Accel (m/s^2): x: %d.%06d, y: %d.%06d, z: %d.%06d", + accel[0].val1, accel[0].val2, + accel[1].val1, accel[1].val2, + accel[2].val1, accel[2].val2); + LOG_INF("ICM42688: Gyro (rad/s): x: %d.%06d, y: %d.%06d, z: %d.%06d", + gyro[0].val1, gyro[0].val2, + gyro[1].val1, gyro[1].val2, + gyro[2].val1, gyro[2].val2); + LOG_INF("ICM42688: Temp (C): %d.%06d", + imu_temp.val1, imu_temp.val2); +} + +void poll_mag(void) +{ + uint32_t cycle_start, cycle_end; + struct sensor_value magn[3]; + + /* Fetch everything */ + cycle_start = k_cycle_get_32(); + sensor_sample_fetch_chan(akm09918, SENSOR_CHAN_ALL); + cycle_end = k_cycle_get_32(); + + sensor_channel_get(akm09918, SENSOR_CHAN_MAGN_XYZ, magn); + + LOG_INF("AKM09918: Fetch took %u cycles", cycle_end - cycle_start); + LOG_INF("AKM09918: Mag (Gauss): x: %d.%06d, y: %d.%06d, z: %d.%06d", + magn[0].val1, magn[0].val2, + magn[1].val1, magn[1].val2, + magn[2].val1, magn[2].val2); +} + +void fifo_stream(struct rtio *r, uint32_t n_bufs) +{ + struct rtio_iodev *iodev; + struct rtio_sqe *sqe; + + LOG_INF("FIFO with RTIO context %p, context size %u", r, sizeof(*r)); + + /* obtain reference to the stream */ + (void)sensor_fifo_iodev(icm42688, &iodev); + + LOG_INF("Setting up RX requests"); + + __ASSERT_NO_MSG(iodev != NULL); + + /* Feed initial read requests */ + for (int i = 0; i < n_bufs; i++) { + sqe = rtio_spsc_acquire(r->sq); + rtio_sqe_prep_read(sqe, iodev, 0, &icm42688_fifo_bufs[i][0], ICM42688_FIFO_BUF_LEN, + (void *)(uintptr_t)i); + rtio_spsc_produce(r->sq); + } + + /* Submits requests */ + rtio_submit(r, 0); + + /* Setup requests, starting stream */ + LOG_INF("Polling RX Requests"); + + /* Enable the fifo automatic triggering (via gpio) */ + sensor_fifo_start(icm42688); + + uint32_t overflows = 0; + + /* Now poll for ready fifo buffers for a little bit, at 32KHz sampling + * and a 1024 buffer read with 16 bytes a sample, the fifo holds + * 64 samples. At 32KHz it triggers every 2ms! this means + * Every buffer has ~2ms to process or we lose the next sample. + * + * Every 8th buffer we busy wait just over 2ms to show how no data + * is lost even with some variable latency involved. This could + * come from data processing or attempting to transport the data + * to another device. + * + * From the sensor a lack of buffer to read into is an underflow + * and from the application perspective being unable to keep up + * is an overflow. + */ + for (int i = 0; i < FIFO_ITERS; i++) { + struct rtio_cqe *cqe = rtio_cqe_consume_block(r); + int32_t result = cqe->result; + uintptr_t buf_idx = (uintptr_t)cqe->userdata; + uint8_t *buf = &icm42688_fifo_bufs[buf_idx][0]; + + rtio_spsc_release(r->cq); + + /* The first byte is the interrupt status and can be + * checked for a FIFO FULL signifying an overflow + */ + struct fifo_header *hdr = (void *)&buf[0]; + + if (hdr->int_status & BIT(1)) { + overflows++; + } + + + if (i % 64 == 0) { + LOG_INF("Poll Mag: Iteration %d, Underflows (sensor overflows) %u, Buf " + "%lu, int status %x, result %d\n", + i, overflows, buf_idx, buf[0], result); + poll_mag(); + k_busy_wait(1000); + } + + /* Now to recycle the buffer by putting it back in the queue */ + struct rtio_sqe *sqe = rtio_spsc_acquire(r->sq); + + __ASSERT_NO_MSG(sqe != NULL); + rtio_sqe_prep_read(sqe, iodev, 0, &icm42688_fifo_bufs[buf_idx][0], + ICM42688_FIFO_BUF_LEN, (void *)(uintptr_t)buf_idx); + rtio_spsc_produce(r->sq); + + rtio_submit(r, 0); + + } + + LOG_INF("Checking In, Sensor Overflows %u", overflows); + + /* All done streaming */ + sensor_fifo_stop(icm42688); + + LOG_INF("DONE! FIFO should be DISABLED"); +} + + +void main(void) +{ + LOG_INF("TDK RoboKit1 Sample"); + + if (!device_is_ready(icm42688)) { + LOG_INF("%s: device not ready.", icm42688->name); + return; + } + + if (!device_is_ready(akm09918)) { + LOG_INF("%s: device not ready.", akm09918->name); + return; + } + + struct sensor_value sample_freq = { .val1 = 10, .val2 = 0 }; + + sensor_attr_set(akm09918, SENSOR_CHAN_MAGN_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, + &sample_freq); + + LOG_INF("Fetch + Read"); + + /* A few polling readings */ + for (int i = 0; i < 10; i++) { + poll_imu(); + poll_mag(); + k_sleep(K_MSEC(100)); + } + + LOG_INF("Showing under/over flows with 1 buffer queue"); + fifo_stream(&r1, 1); + + LOG_INF("Showing no under/over flows with 4 buffer queue"); + fifo_stream(&r4, 4); + + LOG_INF("Done!"); + + while (true) { + k_msleep(1000); + } +}