From 066ac539eda6a3f50ae331e9d0e90ec169b3b7b4 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Wed, 3 May 2023 23:26:09 -0600 Subject: [PATCH] WIP: streaming sensor API Signed-off-by: Yuval Peress --- cmake/linker_script/common/common-rom.cmake | 4 + drivers/sensor/CMakeLists.txt | 2 +- drivers/sensor/Kconfig | 9 + drivers/sensor/default_rtio_sensor.c | 10 +- drivers/sensor/icm42688/CMakeLists.txt | 2 +- drivers/sensor/icm42688/Kconfig | 15 ++ drivers/sensor/icm42688/icm42688.c | 42 ++-- drivers/sensor/icm42688/icm42688.h | 20 +- drivers/sensor/icm42688/icm42688_common.c | 13 +- drivers/sensor/icm42688/icm42688_decoder.c | 129 +++++++++-- drivers/sensor/icm42688/icm42688_decoder.h | 3 +- drivers/sensor/icm42688/icm42688_rtio.c | 235 +++++++++++++++++++- drivers/sensor/icm42688/icm42688_rtio.h | 2 + drivers/sensor/icm42688/icm42688_trigger.c | 44 ++-- drivers/sensor/icm42688/icm42688_trigger.h | 3 +- drivers/sensor/sensor_decoders_init.c | 19 ++ drivers/sensor/sensor_handlers.c | 2 +- drivers/sensor/sensor_shell.c | 220 +++++++++++++++--- include/zephyr/drivers/sensor.h | 86 +++++-- samples/sensor/sensor_shell/prj.conf | 2 +- samples/sensor/sensor_shell/src/main.c | 8 - scripts/build/gen_kobject_list.py | 3 +- 22 files changed, 739 insertions(+), 134 deletions(-) create mode 100644 drivers/sensor/sensor_decoders_init.c diff --git a/cmake/linker_script/common/common-rom.cmake b/cmake/linker_script/common/common-rom.cmake index cc7cd40d570594..97f91270edf5d4 100644 --- a/cmake/linker_script/common/common-rom.cmake +++ b/cmake/linker_script/common/common-rom.cmake @@ -140,6 +140,10 @@ if(CONFIG_SENSOR_INFO) zephyr_iterable_section(NAME sensor_info KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN 4) endif() +if(CONFIG_SENSOR) + zephyr_iterable_section(NAME sensor_decoder_api KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN 4) +endif() + if(CONFIG_MCUMGR) zephyr_iterable_section(NAME mcumgr_handler KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN 4) endif() diff --git a/drivers/sensor/CMakeLists.txt b/drivers/sensor/CMakeLists.txt index 23aabe2139d24b..14876fc52161a2 100644 --- a/drivers/sensor/CMakeLists.txt +++ b/drivers/sensor/CMakeLists.txt @@ -139,7 +139,7 @@ add_subdirectory_ifdef(CONFIG_NTC_THERMISTOR ntc_thermistor) add_subdirectory_ifdef(CONFIG_S11059 s11059) zephyr_library() -zephyr_library_sources(default_rtio_sensor.c) +zephyr_library_sources(sensor_decoders_init.c default_rtio_sensor.c) zephyr_library_sources_ifdef(CONFIG_USERSPACE sensor_handlers.c) zephyr_library_sources_ifdef(CONFIG_SENSOR_SHELL sensor_shell.c) zephyr_library_sources_ifdef(CONFIG_SENSOR_SHELL_BATTERY shell_battery.c) diff --git a/drivers/sensor/Kconfig b/drivers/sensor/Kconfig index 6ea8ed1adb0b6a..167de2f23f7dd4 100644 --- a/drivers/sensor/Kconfig +++ b/drivers/sensor/Kconfig @@ -30,6 +30,15 @@ config SENSOR_SHELL help This shell provides access to basic sensor data. +config SENSOR_SHELL_THREAD_STACK_SIZE + int "Stack size for the sensor shell data processing thread" + depends on SENSOR_SHELL + default 1024 + help + The sensor shell uses a dedicated thread to process data coming from the + sensors in either one-shot or streaming mode. Use this config to control + the size of that thread's stack. + config SENSOR_SHELL_BATTERY bool "Sensor shell 'battery' command" depends on SHELL diff --git a/drivers/sensor/default_rtio_sensor.c b/drivers/sensor/default_rtio_sensor.c index feee040cce685d..46995c414087ed 100644 --- a/drivers/sensor/default_rtio_sensor.c +++ b/drivers/sensor/default_rtio_sensor.c @@ -16,13 +16,13 @@ static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_s static void sensor_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) { - const struct sensor_read_config *cfg = iodev_sqe->sqe->iodev->data; + const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; const struct device *dev = cfg->sensor; const struct sensor_driver_api *api = dev->api; if (api->submit != NULL) { api->submit(dev, iodev_sqe); - } else { + } else if (!cfg->is_streaming) { sensor_submit_fallback(dev, iodev_sqe); } } @@ -69,9 +69,9 @@ static inline uint32_t compute_min_buf_len(size_t num_channels, int num_output_s */ static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) { - const struct sensor_read_config *cfg = iodev_sqe->sqe->iodev->data; + const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; const enum sensor_channel *const channels = cfg->channels; - const size_t num_channels = cfg->num_channels; + const size_t num_channels = cfg->count; int num_output_samples = compute_num_samples(channels, num_channels); uint32_t min_buf_len = compute_min_buf_len(num_channels, num_output_samples); uint64_t timestamp_ns = k_ticks_to_ns_floor64(k_uptime_ticks()); @@ -167,7 +167,7 @@ void z_sensor_processing_loop(struct rtio *ctx, sensor_processing_callback_t cb) rtio_cqe_get_mempool_buffer(ctx, cqe, &buf, &buf_len); /* Release the CQE */ - rtio_cqe_release(ctx); + rtio_cqe_release(ctx, cqe); /* Call the callback */ cb(rc, buf, buf_len, userdata); diff --git a/drivers/sensor/icm42688/CMakeLists.txt b/drivers/sensor/icm42688/CMakeLists.txt index 3b971525d0ba66..1c8d1632632981 100644 --- a/drivers/sensor/icm42688/CMakeLists.txt +++ b/drivers/sensor/icm42688/CMakeLists.txt @@ -6,10 +6,10 @@ zephyr_library_sources( icm42688.c icm42688_common.c icm42688_decoder.c - icm42688_rtio.c icm42688_spi.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 5cdfb3cdc1eef8..a2e07ecac5a484 100644 --- a/drivers/sensor/icm42688/Kconfig +++ b/drivers/sensor/icm42688/Kconfig @@ -21,6 +21,13 @@ config EMUL_ICM42688 Enable the hardware emulator for the ICM42688. Doing so allows exercising sensor APIs for this IMU in native_posix and qemu. +config ICM42688_DECODER + bool "ICM42688 decoder logic" + default y if ICM42688 + help + Compile the ICM42688 decoder API which allows decoding raw data returned + from the sensor. + if ICM42688 choice @@ -42,6 +49,14 @@ 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 + 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 142405a70eb456..1382318a6964ba 100644 --- a/drivers/sensor/icm42688/icm42688.c +++ b/drivers/sensor/icm42688/icm42688.c @@ -127,7 +127,7 @@ int icm42688_channel_get(const struct device *dev, enum sensor_channel chan, { struct icm42688_sensor_data *data = dev->data; - return icm42688_channel_parse_readings(chan, data->readings, &data->dev_data.cfg, val); + return icm42688_channel_parse_readings(chan, data->readings, &data->cfg, val); } int icm42688_sample_fetch(const struct device *dev, enum sensor_channel chan) @@ -165,7 +165,7 @@ static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan, enum sensor_attribute attr, const struct sensor_value *val) { const struct icm42688_sensor_data *data = dev->data; - struct icm42688_cfg new_config = data->dev_data.cfg; + struct icm42688_cfg new_config = data->cfg; int res = 0; __ASSERT_NO_MSG(val != NULL); @@ -213,7 +213,7 @@ static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan, enum sensor_attribute attr, struct sensor_value *val) { const struct icm42688_sensor_data *data = dev->data; - const struct icm42688_cfg *cfg = &data->dev_data.cfg; + const struct icm42688_cfg *cfg = &data->cfg; int res = 0; __ASSERT_NO_MSG(val != NULL); @@ -288,22 +288,17 @@ int icm42688_init(const struct device *dev) LOG_ERR("Failed to initialize triggers"); return res; } - - res = icm42688_trigger_enable_interrupt(dev); - if (res != 0) { - LOG_ERR("Failed to enable triggers"); - return res; - } #endif - data->dev_data.cfg.accel_mode = ICM42688_ACCEL_LN; - data->dev_data.cfg.gyro_mode = ICM42688_GYRO_LN; - data->dev_data.cfg.accel_fs = ICM42688_ACCEL_FS_2G; - data->dev_data.cfg.gyro_fs = ICM42688_GYRO_FS_125; - data->dev_data.cfg.accel_odr = ICM42688_ACCEL_ODR_1000; - data->dev_data.cfg.gyro_odr = ICM42688_GYRO_ODR_1000; + data->cfg.accel_mode = ICM42688_ACCEL_LN; + data->cfg.gyro_mode = ICM42688_GYRO_LN; + data->cfg.accel_fs = ICM42688_ACCEL_FS_2G; + 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 = IS_ENABLED(CONFIG_ICM42688_STREAM); - res = icm42688_configure(dev, &data->dev_data.cfg); + res = icm42688_configure(dev, &data->cfg); if (res != 0) { LOG_ERR("Failed to configure"); return res; @@ -327,8 +322,21 @@ 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) \ + IF_ENABLED(CONFIG_ICM42688_STREAM, (ICM42688_RTIO_DEFINE(inst))); \ + static struct icm42688_sensor_data icm42688_driver_##inst = { \ + IF_ENABLED(CONFIG_ICM42688_STREAM, ( \ + .r = &icm42688_rtio_##inst, \ + .spi_iodev = &icm42688_spi_iodev_##inst, \ + )) \ + }; + #define ICM42688_INIT(inst) \ - static struct icm42688_sensor_data icm42688_driver_##inst = {0}; \ + ICM42688_DEFINE_DATA(inst); \ \ static const struct icm42688_sensor_config icm42688_cfg_##inst = { \ .dev_cfg = \ diff --git a/drivers/sensor/icm42688/icm42688.h b/drivers/sensor/icm42688/icm42688.h index e31de735adfe8d..0c8b3f4632221a 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 { @@ -392,10 +395,7 @@ struct icm42688_trigger_entry { sensor_trigger_handler_t handler; }; -/** - * @brief Device data (struct device) - */ -struct icm42688_dev_data { +struct icm42688_sensor_data { struct icm42688_cfg cfg; #ifdef CONFIG_ICM42688_TRIGGER #if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD) @@ -405,16 +405,20 @@ 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; + atomic_t reading_fifo; +#endif /* CONFIG_ICM42688_STREAM */ const struct device *dev; struct gpio_callback gpio_cb; sensor_trigger_handler_t data_ready_handler; const struct sensor_trigger *data_ready_trigger; struct k_mutex mutex; #endif /* CONFIG_ICM42688_TRIGGER */ -}; - -struct icm42688_sensor_data { - struct icm42688_dev_data dev_data; int16_t readings[7]; }; diff --git a/drivers/sensor/icm42688/icm42688_common.c b/drivers/sensor/icm42688/icm42688_common.c index 6e82f22678bd3e..d48de56799c17e 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); @@ -71,7 +72,7 @@ int icm42688_reset(const struct device *dev) int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) { - struct icm42688_dev_data *dev_data = dev->data; + struct icm42688_sensor_data *dev_data = dev->data; const struct icm42688_dev_cfg *dev_cfg = dev->config; int res; @@ -173,8 +174,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; @@ -263,7 +268,7 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) int icm42688_safely_configure(const struct device *dev, struct icm42688_cfg *cfg) { - struct icm42688_dev_data *drv_data = dev->data; + struct icm42688_sensor_data *drv_data = dev->data; int ret = icm42688_configure(dev, cfg); if (ret == 0) { diff --git a/drivers/sensor/icm42688/icm42688_decoder.c b/drivers/sensor/icm42688/icm42688_decoder.c index 4e359294a0ccdb..24e1de22b740d4 100644 --- a/drivers/sensor/icm42688/icm42688_decoder.c +++ b/drivers/sensor/icm42688/icm42688_decoder.c @@ -11,6 +11,8 @@ #include LOG_MODULE_REGISTER(ICM42688_DECODER, CONFIG_SENSOR_LOG_LEVEL); +#define DT_DRV_COMPAT invensense_icm42688 + int icm42688_get_shift(enum sensor_channel chan) { /* TODO: May need fine tuning */ @@ -119,7 +121,6 @@ int icm42688_encode(const struct device *dev, const enum sensor_channel *const c const size_t num_channels, uint8_t *buf) { struct icm42688_sensor_data *data = dev->data; - struct icm42688_dev_data *dev_data = &data->dev_data; struct icm42688_encoded_data *edata = (struct icm42688_encoded_data *)buf; edata->channels = 0; @@ -128,8 +129,9 @@ int icm42688_encode(const struct device *dev, const enum sensor_channel *const c edata->channels |= icm42688_encode_channel(channels[i]); } - edata->accelerometer_scale = dev_data->cfg.accel_fs; - edata->gyroscope_scale = dev_data->cfg.gyro_fs; + edata->accelerometer_scale = data->cfg.accel_fs; + edata->gyroscope_scale = data->cfg.gyro_fs; + edata->timestamp = k_ticks_to_ns_floor64(k_uptime_ticks()); return 0; } @@ -140,7 +142,7 @@ static int calc_num_samples(uint8_t channels_read) int num_samples = 0; while (channels_read) { - chan = __builtin_clz(channels_read); + chan = __builtin_ctz(channels_read); num_samples += SENSOR_CHANNEL_3_AXIS(chan) ? 3 : 1; channels_read &= ~BIT(chan); } @@ -148,9 +150,9 @@ static int calc_num_samples(uint8_t channels_read) return num_samples; } -static int 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) +static int icm42688_decoder_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) { struct icm42688_encoded_data *edata = (struct icm42688_encoded_data *)buffer; uint8_t channels_read = edata->channels; @@ -158,51 +160,136 @@ static int decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, int chan; int pos; int count = 0; - int num_samples = calc_num_samples(channels_read); + int num_samples = __builtin_popcount(channels_read); cfg.accel_fs = edata->accelerometer_scale; cfg.gyro_fs = edata->gyroscope_scale; channels_read = edata->channels; + if (*fit != 0) { + return 0; + } + /* Skip channels already decoded */ - for(int i = 0; i < *cit && channels_read; i++) { - chan = __builtin_clz(channels_read); + for (int i = 0; i < *cit && channels_read; i++) { + chan = __builtin_ctz(channels_read); channels_read &= ~BIT(chan); } /* Decode remaining channels */ while (channels_read && *cit < num_samples && count < max_count) { - chan = __builtin_clz(channels_read); + chan = __builtin_ctz(channels_read); channels[count] = chan; pos = icm42688_get_channel_position(chan); icm42688_convert_raw_to_q31(&cfg, chan, edata->readings[pos], &values[count]); - count += SENSOR_CHANNEL_3_AXIS(chan) ? 3 : 1; + count++; channels_read &= ~BIT(chan); *cit += 1; } - if (*cit >= num_samples) { + if (*cit >= __builtin_popcount(edata->channels)) { *fit += 1; *cit = 0; } + return count; +} + +static int icm42688_decoder_get_frame_count(const uint8_t *buffer, uint16_t *frame_count) +{ + *frame_count = 1; return 0; } -struct sensor_decoder_api icm42688_decoder = { - .get_frame_count = NULL, - .get_timestamp = NULL, - .get_shift = NULL, - .decode = decode, +static int icm42688_decoder_get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns) +{ + const struct icm42688_encoded_data *edata = (const struct icm42688_encoded_data *)buffer; + + *timestamp_ns = edata->timestamp; + return 0; +} + +static int icm42688_decoder_get_shift(const uint8_t *buffer, enum sensor_channel channel_type, + int8_t *shift) +{ + const struct icm42688_encoded_data *edata = (const struct icm42688_encoded_data *)buffer; + + switch (channel_type) { + case SENSOR_CHAN_ACCEL_XYZ: + case SENSOR_CHAN_ACCEL_X: + case SENSOR_CHAN_ACCEL_Y: + case SENSOR_CHAN_ACCEL_Z: + switch (edata->accelerometer_scale) { + case ICM42688_ACCEL_FS_2G: + *shift = 1; + return 0; + case ICM42688_ACCEL_FS_4G: + *shift = 2; + return 0; + case ICM42688_ACCEL_FS_8G: + *shift = 3; + return 0; + case ICM42688_ACCEL_FS_16G: + *shift = 4; + return 0; + default: + return -EINVAL; + } + case SENSOR_CHAN_GYRO_XYZ: + case SENSOR_CHAN_GYRO_X: + case SENSOR_CHAN_GYRO_Y: + case SENSOR_CHAN_GYRO_Z: + switch (edata->gyroscope_scale) { + case ICM42688_GYRO_FS_15_625: + *shift = 4; + return 0; + case ICM42688_GYRO_FS_31_25: + *shift = 5; + return 0; + case ICM42688_GYRO_FS_62_5: + *shift = 6; + return 0; + case ICM42688_GYRO_FS_125: + *shift = 7; + return 0; + case ICM42688_GYRO_FS_250: + *shift = 8; + return 0; + case ICM42688_GYRO_FS_500: + *shift = 9; + return 0; + case ICM42688_GYRO_FS_1000: + *shift = 10; + return 0; + case ICM42688_GYRO_FS_2000: + *shift = 11; + return 0; + default: + return -EINVAL; + } + case SENSOR_CHAN_DIE_TEMP: + *shift = 0; + return 0; + default: + return -EINVAL; + } +} + +SENSOR_DECODER_DT_DEFINE() = { + .get_frame_count = icm42688_decoder_get_frame_count, + .get_timestamp = icm42688_decoder_get_timestamp, + .get_shift = icm42688_decoder_get_shift, + .decode = icm42688_decoder_decode, }; -int icm42688_get_decoder(const struct device *dev, struct sensor_decoder_api *decoder) + int icm42688_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder) { - *decoder = icm42688_decoder; + ARG_UNUSED(dev); + *decoder = &SENSOR_DECODER_NAME(); return 0; -} + } diff --git a/drivers/sensor/icm42688/icm42688_decoder.h b/drivers/sensor/icm42688/icm42688_decoder.h index 466d73d49272bc..b2699c7b60c4f8 100644 --- a/drivers/sensor/icm42688/icm42688_decoder.h +++ b/drivers/sensor/icm42688/icm42688_decoder.h @@ -17,12 +17,13 @@ struct icm42688_encoded_data { uint16_t accelerometer_scale: 2; uint16_t gyroscope_scale: 3; uint16_t reserved: 4; + uint64_t timestamp; int16_t readings[7]; }; int icm42688_encode(const struct device *dev, const enum sensor_channel *const channels, const size_t num_channels, uint8_t *buf); -int icm42688_get_decoder(const struct device *dev, struct sensor_decoder_api *decoder); +int icm42688_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder); #endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_DECODER_H_ */ diff --git a/drivers/sensor/icm42688/icm42688_rtio.c b/drivers/sensor/icm42688/icm42688_rtio.c index 90399547231f54..d8197196c833b9 100644 --- a/drivers/sensor/icm42688/icm42688_rtio.c +++ b/drivers/sensor/icm42688/icm42688_rtio.c @@ -47,12 +47,12 @@ 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 struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; const enum sensor_channel *const channels = cfg->channels; - const size_t num_channels = cfg->num_channels; - int num_output_samples = 7; /* compute_num_samples(channels, num_channels);*/ + const size_t num_channels = cfg->count; + int num_output_samples = 7; uint32_t min_buf_len = compute_min_buf_len(num_channels, num_output_samples); int rc; uint8_t *buf; @@ -83,3 +83,230 @@ 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_sensor_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]) { + 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]); + 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; +} + +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); + +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; + struct rtio_iodev_sqe *iodev_sqe = sqe->userdata; + + /* TODO report number of frames or bytes read here */ + rtio_iodev_sqe_ok(iodev_sqe, drv_data->fifo_count); + + 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; + 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->dev_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 fifo_header) + packet_size; + const size_t ideal_read_size = sizeof(struct fifo_header) + 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; + } + + /* 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->cfg.gyro_odr, + .gyro_fs = drv_data->cfg.gyro_fs, + .accel_odr = drv_data->cfg.accel_odr, + .accel_fs = drv_data->cfg.accel_fs, + .packet_format = 0, + }; + 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; + + __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 void icm42688_int_status_cb(struct rtio *r, const struct rtio_sqe *sqr, void *arg) +{ + const struct device *dev = arg; + struct icm42688_sensor_data *drv_data = dev->data; + const struct icm42688_sensor_config *drv_cfg = dev->config; + struct rtio_iodev *spi_iodev = drv_data->spi_iodev; + + if (FIELD_GET(BIT_INT_STATUS_FIFO_THS, drv_data->int_status) == 0 && + FIELD_GET(BIT_INT_STATUS_FIFO_FULL, drv_data->int_status) == 0) { + gpio_pin_interrupt_configure_dt(&drv_cfg->dev_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); + + 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_sensor_data *drv_data = dev->data; + struct rtio_iodev *spi_iodev = drv_data->spi_iodev; + struct rtio *r = drv_data->r; + + /* + * 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 748c8a022b0923..9b236aa41f73e2 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 01e555a3f82f0c..24eb8095418de8 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" @@ -20,7 +21,7 @@ LOG_MODULE_DECLARE(ICM42688, CONFIG_SENSOR_LOG_LEVEL); 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); + struct icm42688_sensor_data *data = CONTAINER_OF(cb, struct icm42688_sensor_data, gpio_cb); ARG_UNUSED(dev); ARG_UNUSED(pins); @@ -30,11 +31,15 @@ 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); + } } +#ifdef CONFIG_ICM42688_TRIGGER_OWN_THREAD static void icm42688_thread_cb(const struct device *dev) { - struct icm42688_dev_data *data = dev->data; + struct icm42688_sensor_data *data = dev->data; const struct icm42688_dev_cfg *cfg = dev->config; icm42688_lock(dev); @@ -50,14 +55,12 @@ static void icm42688_thread_cb(const struct device *dev) icm42688_unlock(dev); } -#if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD) - static void icm42688_thread(void *p1, void *p2, void *p3) { ARG_UNUSED(p2); ARG_UNUSED(p3); - struct icm42688_dev_data *data = p1; + struct icm42688_sensor_data *data = p1; while (1) { k_sem_take(&data->gpio_sem, K_FOREVER); @@ -69,7 +72,7 @@ static void icm42688_thread(void *p1, void *p2, void *p3) static void icm42688_work_handler(struct k_work *work) { - struct icm42688_dev_data *data = CONTAINER_OF(work, struct icm42688_dev_data, work); + struct icm42688_sensor_data *data = CONTAINER_OF(work, struct icm42688_sensor_data, work); icm42688_thread_cb(data->dev); } @@ -79,7 +82,7 @@ static void icm42688_work_handler(struct k_work *work) int icm42688_trigger_set(const struct device *dev, const struct sensor_trigger *trig, sensor_trigger_handler_t handler) { - struct icm42688_dev_data *data = dev->data; + struct icm42688_sensor_data *data = dev->data; const struct icm42688_dev_cfg *cfg = dev->config; int res = 0; @@ -92,6 +95,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; break; @@ -108,7 +113,7 @@ int icm42688_trigger_set(const struct device *dev, const struct sensor_trigger * int icm42688_trigger_init(const struct device *dev) { - struct icm42688_dev_data *data = dev->data; + struct icm42688_sensor_data *data = dev->data; const struct icm42688_dev_cfg *cfg = dev->config; int res = 0; @@ -144,10 +149,11 @@ 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; + struct icm42688_sensor_data *data = dev->data; /* pulse-mode (auto clearing), push-pull and active-high */ res = icm42688_spi_single_write(&cfg->spi, REG_INT_CONFIG, @@ -162,21 +168,31 @@ 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) { - struct icm42688_dev_data *data = dev->data; + struct icm42688_sensor_data *data = dev->data; k_mutex_lock(&data->mutex, K_FOREVER); } void icm42688_unlock(const struct device *dev) { - struct icm42688_dev_data *data = dev->data; + struct icm42688_sensor_data *data = dev->data; k_mutex_unlock(&data->mutex); } 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 diff --git a/drivers/sensor/sensor_decoders_init.c b/drivers/sensor/sensor_decoders_init.c new file mode 100644 index 00000000000000..ad4086d1e077bf --- /dev/null +++ b/drivers/sensor/sensor_decoders_init.c @@ -0,0 +1,19 @@ +/* + * Copyright (c) 2023 Google LLC. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +static int sensor_decoders_init(void) +{ + STRUCT_SECTION_FOREACH(sensor_decoder_api, api) + { + k_object_access_all_grant(api); + } + return 0; +} + +SYS_INIT(sensor_decoders_init, POST_KERNEL, 99); diff --git a/drivers/sensor/sensor_handlers.c b/drivers/sensor/sensor_handlers.c index ccca6c882612a6..ebaa249c717aa8 100644 --- a/drivers/sensor/sensor_handlers.c +++ b/drivers/sensor/sensor_handlers.c @@ -59,7 +59,7 @@ static inline int z_vrfy_sensor_channel_get(const struct device *dev, #include static inline int z_vrfy_sensor_get_decoder(const struct device *dev, - struct sensor_decoder_api *decoder) + const struct sensor_decoder_api **decoder) { struct sensor_decoder_api k_decoder; int rc; diff --git a/drivers/sensor/sensor_shell.c b/drivers/sensor/sensor_shell.c index c49c5cd6af3806..10971176cfcb41 100644 --- a/drivers/sensor/sensor_shell.c +++ b/drivers/sensor/sensor_shell.c @@ -11,7 +11,6 @@ #include #include #include -#include #include #define SENSOR_GET_HELP \ @@ -19,6 +18,11 @@ "when no channels are provided. Syntax:\n" \ " .. " +#define SENSOR_STREAM_HELP \ + "Start/stop streaming sensor data. Data ready trigger will be used if no triggers " \ + "are provided. Syntax:\n" \ + " on|off .. " + #define SENSOR_ATTR_GET_HELP \ "Get the sensor's channel attribute. Syntax:\n" \ " [ .. " \ @@ -108,10 +112,26 @@ static const char *sensor_attribute_name[SENSOR_ATTR_COMMON_COUNT] = { [SENSOR_ATTR_FF_DUR] = "ff_dur", }; +static const char *sensor_trigger_type_name[SENSOR_TRIG_COMMON_COUNT] = { + [SENSOR_TRIG_TIMER] = "timer", + [SENSOR_TRIG_DATA_READY] = "data_ready", + [SENSOR_TRIG_DELTA] = "delta", + [SENSOR_TRIG_NEAR_FAR] = "near_far", + [SENSOR_TRIG_THRESHOLD] = "threshold", + [SENSOR_TRIG_TAP] = "tap", + [SENSOR_TRIG_DOUBLE_TAP] = "double_tap", + [SENSOR_TRIG_FREEFALL] = "freefall", + [SENSOR_TRIG_MOTION] = "motion", + [SENSOR_TRIG_STATIONARY] = "stationary", + [SENSOR_TRIG_FIFO_THRESHOLD] = "fifo_thres", + [SENSOR_TRIG_FIFO_FULL] = "fifo_full", +}; + enum dynamic_command_context { NONE, CTX_GET, CTX_ATTR_GET_SET, + CTX_STREAM_ON_OFF, }; static enum dynamic_command_context current_cmd_ctx = NONE; @@ -120,15 +140,27 @@ static enum dynamic_command_context current_cmd_ctx = NONE; static enum sensor_channel iodev_sensor_shell_channels[SENSOR_CHAN_ALL]; static struct sensor_read_config iodev_sensor_shell_read_config = { .sensor = NULL, + .is_streaming = false, .channels = iodev_sensor_shell_channels, - .num_channels = 0, - .max_channels = ARRAY_SIZE(iodev_sensor_shell_channels), + .count = 0, + .max = ARRAY_SIZE(iodev_sensor_shell_channels), }; RTIO_IODEV_DEFINE(iodev_sensor_shell_read, &__sensor_iodev_api, &iodev_sensor_shell_read_config); +/* Create a single common config for streaming */ +static enum sensor_trigger_type iodev_sensor_shell_trigger_types[SENSOR_TRIG_COMMON_COUNT]; +static struct sensor_read_config iodev_sensor_shell_stream_config = { + .sensor = NULL, + .is_streaming = true, + .triggers = iodev_sensor_shell_trigger_types, + .count = 0, + .max = ARRAY_SIZE(iodev_sensor_shell_trigger_types), +}; +RTIO_IODEV_DEFINE(iodev_sensor_shell_stream, &__sensor_iodev_api, + &iodev_sensor_shell_stream_config); + /* Create the RTIO context to service the reading */ -RTIO_EXECUTOR_SIMPLE_DEFINE(sensor_shell_executor); -RTIO_DEFINE_WITH_MEMPOOL(sensor_read_rtio, &sensor_shell_executor, 1, 1, 8, 64, 4); +RTIO_DEFINE_WITH_MEMPOOL(sensor_read_rtio, 8, 8, 32, 64, 4); static int parse_named_int(const char *name, const char *heystack[], size_t count) { @@ -201,7 +233,7 @@ static void sensor_shell_processing_callback(int result, uint8_t *buf, uint32_t void *userdata) { struct sensor_shell_processing_context *ctx = userdata; - struct sensor_decoder_api decoder; + const struct sensor_decoder_api *decoder; sensor_frame_iterator_t fit = {0}; sensor_channel_iterator_t cit = {0}; uint64_t timestamp; @@ -211,7 +243,7 @@ static void sensor_shell_processing_callback(int result, uint8_t *buf, uint32_t ARG_UNUSED(buf_len); - if (result != 0) { + if (result < 0) { shell_error(ctx->sh, "Read failed"); return; } @@ -222,17 +254,17 @@ static void sensor_shell_processing_callback(int result, uint8_t *buf, uint32_t return; } - rc = decoder.get_timestamp(buf, ×tamp); + rc = decoder->get_timestamp(buf, ×tamp); if (rc != 0) { shell_error(ctx->sh, "Failed to get fetch timestamp for '%s'", ctx->dev->name); return; } shell_print(ctx->sh, "Got samples at %" PRIu64 " ns", timestamp); - while (decoder.decode(buf, &fit, &cit, &channel, &q, 1) > 0) { + while (decoder->decode(buf, &fit, &cit, &channel, &q, 1) > 0) { int8_t shift; - rc = decoder.get_shift(buf, channel, &shift); + rc = decoder->get_shift(buf, channel, &shift); if (rc != 0) { shell_error(ctx->sh, "Failed to get bitshift for channel %d", channel); continue; @@ -261,6 +293,7 @@ static void sensor_shell_processing_callback(int result, uint8_t *buf, uint32_t static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[]) { + static struct sensor_shell_processing_context ctx; const struct device *dev; int err; @@ -275,11 +308,11 @@ static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[]) for (int i = 0; i < ARRAY_SIZE(iodev_sensor_shell_channels); ++i) { iodev_sensor_shell_channels[i] = i; } - iodev_sensor_shell_read_config.num_channels = - iodev_sensor_shell_read_config.max_channels; + iodev_sensor_shell_read_config.count = + iodev_sensor_shell_read_config.max; } else { /* read specific channels */ - iodev_sensor_shell_read_config.num_channels = 0; + iodev_sensor_shell_read_config.count = 0; for (int i = 2; i < argc; ++i) { int chan = parse_named_int(argv[i], sensor_channel_name, ARRAY_SIZE(sensor_channel_name)); @@ -288,31 +321,42 @@ static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[]) shell_error(sh, "Failed to read channel (%s)", argv[i]); continue; } - iodev_sensor_shell_channels[iodev_sensor_shell_read_config.num_channels++] = + iodev_sensor_shell_channels[iodev_sensor_shell_read_config.count++] = chan; } } /* TODO(yperess) use sensor_reconfigure_read_iodev? */ - if (iodev_sensor_shell_read_config.num_channels == 0) { + if (iodev_sensor_shell_read_config.count == 0) { shell_error(sh, "No channels to read, bailing"); return -EINVAL; } iodev_sensor_shell_read_config.sensor = dev; - struct sensor_shell_processing_context ctx = { - .dev = dev, - .sh = sh, - }; + ctx.dev = dev; + ctx.sh = sh; err = sensor_read(&iodev_sensor_shell_read, &sensor_read_rtio, &ctx); if (err < 0) { shell_error(sh, "Failed to read sensor: %d", err); } - z_sensor_processing_loop(&sensor_read_rtio, sensor_shell_processing_callback); +// z_sensor_processing_loop(&sensor_read_rtio, sensor_shell_processing_callback); return 0; } +static void sensor_shell_processing_entry_point(void *a, void *b, void *c) +{ + ARG_UNUSED(a); + ARG_UNUSED(b); + ARG_UNUSED(c); + + while (true) { + z_sensor_processing_loop(&sensor_read_rtio, sensor_shell_processing_callback); + } +} +K_THREAD_DEFINE(sensor_shell_processing_tid, CONFIG_SENSOR_SHELL_THREAD_STACK_SIZE, + sensor_shell_processing_entry_point, NULL, NULL, NULL, 0, 0, 0); + static int cmd_sensor_attr_set(const struct shell *shell_ptr, size_t argc, char *argv[]) { const struct device *dev; @@ -422,10 +466,94 @@ static int cmd_sensor_attr_get(const struct shell *shell_ptr, size_t argc, char return 0; } -static void channel_name_get(size_t idx, struct shell_static_entry *entry); +static int cmd_sensor_stream(const struct shell *shell_ptr, size_t argc, char *argv[]) +{ + static struct rtio_sqe *current_streaming_handle; + static struct sensor_shell_processing_context ctx; + const struct device *dev = device_get_binding(argv[1]); + + if (dev == NULL) { + shell_error(shell_ptr, "Device unknown (%s)", argv[1]); + return -ENODEV; + } + + if (current_streaming_handle != NULL) { + shell_print(shell_ptr, "Disabling existing stream"); + rtio_sqe_cancel(current_streaming_handle); + } + + if (strcmp("off", argv[2]) == 0) { + return 0; + } + + if (strcmp("on", argv[2]) != 0) { + shell_error(shell_ptr, "Unknown streaming operation (%s)", argv[2]); + return -EINVAL; + } + + shell_print(shell_ptr, "Enabling stream..."); + iodev_sensor_shell_stream_config.sensor = dev; + if (argc == 3) { + iodev_sensor_shell_stream_config.count = 1; + iodev_sensor_shell_trigger_types[0] = SENSOR_TRIG_DATA_READY; + } else { + iodev_sensor_shell_stream_config.count = argc - 3; + for (int i = 3; i < argc; ++i) { + int trigger = parse_named_int(argv[i], sensor_trigger_type_name, + ARRAY_SIZE(sensor_trigger_type_name)); + + if (trigger < 0) { + shell_error(shell_ptr, "Invalid trigger (%s)", argv[i]); + return -EINVAL; + } + iodev_sensor_shell_trigger_types[i - 3] = trigger; + } + } + + ctx.dev = dev; + ctx.sh = shell_ptr; + + int rc = sensor_stream(&iodev_sensor_shell_stream, &sensor_read_rtio, &ctx, + ¤t_streaming_handle); + + if (rc != 0) { + shell_error(shell_ptr, "Failed to start stream"); + } + return rc; +} +static void channel_name_get(size_t idx, struct shell_static_entry *entry); SHELL_DYNAMIC_CMD_CREATE(dsub_channel_name, channel_name_get); +static void attribute_name_get(size_t idx, struct shell_static_entry *entry); +SHELL_DYNAMIC_CMD_CREATE(dsub_attribute_name, attribute_name_get); + +static void channel_name_get(size_t idx, struct shell_static_entry *entry) +{ + int cnt = 0; + + entry->syntax = NULL; + entry->handler = NULL; + entry->help = NULL; + if (current_cmd_ctx == CTX_GET) { + entry->subcmd = &dsub_channel_name; + } else if (current_cmd_ctx == CTX_ATTR_GET_SET) { + entry->subcmd = &dsub_attribute_name; + } else { + entry->subcmd = NULL; + } + + for (int i = 0; i < SENSOR_CHAN_ALL; i++) { + if (sensor_channel_name[i] != NULL) { + if (cnt == idx) { + entry->syntax = sensor_channel_name[i]; + break; + } + cnt++; + } + } +} + static void attribute_name_get(size_t idx, struct shell_static_entry *entry) { int cnt = 0; @@ -445,27 +573,23 @@ static void attribute_name_get(size_t idx, struct shell_static_entry *entry) } } } -SHELL_DYNAMIC_CMD_CREATE(dsub_attribute_name, attribute_name_get); -static void channel_name_get(size_t idx, struct shell_static_entry *entry) + +static void trigger_name_get(size_t idx, struct shell_static_entry *entry); +SHELL_DYNAMIC_CMD_CREATE(dsub_trigger_name, trigger_name_get); +static void trigger_name_get(size_t idx, struct shell_static_entry *entry) { int cnt = 0; entry->syntax = NULL; entry->handler = NULL; entry->help = NULL; - if (current_cmd_ctx == CTX_GET) { - entry->subcmd = &dsub_channel_name; - } else if (current_cmd_ctx == CTX_ATTR_GET_SET) { - entry->subcmd = &dsub_attribute_name; - } else { - entry->subcmd = NULL; - } + entry->subcmd = &dsub_trigger_name; - for (int i = 0; i < SENSOR_CHAN_ALL; i++) { - if (sensor_channel_name[i] != NULL) { + for (int i = 0; i < SENSOR_TRIG_COMMON_COUNT; i++) { + if (sensor_trigger_type_name[i] != NULL) { if (cnt == idx) { - entry->syntax = sensor_channel_name[i]; + entry->syntax = sensor_trigger_type_name[i]; break; } cnt++; @@ -473,6 +597,22 @@ static void channel_name_get(size_t idx, struct shell_static_entry *entry) } } +static void stream_on_off(size_t idx, struct shell_static_entry *entry) +{ + entry->syntax = NULL; + entry->handler = NULL; + entry->help = NULL; + + if (idx == 0) { + entry->syntax = "on"; + entry->subcmd = &dsub_trigger_name; + } else if (idx == 1) { + entry->syntax = "off"; + entry->subcmd = NULL; + } +} +SHELL_DYNAMIC_CMD_CREATE(dsub_stream_on_off, stream_on_off); + static void device_name_get(size_t idx, struct shell_static_entry *entry); SHELL_DYNAMIC_CMD_CREATE(dsub_device_name, device_name_get); @@ -500,6 +640,18 @@ static void device_name_get_for_attr(size_t idx, struct shell_static_entry *entr } SHELL_DYNAMIC_CMD_CREATE(dsub_device_name_for_attr, device_name_get_for_attr); +static void device_name_get_for_trig(size_t idx, struct shell_static_entry *entry) +{ + const struct device *dev = shell_device_lookup(idx, NULL); + + current_cmd_ctx = CTX_STREAM_ON_OFF; + entry->syntax = (dev != NULL) ? dev->name : NULL; + entry->handler = NULL; + entry->help = NULL; + entry->subcmd = &dsub_stream_on_off; +} +SHELL_DYNAMIC_CMD_CREATE(dsub_device_name_for_trig, device_name_get_for_trig); + static int cmd_get_sensor_info(const struct shell *sh, size_t argc, char **argv) { ARG_UNUSED(argc); @@ -531,6 +683,8 @@ SHELL_STATIC_SUBCMD_SET_CREATE(sub_sensor, cmd_sensor_attr_set, 2, 255), SHELL_CMD_ARG(attr_get, &dsub_device_name_for_attr, SENSOR_ATTR_GET_HELP, cmd_sensor_attr_get, 2, 255), + SHELL_CMD_ARG(stream, &dsub_device_name_for_trig, SENSOR_STREAM_HELP, + cmd_sensor_stream, 2, 255), SHELL_COND_CMD(CONFIG_SENSOR_INFO, info, NULL, SENSOR_INFO_HELP, cmd_get_sensor_info), SHELL_SUBCMD_SET_END diff --git a/include/zephyr/drivers/sensor.h b/include/zephyr/drivers/sensor.h index 399bd549696fbe..6b66d3a0d77574 100644 --- a/include/zephyr/drivers/sensor.h +++ b/include/zephyr/drivers/sensor.h @@ -245,6 +245,9 @@ enum sensor_trigger_type { /** Trigger fires when no motion has been detected for a while. */ SENSOR_TRIG_STATIONARY, + + SENSOR_TRIG_FIFO_THRESHOLD, + SENSOR_TRIG_FIFO_FULL, /** * Number of all common sensor triggers. */ @@ -508,7 +511,7 @@ struct sensor_decoder_api { * @see sensor_get_decoder for more details */ typedef int (*sensor_get_decoder_t)(const struct device *dev, - struct sensor_decoder_api *api); + const struct sensor_decoder_api **api); /* * Internal data structure used to store information about the IODevice for async reading and @@ -516,9 +519,13 @@ typedef int (*sensor_get_decoder_t)(const struct device *dev, */ struct sensor_read_config { const struct device *sensor; - enum sensor_channel *const channels; - size_t num_channels; - const size_t max_channels; + const bool is_streaming; + union { + enum sensor_channel *const channels; + enum sensor_trigger_type *const triggers; + }; + size_t count; + const size_t max; }; /** @@ -539,12 +546,24 @@ struct sensor_read_config { static enum sensor_channel __channel_array_##name[] = {__VA_ARGS__}; \ static struct sensor_read_config __sensor_read_config_##name = { \ .sensor = DEVICE_DT_GET(dt_node), \ + .is_streaming = false, \ .channels = __channel_array_##name, \ - .num_channels = ARRAY_SIZE(__channel_array_##name), \ - .max_channels = ARRAY_SIZE(__channel_array_##name), \ + .count = ARRAY_SIZE(__channel_array_##name), \ + .max = ARRAY_SIZE(__channel_array_##name), \ }; \ RTIO_IODEV_DEFINE(name, &__sensor_iodev_api, &__sensor_read_config_##name) +#define SENSOR_DT_STREAM_IODEV(name, dt_node, ...) \ + static enum sensor_trigger_type __trigger_array_##name[] = {__VA_ARGS__}; \ + static struct sensor_read_config __sensor_read_config_##name = { \ + .sensor = DEVICE_DT_GET(dt_node), \ + .is_streaming = true, \ + .triggers = __trigger_array_##name, \ + .count = ARRAY_SIZE(__trigger_array_##name), \ + .max = ARRAY_SIZE(__trigger_array_##name), \ + }; \ + RTIO_IODEV_DEFINE(name &__sensor_iodev_api, &__sensor_read_config_##name) + /* Used to submit an RTIO sqe to the sensor's iodev */ typedef int (*sensor_submit_t)(const struct device *sensor, struct rtio_iodev_sqe *sqe); @@ -799,17 +818,17 @@ struct __attribute__((__packed__)) sensor_data_generic_header { * @return < 0 on error */ __syscall int sensor_get_decoder(const struct device *dev, - struct sensor_decoder_api *decoder); + const struct sensor_decoder_api **decoder); static inline int z_impl_sensor_get_decoder(const struct device *dev, - struct sensor_decoder_api *decoder) + const struct sensor_decoder_api **decoder) { const struct sensor_driver_api *api = (const struct sensor_driver_api *)dev->api; __ASSERT_NO_MSG(api != NULL); if (api->get_decoder == NULL) { - *decoder = __sensor_default_decoder; + *decoder = &__sensor_default_decoder; return 0; } @@ -845,13 +864,36 @@ static inline int z_impl_sensor_reconfigure_read_iodev(struct rtio_iodev *iodev, { struct sensor_read_config *cfg = (struct sensor_read_config *)iodev->data; - if (cfg->max_channels < num_channels) { + if (cfg->max < num_channels || cfg->is_streaming) { return -ENOMEM; } cfg->sensor = sensor; memcpy(cfg->channels, channels, num_channels * sizeof(enum sensor_channel)); - cfg->num_channels = num_channels; + cfg->count = num_channels; + return 0; +} + +static inline int sensor_stream(struct rtio_iodev *iodev, struct rtio *ctx, void *userdata, + struct rtio_sqe **handle) +{ + if (IS_ENABLED(CONFIG_USERSPACE)) { + struct rtio_sqe sqe; + + rtio_sqe_prep_read_multishot(&sqe, iodev, RTIO_PRIO_NORM, userdata); + rtio_sqe_copy_in_get_handles(ctx, &sqe, handle, 1); + } else { + struct rtio_sqe *sqe = rtio_sqe_acquire(ctx); + + if (sqe == NULL) { + return -ENOMEM; + } + if (handle != NULL) { + *handle = sqe; + } + rtio_sqe_prep_read_multishot(sqe, iodev, RTIO_PRIO_NORM, userdata); + } + rtio_submit(ctx, 0); return 0; } @@ -873,7 +915,7 @@ static inline int sensor_read(struct rtio_iodev *iodev, struct rtio *ctx, void * if (IS_ENABLED(CONFIG_USERSPACE)) { struct rtio_sqe sqe; - rtio_sqe_prep_read_with_pool(&sqe, iodev, 0, userdata); + rtio_sqe_prep_read_with_pool(&sqe, iodev, RTIO_PRIO_NORM, userdata); rtio_sqe_copy_in(ctx, &sqe, 1); } else { struct rtio_sqe *sqe = rtio_sqe_acquire(ctx); @@ -881,7 +923,7 @@ static inline int sensor_read(struct rtio_iodev *iodev, struct rtio *ctx, void * if (sqe == NULL) { return -ENOMEM; } - rtio_sqe_prep_read_with_pool(sqe, iodev, 0, userdata); + rtio_sqe_prep_read_with_pool(sqe, iodev, RTIO_PRIO_NORM, userdata); } rtio_submit(ctx, 0); return 0; @@ -1162,6 +1204,24 @@ struct sensor_info { #define SENSOR_DEVICE_DT_INST_DEFINE(inst, ...) \ SENSOR_DEVICE_DT_DEFINE(DT_DRV_INST(inst), __VA_ARGS__) +#define SENSOR_DECODER_NAME() UTIL_CAT(DT_DRV_COMPAT, __decoder_api) + +#define SENSOR_DECODER_DT_DEFINE() \ + COND_CODE_1(DT_HAS_COMPAT_STATUS_OKAY(DT_DRV_COMPAT), (), (static)) \ + const STRUCT_SECTION_ITERABLE(sensor_decoder_api, SENSOR_DECODER_NAME()) + +#define Z_MAYBE_SENSOR_DECODER_DECLARE_INTERNAL_IDX(node_id, prop, idx) \ + extern const struct sensor_decoder_api UTIL_CAT( \ + DT_STRING_TOKEN_BY_IDX(node_id, prop, idx), __decoder_api); + +#define Z_MAYBE_SENSOR_DECODER_DECLARE_INTERNAL(node_id) \ + COND_CODE_1(DT_NODE_HAS_PROP(node_id, compatible), \ + (DT_FOREACH_PROP_ELEM(node_id, compatible, \ + Z_MAYBE_SENSOR_DECODER_DECLARE_INTERNAL_IDX)), \ + ()) + +DT_FOREACH_STATUS_OKAY_NODE(Z_MAYBE_SENSOR_DECODER_DECLARE_INTERNAL) + /** * @brief Helper function for converting struct sensor_value to integer milli units. * diff --git a/samples/sensor/sensor_shell/prj.conf b/samples/sensor/sensor_shell/prj.conf index 9f5cc48698e9e4..b9c823bc9cddf5 100644 --- a/samples/sensor/sensor_shell/prj.conf +++ b/samples/sensor/sensor_shell/prj.conf @@ -6,5 +6,5 @@ CONFIG_SENSOR_INFO=y CONFIG_LOG=y CONFIG_ICM42688_TRIGGER_NONE=y -CONFIG_ADC_ADS7052_INIT_PRIORITY=99 +# CONFIG_ADC_ADS7052_INIT_PRIORITY=99 CONFIG_RTIO_CONSUME_SEM=y diff --git a/samples/sensor/sensor_shell/src/main.c b/samples/sensor/sensor_shell/src/main.c index 235cc58982cd4f..8eae5d4198cfad 100644 --- a/samples/sensor/sensor_shell/src/main.c +++ b/samples/sensor/sensor_shell/src/main.c @@ -76,13 +76,5 @@ static void data_ready_trigger_handler(const struct device *sensor, int main(void) { - STRUCT_SECTION_FOREACH(sensor_info, sensor) - { - struct sensor_trigger trigger = { - .chan = SENSOR_CHAN_ALL, - .type = SENSOR_TRIG_DATA_READY, - }; - sensor_trigger_set(sensor->dev, &trigger, data_ready_trigger_handler); - } return 0; } diff --git a/scripts/build/gen_kobject_list.py b/scripts/build/gen_kobject_list.py index fd716bfef38310..02cbcc633cef3d 100755 --- a/scripts/build/gen_kobject_list.py +++ b/scripts/build/gen_kobject_list.py @@ -113,7 +113,8 @@ ("ztest_unit_test", ("CONFIG_ZTEST_NEW_API", True, False)), ("ztest_test_rule", ("CONFIG_ZTEST_NEW_API", True, False)), ("rtio", ("CONFIG_RTIO", False, False)), - ("rtio_iodev", ("CONFIG_RTIO", False, False)) + ("rtio_iodev", ("CONFIG_RTIO", False, False)), + ("sensor_decoder_api", ("CONFIG_SENSOR", True, False)) ]) def kobject_to_enum(kobj):