diff --git a/drivers/sensor/icm42688/CMakeLists.txt b/drivers/sensor/icm42688/CMakeLists.txt index cf9308bb95c447..fbc63b6a4b2291 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_stream.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 2944a9d1183ad4..d7ee43bc7618bf 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 43fd6a72f47f68..ee92c30987ecfc 100644 --- a/drivers/sensor/icm42688/icm42688.c +++ b/drivers/sensor/icm42688/icm42688.c @@ -82,7 +82,7 @@ int icm42688_channel_parse_readings(enum sensor_channel chan, int16_t readings[7 } static int icm42688_channel_get(const struct device *dev, enum sensor_channel chan, - struct sensor_value *val) + struct sensor_value *val) { struct icm42688_dev_data *data = dev->data; @@ -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; @@ -257,7 +270,13 @@ int icm42688_init(const struct device *dev) data->cfg.gyro_mode = ICM42688_GYRO_LN; data->cfg.gyro_fs = ICM42688_GYRO_FS_125; data->cfg.gyro_odr = ICM42688_GYRO_ODR_1000; - data->cfg.fifo_en = false; + data->cfg.temp_dis = false; + data->cfg.fifo_en = IS_ENABLED(CONFIG_ICM42688_STREAM); + data->cfg.fifo_wm = 0; + data->cfg.fifo_hires = 0; + data->cfg.interrupt1_drdy = 0; + data->cfg.interrupt1_fifo_ths = 0; + data->cfg.interrupt1_fifo_full = 0; res = icm42688_configure(dev, &data->cfg); if (res != 0) { @@ -283,8 +302,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 18ae4606e72593..c4167e6a3b9cf4 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 3ccd107cfbb3ef..9cb3037ed06ea3 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 0a340aff369567..3dc556ac61c7c3 100644 --- a/drivers/sensor/icm42688/icm42688_decoder.c +++ b/drivers/sensor/icm42688/icm42688_decoder.c @@ -198,9 +198,262 @@ int icm42688_encode(const struct device *dev, const enum sensor_channel *const c return 0; } +#define IS_ACCEL(chan) ((chan) >= SENSOR_CHAN_ACCEL_X && (chan) <= SENSOR_CHAN_ACCEL_XYZ) +#define IS_GYRO(chan) ((chan) >= SENSOR_CHAN_GYRO_X && (chan) <= SENSOR_CHAN_GYRO_XYZ) + +static inline q31_t icm42688_read_temperature_from_packet(const uint8_t *pkt) +{ + int32_t temperature; + int32_t whole; + int32_t fraction; + + /* Temperature always assumes a shift of 9 for a range of (-273,273) C */ + if (FIELD_GET(FIFO_HEADER_20, pkt[0]) == 1) { + temperature = (pkt[0xd] << 8) | pkt[0xe]; + + icm42688_temp_c(temperature, &whole, &fraction); + } else { + if (FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1 && + FIELD_GET(FIFO_HEADER_GYRO, pkt[0]) == 1) { + temperature = pkt[0xd]; + } else { + temperature = pkt[0x7]; + } + + int64_t sensitivity = 207; + int64_t temperature100 = (temperature * 100) + (25 * sensitivity); + + whole = temperature100 / sensitivity; + fraction = + ((temperature100 - whole * sensitivity) * INT64_C(1000000)) / sensitivity; + } + __ASSERT_NO_MSG(whole >= -512 && whole <= 511); + return FIELD_PREP(GENMASK(31, 22), whole) | (fraction * GENMASK64(21, 0) / 1000000); +} + +static int icm42688_read_imu_from_packet(const uint8_t *pkt, bool is_accel, int fs, + uint8_t axis_offset, q31_t *out) +{ + int32_t value; + int64_t scale = 0; + int32_t max = BIT(15); + int offset = 1 + (axis_offset * 2); + + if (is_accel) { + switch (fs) { + case ICM42688_ACCEL_FS_2G: + scale = INT64_C(2) * BIT(31 - 5) * 9.80665; + break; + case ICM42688_ACCEL_FS_4G: + scale = INT64_C(4) * BIT(31 - 6) * 9.80665; + break; + case ICM42688_ACCEL_FS_8G: + scale = INT64_C(8) * BIT(31 - 7) * 9.80665; + break; + case ICM42688_ACCEL_FS_16G: + scale = INT64_C(16) * BIT(31 - 8) * 9.80665; + break; + } + } else { + switch (fs) { + case ICM42688_GYRO_FS_2000: + scale = 164; + break; + case ICM42688_GYRO_FS_1000: + scale = 328; + break; + case ICM42688_GYRO_FS_500: + scale = 655; + break; + case ICM42688_GYRO_FS_250: + scale = 1310; + break; + case ICM42688_GYRO_FS_125: + scale = 2620; + break; + case ICM42688_GYRO_FS_62_5: + scale = 5243; + break; + case ICM42688_GYRO_FS_31_25: + scale = 10486; + break; + case ICM42688_GYRO_FS_15_625: + scale = 20972; + break; + } + } + + if (!is_accel && FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1) { + offset += 7; + } + + value = (int16_t)sys_le16_to_cpu((pkt[offset] << 8) | pkt[offset + 1]); + + if (FIELD_GET(FIFO_HEADER_20, pkt[0]) == 1) { + uint32_t mask = is_accel ? GENMASK(7, 4) : GENMASK(3, 0); + + offset = 0x11 + axis_offset; + value = (value << 4) | FIELD_GET(mask, pkt[offset]); + /* In 20 bit mode, FS can only be +/-16g and +/-2000dps */ + scale = is_accel ? (INT64_C(16) * BIT(8) * 9.80665) : 131; + max = is_accel ? BIT(18) : BIT(19); + if (value == -524288) { + /* Invalid 20 bit value */ + return -ENODATA; + } + } else { + if (value <= -32767) { + /* Invalid 16 bit value */ + return -ENODATA; + } + } + + *out = (q31_t)(value * scale / max); + return 0; +} + +static uint32_t accel_period_ns[] = { + [ICM42688_ACCEL_ODR_1_5625] = UINT32_C(10000000000000) / 15625, + [ICM42688_ACCEL_ODR_3_125] = UINT32_C(10000000000000) / 31250, + [ICM42688_ACCEL_ODR_6_25] = UINT32_C(10000000000000) / 62500, + [ICM42688_ACCEL_ODR_12_5] = UINT32_C(10000000000000) / 12500, + [ICM42688_ACCEL_ODR_25] = UINT32_C(1000000000) / 25, + [ICM42688_ACCEL_ODR_50] = UINT32_C(1000000000) / 50, + [ICM42688_ACCEL_ODR_100] = UINT32_C(1000000000) / 100, + [ICM42688_ACCEL_ODR_200] = UINT32_C(1000000000) / 200, + [ICM42688_ACCEL_ODR_500] = UINT32_C(1000000000) / 500, + [ICM42688_ACCEL_ODR_1000] = UINT32_C(1000000), + [ICM42688_ACCEL_ODR_2000] = UINT32_C(1000000) / 2, + [ICM42688_ACCEL_ODR_4000] = UINT32_C(1000000) / 4, + [ICM42688_ACCEL_ODR_8000] = UINT32_C(1000000) / 8, + [ICM42688_ACCEL_ODR_16000] = UINT32_C(1000000) / 16, + [ICM42688_ACCEL_ODR_32000] = UINT32_C(1000000) / 32, +}; + +static uint32_t gyro_period_ns[] = { + [ICM42688_GYRO_ODR_12_5] = UINT32_C(10000000000000) / 12500, + [ICM42688_GYRO_ODR_25] = UINT32_C(1000000000) / 25, + [ICM42688_GYRO_ODR_50] = UINT32_C(1000000000) / 50, + [ICM42688_GYRO_ODR_100] = UINT32_C(1000000000) / 100, + [ICM42688_GYRO_ODR_200] = UINT32_C(1000000000) / 200, + [ICM42688_GYRO_ODR_500] = UINT32_C(1000000000) / 500, + [ICM42688_GYRO_ODR_1000] = UINT32_C(1000000), + [ICM42688_GYRO_ODR_2000] = UINT32_C(1000000) / 2, + [ICM42688_GYRO_ODR_4000] = UINT32_C(1000000) / 4, + [ICM42688_GYRO_ODR_8000] = UINT32_C(1000000) / 8, + [ICM42688_GYRO_ODR_16000] = UINT32_C(1000000) / 16, + [ICM42688_GYRO_ODR_32000] = UINT32_C(1000000) / 32, +}; + +static int icm42688_fifo_decode(const uint8_t *buffer, enum sensor_channel channel, + size_t channel_idx, uint32_t *fit, uint16_t max_count, + void *data_out) +{ + const struct icm42688_fifo_data *edata = (const struct icm42688_fifo_data *)buffer; + const uint8_t *buffer_end = buffer + sizeof(struct icm42688_fifo_data) + edata->fifo_count; + int accel_frame_count = 0; + int gyro_frame_count = 0; + int count = 0; + int rc; + + if ((uintptr_t)buffer_end <= *fit || channel_idx != 0) { + return 0; + } + + ((struct sensor_data_header *)data_out)->base_timestamp_ns = edata->header.timestamp; + + buffer += sizeof(struct icm42688_fifo_data); + while (count < max_count && buffer < buffer_end) { + const bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]) == 1; + const bool has_accel = FIELD_GET(FIFO_HEADER_ACCEL, buffer[0]) == 1; + const bool has_gyro = FIELD_GET(FIFO_HEADER_GYRO, buffer[0]) == 1; + const uint8_t *frame_end = buffer; + + if (is_20b) { + frame_end += 20; + } else if (has_accel && has_gyro) { + frame_end += 16; + } else { + frame_end += 8; + } + if (has_accel) { + accel_frame_count++; + } + if (has_gyro) { + gyro_frame_count++; + } + + if ((uintptr_t)buffer < *fit) { + /* This frame was already decoded, move on to the next frame */ + buffer = frame_end; + continue; + } + if (channel == SENSOR_CHAN_DIE_TEMP) { + struct sensor_q31_data *data = (struct sensor_q31_data *)data_out; + + data->shift = 9; + if (has_accel) { + data->readings[count].timestamp_delta = + accel_period_ns[edata->accel_odr] * (accel_frame_count - 1); + } else { + data->readings[count].timestamp_delta = + gyro_period_ns[edata->gyro_odr] * (gyro_frame_count - 1); + } + data->readings[count].temperature = + icm42688_read_temperature_from_packet(buffer); + } else if (IS_ACCEL(channel) && has_accel) { + /* Decode accel */ + struct sensor_three_axis_data *data = + (struct sensor_three_axis_data *)data_out; + uint64_t period_ns = accel_period_ns[edata->accel_odr]; + + icm42688_get_shift(SENSOR_CHAN_ACCEL_XYZ, edata->header.accel_fs, + edata->header.gyro_fs, &data->shift); + + data->readings[count].timestamp_delta = (accel_frame_count - 1) * period_ns; + rc = icm42688_read_imu_from_packet(buffer, true, edata->header.accel_fs, 0, + &data->readings[count].x); + rc |= icm42688_read_imu_from_packet(buffer, true, edata->header.accel_fs, 1, + &data->readings[count].y); + rc |= icm42688_read_imu_from_packet(buffer, true, edata->header.accel_fs, 2, + &data->readings[count].z); + if (rc != 0) { + accel_frame_count--; + buffer = frame_end; + continue; + } + } else if (IS_GYRO(channel) && has_gyro) { + /* Decode gyro */ + struct sensor_three_axis_data *data = + (struct sensor_three_axis_data *)data_out; + uint64_t period_ns = accel_period_ns[edata->gyro_odr]; + + icm42688_get_shift(SENSOR_CHAN_GYRO_XYZ, edata->header.accel_fs, + edata->header.gyro_fs, &data->shift); + + data->readings[count].timestamp_delta = (gyro_frame_count - 1) * period_ns; + rc = icm42688_read_imu_from_packet(buffer, false, edata->header.gyro_fs, 0, + &data->readings[count].x); + rc |= icm42688_read_imu_from_packet(buffer, false, edata->header.gyro_fs, 1, + &data->readings[count].y); + rc |= icm42688_read_imu_from_packet(buffer, false, edata->header.gyro_fs, 2, + &data->readings[count].z); + if (rc != 0) { + gyro_frame_count--; + buffer = frame_end; + continue; + } + } + buffer = frame_end; + *fit = (uintptr_t)frame_end; + count++; + } + return count; +} + static int icm42688_one_shot_decode(const uint8_t *buffer, enum sensor_channel channel, - size_t channel_idx, uint32_t *fit, - uint16_t max_count, void *data_out) + size_t channel_idx, uint32_t *fit, uint16_t max_count, + void *data_out) { const struct icm42688_encoded_data *edata = (const struct icm42688_encoded_data *)buffer; const struct icm42688_decoder_header *header = &edata->header; @@ -318,34 +571,76 @@ static int icm42688_one_shot_decode(const uint8_t *buffer, enum sensor_channel c } static int icm42688_decoder_decode(const uint8_t *buffer, enum sensor_channel channel, - size_t channel_idx, uint32_t *fit, - uint16_t max_count, void *data_out) + size_t channel_idx, uint32_t *fit, uint16_t max_count, + void *data_out) { + const struct icm42688_decoder_header *header = + (const struct icm42688_decoder_header *)buffer; + + if (header->is_fifo) { + return icm42688_fifo_decode(buffer, channel, channel_idx, fit, max_count, data_out); + } return icm42688_one_shot_decode(buffer, channel, channel_idx, fit, max_count, data_out); } static int icm42688_decoder_get_frame_count(const uint8_t *buffer, enum sensor_channel channel, size_t channel_idx, uint16_t *frame_count) { - ARG_UNUSED(buffer); + const struct icm42688_fifo_data *data = (const struct icm42688_fifo_data *)buffer; + const struct icm42688_decoder_header *header = &data->header; + if (channel_idx != 0) { return -ENOTSUP; } - switch (channel) { - case SENSOR_CHAN_ACCEL_X: - case SENSOR_CHAN_ACCEL_Y: - case SENSOR_CHAN_ACCEL_Z: - case SENSOR_CHAN_ACCEL_XYZ: - case SENSOR_CHAN_GYRO_X: - case SENSOR_CHAN_GYRO_Y: - case SENSOR_CHAN_GYRO_Z: - case SENSOR_CHAN_GYRO_XYZ: - case SENSOR_CHAN_DIE_TEMP: - *frame_count = 1; + + if (!header->is_fifo) { + switch (channel) { + case SENSOR_CHAN_ACCEL_X: + case SENSOR_CHAN_ACCEL_Y: + case SENSOR_CHAN_ACCEL_Z: + case SENSOR_CHAN_ACCEL_XYZ: + case SENSOR_CHAN_GYRO_X: + case SENSOR_CHAN_GYRO_Y: + case SENSOR_CHAN_GYRO_Z: + case SENSOR_CHAN_GYRO_XYZ: + case SENSOR_CHAN_DIE_TEMP: + *frame_count = 1; + return 0; + default: + return -ENOTSUP; + } return 0; - default: - return -ENOTSUP; } + + /* 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; } static int icm42688_decoder_get_size_info(enum sensor_channel channel, size_t *base_size, @@ -372,10 +667,31 @@ static int icm42688_decoder_get_size_info(enum sensor_channel channel, size_t *b } } +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_WATERMARK: + 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; + } +} + SENSOR_DECODER_API_DT_DEFINE() = { .get_frame_count = icm42688_decoder_get_frame_count, .get_size_info = icm42688_decoder_get_size_info, .decode = icm42688_decoder_decode, + .has_trigger = icm24688_decoder_has_trigger, }; int icm42688_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder) diff --git a/drivers/sensor/icm42688/icm42688_decoder.h b/drivers/sensor/icm42688/icm42688_decoder.h index 2ef806373c9402..499cf3d0801554 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 dfc348c072c8c7..5f3b58f0b3c074 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 c1b20689393d6c..ccb9532ee45a2c 100644 --- a/drivers/sensor/icm42688/icm42688_rtio.c +++ b/drivers/sensor/icm42688/icm42688_rtio.c @@ -8,6 +8,7 @@ #include "icm42688.h" #include "icm42688_decoder.h" #include "icm42688_reg.h" +#include "icm42688_rtio.h" #include "icm42688_spi.h" #include @@ -42,7 +43,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 +79,17 @@ 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) +{ + const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; + + if (!cfg->is_streaming) { + return icm42688_submit_one_shot(dev, iodev_sqe); + } else if (IS_ENABLED(CONFIG_ICM42688_STREAM)) { + return icm42688_submit_stream(dev, iodev_sqe); + } else { + return -ENOTSUP; + } +} + BUILD_ASSERT(sizeof(struct icm42688_decoder_header) == 9); diff --git a/drivers/sensor/icm42688/icm42688_rtio.h b/drivers/sensor/icm42688/icm42688_rtio.h index 748c8a022b0923..888e8e95357aea 100644 --- a/drivers/sensor/icm42688/icm42688_rtio.h +++ b/drivers/sensor/icm42688/icm42688_rtio.h @@ -7,6 +7,13 @@ #ifndef ZEPHYR_DRIVERS_SENSOR_ICM42688_RTIO_H_ #define ZEPHYR_DRIVERS_SENSOR_ICM42688_RTIO_H_ +#include +#include + int icm42688_submit(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe); +int icm42688_submit_stream(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_rtio_stream.c b/drivers/sensor/icm42688/icm42688_rtio_stream.c new file mode 100644 index 00000000000000..57d1a66cfde9cc --- /dev/null +++ b/drivers/sensor/icm42688/icm42688_rtio_stream.c @@ -0,0 +1,319 @@ +/* + * Copyright (c) 2023 Google LLC + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include "icm42688.h" +#include "icm42688_decoder.h" +#include "icm42688_reg.h" +#include "icm42688_rtio.h" + +LOG_MODULE_DECLARE(ICM42688_RTIO); + +int icm42688_submit_stream(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; + struct icm42688_dev_data *data = sensor->data; + 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_WATERMARK: + 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) { + int rc = icm42688_safely_configure(sensor, &new_config); + + if (rc != 0) { + LOG_ERR("Failed to configure sensor"); + return rc; + } + } + + data->streaming_sqe = iodev_sqe; + return 0; +} + +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_WATERMARK); + 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_trigger.c b/drivers/sensor/icm42688/icm42688_trigger.c index adaecf84baa656..32b60f3cd75945 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_WATERMARK: + 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 5ed382eb0d403a..e0397591618120 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