From f707e7a18750ffb610c6fd7299b070c82174fa55 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Wed, 5 Jul 2023 13:42:04 -0600 Subject: [PATCH] icm42688: Implement streaming APIs Add streaming implementation for icm42688 using both threshold and full FIFO triggers. Signed-off-by: Yuval Peress --- drivers/sensor/icm42688/CMakeLists.txt | 1 + drivers/sensor/icm42688/Kconfig | 10 + drivers/sensor/icm42688/icm42688.c | 24 +- drivers/sensor/icm42688/icm42688.h | 12 + drivers/sensor/icm42688/icm42688_common.c | 9 +- drivers/sensor/icm42688/icm42688_decoder.c | 157 +++++++++- drivers/sensor/icm42688/icm42688_decoder.h | 9 + drivers/sensor/icm42688/icm42688_reg.h | 8 + drivers/sensor/icm42688/icm42688_rtio.c | 315 ++++++++++++++++++++- drivers/sensor/icm42688/icm42688_rtio.h | 2 + drivers/sensor/icm42688/icm42688_trigger.c | 24 +- drivers/sensor/icm42688/icm42688_trigger.h | 3 +- 12 files changed, 562 insertions(+), 12 deletions(-) diff --git a/drivers/sensor/icm42688/CMakeLists.txt b/drivers/sensor/icm42688/CMakeLists.txt index cf9308bb95c4478..4bc7db3b6173732 100644 --- a/drivers/sensor/icm42688/CMakeLists.txt +++ b/drivers/sensor/icm42688/CMakeLists.txt @@ -10,6 +10,7 @@ zephyr_library_sources( zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API icm42688_rtio.c) zephyr_library_sources_ifdef(CONFIG_ICM42688_DECODER icm42688_decoder.c) +zephyr_library_sources_ifdef(CONFIG_ICM42688_STREAM icm42688_rtio.c) zephyr_library_sources_ifdef(CONFIG_ICM42688_TRIGGER icm42688_trigger.c) zephyr_library_sources_ifdef(CONFIG_EMUL_ICM42688 icm42688_emul.c) zephyr_include_directories_ifdef(CONFIG_EMUL_ICM42688 .) diff --git a/drivers/sensor/icm42688/Kconfig b/drivers/sensor/icm42688/Kconfig index 2944a9d1183ad41..d7ee43bc7618bff 100644 --- a/drivers/sensor/icm42688/Kconfig +++ b/drivers/sensor/icm42688/Kconfig @@ -33,6 +33,7 @@ if ICM42688 choice prompt "Trigger mode" + default ICM42688_TRIGGER_NONE if ICM42688_STREAM default ICM42688_TRIGGER_GLOBAL_THREAD help Specify the type of triggering to be used by the driver @@ -50,6 +51,15 @@ config ICM42688_TRIGGER_OWN_THREAD endchoice +config ICM42688_STREAM + bool "Use hardware FIFO to stream data" + select ICM42688_TRIGGER + default y + depends on SPI_RTIO + depends on SENSOR_ASYNC_API + help + Use this config option to enable streaming sensor data via RTIO subsystem. + config ICM42688_TRIGGER bool diff --git a/drivers/sensor/icm42688/icm42688.c b/drivers/sensor/icm42688/icm42688.c index b8ebef4f147eec8..12af70385134844 100644 --- a/drivers/sensor/icm42688/icm42688.c +++ b/drivers/sensor/icm42688/icm42688.c @@ -156,6 +156,19 @@ static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan, res = -EINVAL; } break; + case SENSOR_CHAN_ALL: + if (attr == SENSOR_ATTR_FIFO_WATERMARK) { + int64_t mval = sensor_value_to_micro(val); + + if (mval < 0 || mval > 1000000) { + return -EINVAL; + } + new_config.fifo_wm = CLAMP(mval * 2048 / 1000000, 0, 2048); + } else { + LOG_ERR("Unsupported attribute"); + res = -EINVAL; + } + break; default: LOG_ERR("Unsupported channel"); res = -EINVAL; @@ -258,7 +271,7 @@ int icm42688_init(const struct device *dev) data->cfg.gyro_fs = ICM42688_GYRO_FS_125; data->cfg.accel_odr = ICM42688_ACCEL_ODR_1000; data->cfg.gyro_odr = ICM42688_GYRO_ODR_1000; - data->cfg.fifo_en = false; + data->cfg.fifo_en = IS_ENABLED(CONFIG_ICM42688_STREAM); res = icm42688_configure(dev, &data->cfg); if (res != 0) { @@ -284,8 +297,15 @@ void icm42688_unlock(const struct device *dev) #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) \ + SPI_DT_IODEV_DEFINE(icm42688_spi_iodev_##inst, DT_DRV_INST(inst), ICM42688_SPI_CFG, 0U); \ + RTIO_DEFINE(icm42688_rtio_##inst, 8, 4); + #define ICM42688_DEFINE_DATA(inst) \ - static struct icm42688_dev_data icm42688_driver_##inst; + IF_ENABLED(CONFIG_ICM42688_STREAM, (ICM42688_RTIO_DEFINE(inst))); \ + static struct icm42688_dev_data icm42688_driver_##inst = { \ + IF_ENABLED(CONFIG_ICM42688_STREAM, (.r = &icm42688_rtio_##inst, \ + .spi_iodev = &icm42688_spi_iodev_##inst,))}; #define ICM42688_INIT(inst) \ ICM42688_DEFINE_DATA(inst); \ diff --git a/drivers/sensor/icm42688/icm42688.h b/drivers/sensor/icm42688/icm42688.h index 18ae4606e72593a..c4167e6a3b9cf4b 100644 --- a/drivers/sensor/icm42688/icm42688.h +++ b/drivers/sensor/icm42688/icm42688.h @@ -385,6 +385,9 @@ struct icm42688_cfg { /* TODO additional FIFO options */ /* TODO interrupt options */ + bool interrupt1_drdy; + bool interrupt1_fifo_ths; + bool interrupt1_fifo_full; }; struct icm42688_trigger_entry { @@ -405,6 +408,15 @@ struct icm42688_dev_data { #elif defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD) struct k_work work; #endif +#ifdef CONFIG_ICM42688_STREAM + struct rtio_iodev_sqe *streaming_sqe; + struct rtio *r; + struct rtio_iodev *spi_iodev; + uint8_t int_status; + uint16_t fifo_count; + uint64_t timestamp; + atomic_t reading_fifo; +#endif /* CONFIG_ICM42688_STREAM */ const struct device *dev; struct gpio_callback gpio_cb; sensor_trigger_handler_t data_ready_handler; diff --git a/drivers/sensor/icm42688/icm42688_common.c b/drivers/sensor/icm42688/icm42688_common.c index 3ccd107cfbb3ef7..9cb3037ed06ea3b 100644 --- a/drivers/sensor/icm42688/icm42688_common.c +++ b/drivers/sensor/icm42688/icm42688_common.c @@ -12,6 +12,7 @@ #include "icm42688.h" #include "icm42688_reg.h" #include "icm42688_spi.h" +#include "icm42688_trigger.h" #include LOG_MODULE_REGISTER(ICM42688_LL, CONFIG_SENSOR_LOG_LEVEL); @@ -165,8 +166,12 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) } /* Pulse mode with async reset (resets interrupt line on int status read) */ - res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_CONFIG, - BIT_INT1_DRIVE_CIRCUIT | BIT_INT1_POLARITY); + if (IS_ENABLED(CONFIG_ICM42688_TRIGGER)) { + res = icm42688_trigger_enable_interrupt(dev, cfg); + } else { + res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_CONFIG, + BIT_INT1_DRIVE_CIRCUIT | BIT_INT1_POLARITY); + } if (res) { LOG_ERR("Error writing to INT_CONFIG"); return res; diff --git a/drivers/sensor/icm42688/icm42688_decoder.c b/drivers/sensor/icm42688/icm42688_decoder.c index 42a44496ef70e09..9c79ccaa2c1c78e 100644 --- a/drivers/sensor/icm42688/icm42688_decoder.c +++ b/drivers/sensor/icm42688/icm42688_decoder.c @@ -218,6 +218,98 @@ int icm42688_encode(const struct device *dev, const enum sensor_channel *const c return 0; } +static int icm42688_fifo_decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, + q31_t *values, uint8_t max_count) +{ + const struct icm42688_fifo_data *edata = (const struct icm42688_fifo_data *)buffer; + + if (edata->fifo_count <= *fit) { + return 0; + } + + /* Jump to the frame start */ + buffer += sizeof(struct icm42688_fifo_data) + *fit; + + const bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]); + const bool has_accel = FIELD_GET(FIFO_HEADER_ACCEL, buffer[0]); + const bool has_gyro = FIELD_GET(FIFO_HEADER_GYRO, buffer[0]); + const int channel_count = 1 + (has_accel ? 3 : 0) + (has_gyro ? 3 : 0); + const uint8_t *frame_end = buffer + /* header */ 1 + /* accel */ (has_accel ? 6 : 0) + + /* gyro */ (has_gyro ? 6 : 0) + + /* timestamp */ (has_accel && has_gyro ? 2 : 0) + + /* temperature */ (is_20b ? 2 : 1); + int header_channels[7]; + int count = 0; + + if (*cit >= channel_count) { + return -1; + } + + if (has_accel) { + header_channels[count++] = SENSOR_CHAN_ACCEL_X; + header_channels[count++] = SENSOR_CHAN_ACCEL_Y; + header_channels[count++] = SENSOR_CHAN_ACCEL_Z; + } + if (has_gyro) { + header_channels[count++] = SENSOR_CHAN_GYRO_X; + header_channels[count++] = SENSOR_CHAN_GYRO_Y; + header_channels[count++] = SENSOR_CHAN_GYRO_Z; + } + header_channels[count] = SENSOR_CHAN_DIE_TEMP; + count = 0; + + while (count < max_count && *cit < channel_count) { + /* + * [0]@0 - accel_x + * [1]@2 - accel_y + * [2]@4 - accel_z + * [3]@6 - gyro_x + * [4]@8 - gyro_y + * [5]@a - gyro_z + * [6]@c - temp + */ + int offset = 1 + *cit * 2; + + channels[count] = header_channels[*cit]; + int32_t value; + int32_t value_max = INT16_MAX; + + if (channels[count] == SENSOR_CHAN_DIE_TEMP) { + value = buffer[offset]; + if (is_20b) { + value |= (buffer[offset + 1] << 8); + } else { + value_max = INT8_MAX; + } + } else { + value = (buffer[offset] << 8) | buffer[offset + 1]; + if (is_20b) { + int mask = channels[count] < SENSOR_CHAN_ACCEL_XYZ ? GENMASK(7, 4) + : GENMASK(3, 0); + /* Get the offset after the end of the frame data */ + int ext_offset = + (channels[count] - (channels[count] < SENSOR_CHAN_ACCEL_XYZ + ? SENSOR_CHAN_ACCEL_X + : SENSOR_CHAN_GYRO_X)); + + value = (value << 4) | FIELD_GET(mask, frame_end[ext_offset]); + value_max = (1 << 19) - 1; + } + } + values[count] = (q31_t)((int64_t)value * INT32_MAX / value_max); + count++; + *cit += 1; + } + + if (*cit == channel_count) { + /* Reached the end of the frame */ + *fit += (uintptr_t)frame_end - (uintptr_t)buffer + (is_20b ? 3 : 0); + *cit = 0; + } + return count; +} + static int icm42688_one_shot_decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, sensor_channel_iterator_t *cit, enum sensor_channel *channels, q31_t *values, uint8_t max_count) @@ -271,13 +363,53 @@ static int icm42688_decoder_decode(const uint8_t *buffer, sensor_frame_iterator_ sensor_channel_iterator_t *cit, enum sensor_channel *channels, q31_t *values, uint8_t max_count) { + const struct icm42688_decoder_header *header = + (const struct icm42688_decoder_header *)buffer; + + if (header->is_fifo) { + return icm42688_fifo_decode(buffer, fit, cit, channels, values, max_count); + } return icm42688_one_shot_decode(buffer, fit, cit, channels, values, max_count); } static int icm42688_decoder_get_frame_count(const uint8_t *buffer, uint16_t *frame_count) { - ARG_UNUSED(buffer); - *frame_count = 1; + const struct icm42688_fifo_data *data = (const struct icm42688_fifo_data *)buffer; + const struct icm42688_decoder_header *header = &data->header; + + if (!header->is_fifo) { + *frame_count = 1; + return 0; + } + + /* Skip the header */ + buffer += sizeof(struct icm42688_fifo_data); + + uint16_t count = 0; + const uint8_t *end = buffer + data->fifo_count; + + while (buffer < end) { + bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]); + int size = is_20b ? 3 : 2; + + if (FIELD_GET(FIFO_HEADER_ACCEL, buffer[0])) { + size += 6; + } + if (FIELD_GET(FIFO_HEADER_GYRO, buffer[0])) { + size += 6; + } + if (FIELD_GET(FIFO_HEADER_TIMESTAMP_FSYNC, buffer[0])) { + size += 2; + } + if (is_20b) { + size += 3; + } + + buffer += size; + ++count; + } + + *frame_count = count; return 0; } @@ -290,6 +422,26 @@ static int icm42688_decoder_get_timestamp(const uint8_t *buffer, uint64_t *times return 0; } +static bool icm24688_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger) +{ + const struct icm42688_fifo_data *edata = (const struct icm42688_fifo_data *)buffer; + + if (!edata->header.is_fifo) { + return false; + } + + switch (trigger) { + case SENSOR_TRIG_DATA_READY: + return FIELD_GET(BIT_INT_STATUS_DATA_RDY, edata->int_status); + case SENSOR_TRIG_FIFO_THRESHOLD: + return FIELD_GET(BIT_INT_STATUS_FIFO_THS, edata->int_status); + case SENSOR_TRIG_FIFO_FULL: + return FIELD_GET(BIT_INT_STATUS_FIFO_FULL, edata->int_status); + default: + return false; + } +} + static int icm42688_decoder_get_shift(const uint8_t *buffer, enum sensor_channel channel_type, int8_t *shift) { @@ -303,6 +455,7 @@ SENSOR_DECODER_API_DT_DEFINE() = { .get_frame_count = icm42688_decoder_get_frame_count, .get_timestamp = icm42688_decoder_get_timestamp, .get_shift = icm42688_decoder_get_shift, + .has_trigger = icm24688_decoder_has_trigger, .decode = icm42688_decoder_decode, }; diff --git a/drivers/sensor/icm42688/icm42688_decoder.h b/drivers/sensor/icm42688/icm42688_decoder.h index 2ef806373c94021..499cf3d0801554d 100644 --- a/drivers/sensor/icm42688/icm42688_decoder.h +++ b/drivers/sensor/icm42688/icm42688_decoder.h @@ -18,6 +18,15 @@ struct icm42688_decoder_header { uint8_t reserved: 2; } __attribute__((__packed__)); +struct icm42688_fifo_data { + struct icm42688_decoder_header header; + uint8_t int_status; + uint16_t gyro_odr: 4; + uint16_t accel_odr: 4; + uint16_t fifo_count: 11; + uint16_t reserved: 5; +} __attribute__((__packed__)); + struct icm42688_encoded_data { struct icm42688_decoder_header header; struct { diff --git a/drivers/sensor/icm42688/icm42688_reg.h b/drivers/sensor/icm42688/icm42688_reg.h index dfc348c072c8c75..5f3b58f0b3c074d 100644 --- a/drivers/sensor/icm42688/icm42688_reg.h +++ b/drivers/sensor/icm42688/icm42688_reg.h @@ -286,4 +286,12 @@ #define MCLK_POLL_ATTEMPTS 100 #define SOFT_RESET_TIME_MS 2 /* 1ms + elbow room */ +/* FIFO header */ +#define FIFO_HEADER_ACCEL BIT(6) +#define FIFO_HEADER_GYRO BIT(5) +#define FIFO_HEADER_20 BIT(4) +#define FIFO_HEADER_TIMESTAMP_FSYNC GENMASK(3, 2) +#define FIFO_HEADER_ODR_ACCEL BIT(1) +#define FIFO_HEADER_ODR_GYRO BIT(0) + #endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_REG_H_ */ diff --git a/drivers/sensor/icm42688/icm42688_rtio.c b/drivers/sensor/icm42688/icm42688_rtio.c index c1b20689393d6c0..1d70aadfd534139 100644 --- a/drivers/sensor/icm42688/icm42688_rtio.c +++ b/drivers/sensor/icm42688/icm42688_rtio.c @@ -42,7 +42,7 @@ static int icm42688_rtio_sample_fetch(const struct device *dev, int16_t readings return 0; } -int icm42688_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +static int icm42688_submit_one_shot(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) { const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; const enum sensor_channel *const channels = cfg->channels; @@ -78,4 +78,317 @@ int icm42688_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) return 0; } +int icm42688_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + struct icm42688_dev_data *data = dev->data; + const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; + int rc; + + if (!cfg->is_streaming) { + return icm42688_submit_one_shot(dev, iodev_sqe); + } + + /* Only support */ + struct icm42688_cfg new_config = data->cfg; + + new_config.interrupt1_drdy = false; + new_config.interrupt1_fifo_ths = false; + new_config.interrupt1_fifo_full = false; + for (int i = 0; i < cfg->count; ++i) { + switch (cfg->triggers[i].trigger) { + case SENSOR_TRIG_DATA_READY: + new_config.interrupt1_drdy = true; + break; + case SENSOR_TRIG_FIFO_THRESHOLD: + new_config.interrupt1_fifo_ths = true; + break; + case SENSOR_TRIG_FIFO_FULL: + new_config.interrupt1_fifo_full = true; + break; + default: + LOG_DBG("Trigger (%d) not supported", cfg->triggers[i].trigger); + break; + } + } + + if (new_config.interrupt1_drdy != data->cfg.interrupt1_drdy || + new_config.interrupt1_fifo_ths != data->cfg.interrupt1_fifo_ths || + new_config.interrupt1_fifo_full != data->cfg.interrupt1_fifo_full) { + rc = icm42688_safely_configure(dev, &new_config); + if (rc != 0) { + LOG_ERR("Failed to configure sensor"); + return rc; + } + } + + data->streaming_sqe = iodev_sqe; + return 0; +} + BUILD_ASSERT(sizeof(struct icm42688_decoder_header) == 9); + +static void icm42688_complete_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg) +{ + const struct device *dev = arg; + struct icm42688_dev_data *drv_data = dev->data; + const struct icm42688_dev_cfg *drv_cfg = dev->config; + struct rtio_iodev_sqe *iodev_sqe = sqe->userdata; + + rtio_iodev_sqe_ok(iodev_sqe, drv_data->fifo_count); + + gpio_pin_interrupt_configure_dt(&drv_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_dev_data *drv_data = dev->data; + const struct icm42688_dev_cfg *drv_cfg = dev->config; + struct rtio_iodev *spi_iodev = drv_data->spi_iodev; + 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_iodev_sqe *iodev_sqe = drv_data->streaming_sqe; + + drv_data->streaming_sqe = NULL; + + /* Not inherently an underrun/overrun as we may have a buffer to fill next time */ + if (iodev_sqe == NULL) { + LOG_DBG("No pending SQE"); + gpio_pin_interrupt_configure_dt(&drv_cfg->gpio_int1, + GPIO_INT_EDGE_TO_ACTIVE); + return; + } + + const size_t packet_size = drv_data->cfg.fifo_hires ? 20 : 16; + const size_t min_read_size = sizeof(struct icm42688_fifo_data) + packet_size; + const size_t ideal_read_size = sizeof(struct icm42688_fifo_data) + fifo_count; + uint8_t *buf; + uint32_t buf_len; + + if (rtio_sqe_rx_buf(iodev_sqe, min_read_size, ideal_read_size, &buf, &buf_len) != 0) { + LOG_ERR("Failed to get buffer"); + rtio_iodev_sqe_err(iodev_sqe, -ENOMEM); + return; + } + LOG_DBG("Requesting buffer [%u, %u] got %u", (unsigned int)min_read_size, + (unsigned int)ideal_read_size, buf_len); + + /* 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 icm42688_fifo_data hdr = { + .header = { + .is_fifo = true, + .gyro_fs = drv_data->cfg.gyro_fs, + .accel_fs = drv_data->cfg.accel_fs, + .timestamp = drv_data->timestamp, + }, + .int_status = drv_data->int_status, + .gyro_odr = drv_data->cfg.gyro_odr, + .accel_odr = drv_data->cfg.accel_odr, + }; + uint32_t buf_avail = buf_len; + + memcpy(buf, &hdr, sizeof(hdr)); + buf_avail -= sizeof(hdr); + + uint32_t read_len = MIN(fifo_count, buf_avail); + uint32_t pkts = read_len / packet_size; + + read_len = pkts * packet_size; + ((struct icm42688_fifo_data *)buf)->fifo_count = read_len; + + __ASSERT_NO_MSG(read_len % pkt_size == 0); + + uint8_t *read_buf = buf + sizeof(hdr); + + /* Flush out completions */ + struct rtio_cqe *cqe; + + do { + cqe = rtio_cqe_consume(r); + if (cqe != NULL) { + rtio_cqe_release(r, cqe); + } + } while (cqe != NULL); + + /* Setup new rtio chain to read the fifo data and report then check the + * result + */ + struct rtio_sqe *write_fifo_addr = rtio_sqe_acquire(r); + struct rtio_sqe *read_fifo_data = rtio_sqe_acquire(r); + struct rtio_sqe *complete_op = rtio_sqe_acquire(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, spi_iodev, RTIO_PRIO_NORM, ®_addr, 1, NULL); + write_fifo_addr->flags = RTIO_SQE_TRANSACTION; + rtio_sqe_prep_read(read_fifo_data, spi_iodev, RTIO_PRIO_NORM, read_buf, read_len, + iodev_sqe); + + rtio_sqe_prep_callback(complete_op, icm42688_complete_cb, (void *)dev, iodev_sqe); + + rtio_submit(r, 0); +} + +static struct sensor_stream_trigger * +icm42688_get_read_config_trigger(const struct sensor_read_config *cfg, + enum sensor_trigger_type trig) +{ + for (int i = 0; i < cfg->count; ++i) { + if (cfg->triggers[i].trigger == trig) { + return &cfg->triggers[i]; + } + } + LOG_DBG("Unsupported trigger (%d)", trig); + return NULL; +} + +static void icm42688_int_status_cb(struct rtio *r, const struct rtio_sqe *sqr, void *arg) +{ + const struct device *dev = arg; + struct icm42688_dev_data *drv_data = dev->data; + const struct icm42688_dev_cfg *drv_cfg = dev->config; + struct rtio_iodev *spi_iodev = drv_data->spi_iodev; + struct rtio_iodev_sqe *streaming_sqe = drv_data->streaming_sqe; + struct sensor_read_config *read_config; + + if (streaming_sqe == NULL) { + return; + } + + read_config = (struct sensor_read_config *)streaming_sqe->sqe.iodev->data; + __ASSERT_NO_MSG(read_config != NULL); + + if (!read_config->is_streaming) { + /* Oops, not really configured for streaming data */ + return; + } + + struct sensor_stream_trigger *fifo_ths_cfg = + icm42688_get_read_config_trigger(read_config, SENSOR_TRIG_FIFO_THRESHOLD); + bool has_fifo_ths_trig = fifo_ths_cfg != NULL && + FIELD_GET(BIT_INT_STATUS_FIFO_THS, drv_data->int_status) != 0; + + struct sensor_stream_trigger *fifo_full_cfg = + icm42688_get_read_config_trigger(read_config, SENSOR_TRIG_FIFO_FULL); + bool has_fifo_full_trig = fifo_full_cfg != NULL && + FIELD_GET(BIT_INT_STATUS_FIFO_FULL, drv_data->int_status) != 0; + + if (!has_fifo_ths_trig && !has_fifo_full_trig) { + gpio_pin_interrupt_configure_dt(&drv_cfg->gpio_int1, + GPIO_INT_EDGE_TO_ACTIVE); + return; + } + + /* Flush completions */ + struct rtio_cqe *cqe; + + do { + cqe = rtio_cqe_consume(r); + if (cqe != NULL) { + rtio_cqe_release(r, cqe); + } + } while (cqe != NULL); + + enum sensor_stream_data_opt data_opt; + + if (has_fifo_ths_trig && !has_fifo_full_trig) { + /* Only care about fifo threshold */ + data_opt = fifo_ths_cfg->opt; + } else if (!has_fifo_ths_trig && has_fifo_full_trig) { + /* Only care about fifo full */ + data_opt = fifo_full_cfg->opt; + } else { + /* Both fifo threshold and full */ + data_opt = MIN(fifo_ths_cfg->opt, fifo_full_cfg->opt); + } + + if (data_opt == SENSOR_STREAM_DATA_NOP || data_opt == SENSOR_STREAM_DATA_DROP) { + uint8_t *buf; + uint32_t buf_len; + + /* Clear streaming_sqe since we're done with the call */ + drv_data->streaming_sqe = NULL; + if (rtio_sqe_rx_buf(streaming_sqe, sizeof(struct icm42688_fifo_data), + sizeof(struct icm42688_fifo_data), &buf, &buf_len) != 0) { + rtio_iodev_sqe_err(streaming_sqe, -ENOMEM); + return; + } + + struct icm42688_fifo_data *data = (struct icm42688_fifo_data *)buf; + + memset(buf, 0, buf_len); + data->header.timestamp = drv_data->timestamp; + data->int_status = drv_data->int_status; + data->fifo_count = 0; + rtio_iodev_sqe_ok(streaming_sqe, 0); + gpio_pin_interrupt_configure_dt(&drv_cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE); + if (data_opt == SENSOR_STREAM_DATA_DROP) { + /* Flush the FIFO */ + struct rtio_sqe *write_signal_path_reset = rtio_sqe_acquire(r); + uint8_t write_buffer[] = { + FIELD_GET(REG_ADDRESS_MASK, REG_SIGNAL_PATH_RESET), + BIT_FIFO_FLUSH, + }; + + rtio_sqe_prep_tiny_write(write_signal_path_reset, spi_iodev, RTIO_PRIO_NORM, + write_buffer, ARRAY_SIZE(write_buffer), NULL); + /* TODO Add a new flag for fire-and-forget so we don't have to block here */ + rtio_submit(r, 1); + ARG_UNUSED(rtio_cqe_consume(r)); + } + return; + } + + /* We need the data, read the fifo length */ + struct rtio_sqe *write_fifo_count_reg = rtio_sqe_acquire(r); + struct rtio_sqe *read_fifo_count = rtio_sqe_acquire(r); + struct rtio_sqe *check_fifo_count = rtio_sqe_acquire(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, spi_iodev, RTIO_PRIO_NORM, ®, 1, NULL); + write_fifo_count_reg->flags = RTIO_SQE_TRANSACTION; + rtio_sqe_prep_read(read_fifo_count, spi_iodev, RTIO_PRIO_NORM, read_buf, 2, NULL); + rtio_sqe_prep_callback(check_fifo_count, icm42688_fifo_count_cb, arg, NULL); + + rtio_submit(r, 0); +} + +void icm42688_fifo_event(const struct device *dev) +{ + struct icm42688_dev_data *drv_data = dev->data; + struct rtio_iodev *spi_iodev = drv_data->spi_iodev; + struct rtio *r = drv_data->r; + + if (drv_data->streaming_sqe == NULL) { + return; + } + + drv_data->timestamp = k_ticks_to_ns_floor64(k_uptime_ticks()); + + /* + * 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(r); + struct rtio_sqe *read_int_reg = rtio_sqe_acquire(r); + struct rtio_sqe *check_int_status = rtio_sqe_acquire(r); + uint8_t reg = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_INT_STATUS); + + rtio_sqe_prep_tiny_write(write_int_reg, spi_iodev, RTIO_PRIO_NORM, ®, 1, NULL); + write_int_reg->flags = RTIO_SQE_TRANSACTION; + rtio_sqe_prep_read(read_int_reg, spi_iodev, RTIO_PRIO_NORM, &drv_data->int_status, 1, NULL); + rtio_sqe_prep_callback(check_int_status, icm42688_int_status_cb, (void *)dev, NULL); + rtio_submit(r, 0); +} diff --git a/drivers/sensor/icm42688/icm42688_rtio.h b/drivers/sensor/icm42688/icm42688_rtio.h index 748c8a022b09238..9b236aa41f73e25 100644 --- a/drivers/sensor/icm42688/icm42688_rtio.h +++ b/drivers/sensor/icm42688/icm42688_rtio.h @@ -9,4 +9,6 @@ int icm42688_submit(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe); +void icm42688_fifo_event(const struct device *dev); + #endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_RTIO_H_ */ diff --git a/drivers/sensor/icm42688/icm42688_trigger.c b/drivers/sensor/icm42688/icm42688_trigger.c index adaecf84baa6566..b00df2596bcabd2 100644 --- a/drivers/sensor/icm42688/icm42688_trigger.c +++ b/drivers/sensor/icm42688/icm42688_trigger.c @@ -12,6 +12,7 @@ #include "icm42688.h" #include "icm42688_reg.h" +#include "icm42688_rtio.h" #include "icm42688_spi.h" #include "icm42688_trigger.h" @@ -30,6 +31,9 @@ static void icm42688_gpio_callback(const struct device *dev, struct gpio_callbac #elif defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD) k_work_submit(&data->work); #endif + if (IS_ENABLED(CONFIG_ICM42688_STREAM)) { + icm42688_fifo_event(data->dev); + } } #if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD) || defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD) @@ -90,6 +94,8 @@ int icm42688_trigger_set(const struct device *dev, const struct sensor_trigger * switch (trig->type) { case SENSOR_TRIG_DATA_READY: + case SENSOR_TRIG_FIFO_THRESHOLD: + case SENSOR_TRIG_FIFO_FULL: data->data_ready_handler = handler; data->data_ready_trigger = trig; @@ -146,7 +152,7 @@ int icm42688_trigger_init(const struct device *dev) return gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE); } -int icm42688_trigger_enable_interrupt(const struct device *dev) +int icm42688_trigger_enable_interrupt(const struct device *dev, struct icm42688_cfg *new_cfg) { int res; const struct icm42688_dev_cfg *cfg = dev->config; @@ -164,9 +170,19 @@ int icm42688_trigger_enable_interrupt(const struct device *dev) return res; } - /* enable data ready interrupt on INT1 pin */ - return icm42688_spi_single_write(&cfg->spi, REG_INT_SOURCE0, - FIELD_PREP(BIT_UI_DRDY_INT1_EN, 1)); + /* enable interrupts on INT1 pin */ + uint8_t value = 0; + + if (new_cfg->interrupt1_drdy) { + value |= FIELD_PREP(BIT_UI_DRDY_INT1_EN, 1); + } + if (new_cfg->interrupt1_fifo_ths) { + value |= FIELD_PREP(BIT_FIFO_THS_INT1_EN, 1); + } + if (new_cfg->interrupt1_fifo_full) { + value |= FIELD_PREP(BIT_FIFO_FULL_INT1_EN, 1); + } + return icm42688_spi_single_write(&cfg->spi, REG_INT_SOURCE0, value); } void icm42688_lock(const struct device *dev) diff --git a/drivers/sensor/icm42688/icm42688_trigger.h b/drivers/sensor/icm42688/icm42688_trigger.h index 5ed382eb0d403a1..e03975916181203 100644 --- a/drivers/sensor/icm42688/icm42688_trigger.h +++ b/drivers/sensor/icm42688/icm42688_trigger.h @@ -26,9 +26,10 @@ int icm42688_trigger_init(const struct device *dev); * @brief enable the trigger gpio interrupt * * @param dev icm42688 device pointer + * @param new_cfg New configuration to use for the device * @return int 0 on success, negative error code otherwise */ -int icm42688_trigger_enable_interrupt(const struct device *dev); +int icm42688_trigger_enable_interrupt(const struct device *dev, struct icm42688_cfg *new_cfg); /** * @brief lock access to the icm42688 device driver