sensors: Add streaming APIs

drivers/sensor/CMakeLists.txt

@@ -161,5 +161,6 @@ zephyr_library_property(ALLOW_EMPTY TRUE)
 
 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_STREAM sensor_shell_stream.c)
 zephyr_library_sources_ifdef(CONFIG_SENSOR_SHELL_BATTERY shell_battery.c)
 zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API sensor_decoders_init.c default_rtio_sensor.c)

drivers/sensor/Kconfig

@@ -36,6 +36,23 @@ config SENSOR_SHELL
 	help
 	  This shell provides access to basic sensor data.
 
+config SENSOR_SHELL_STREAM
+	bool "Sensor shell 'stream' command"
+	depends on SENSOR_SHELL
+	help
+	  Add the 'stream' subcommand to the sensor shell. When run on drivers that
+	  support streaming (usually hardware FIFO backed), the shell will continue
+	  to print new values as they come until the stream is closed.
+
+config SENSOR_SHELL_THREAD_STACK_SIZE
+	int "Stack size for the sensor shell data processing thread"
+	depends on SENSOR_SHELL_STREAM
+	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

drivers/sensor/default_rtio_sensor.c

@@ -28,8 +28,10 @@ static void sensor_iodev_submit(struct rtio_iodev_sqe *iodev_sqe)
 
 	if (api->submit != NULL) {
 		api->submit(dev, iodev_sqe);
-	} else {
+	} else if (!cfg->is_streaming) {
 		sensor_submit_fallback(dev, iodev_sqe);
+	} else {
+		rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
 	}
 }
 
@@ -235,7 +237,7 @@ static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_s
 		}
 		sample_idx += num_samples;
 	}
-	LOG_DBG("Total channels in header: %u", header->num_channels);
+	LOG_DBG("Total channels in header: %" PRIu32, header->num_channels);
 	rtio_iodev_sqe_ok(iodev_sqe, 0);
 }
 

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 .)

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
 

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,17 @@ static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan,
 			res = -EINVAL;
 		}
 		break;
+	case SENSOR_CHAN_ALL:
+		if (attr == SENSOR_ATTR_BATCH_DURATION) {
+			if (val->val1 < 0) {
+				return -EINVAL;
+			}
+			new_config.batch_ticks = val->val1;
+		} else {
+			LOG_ERR("Unsupported attribute");
+			res = -EINVAL;
+		}
+		break;
 	default:
 		LOG_ERR("Unsupported channel");
 		res = -EINVAL;
@@ -204,6 +215,15 @@ static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan,
 			res = -EINVAL;
 		}
 		break;
+	case SENSOR_CHAN_ALL:
+		if (attr == SENSOR_ATTR_BATCH_DURATION) {
+			val->val1 = cfg->batch_ticks;
+			val->val2 = 0;
+		} else {
+			LOG_ERR("Unsupported attribute");
+			res = -EINVAL;
+		}
+		break;
 	default:
 		LOG_ERR("Unsupported channel");
 		res = -EINVAL;
@@ -257,7 +277,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.batch_ticks = 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 +309,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);                                                                \

drivers/sensor/icm42688/icm42688.h

@@ -380,11 +380,14 @@ struct icm42688_cfg {
 	/* TODO timestamp options */
 
 	bool fifo_en;
-	uint16_t fifo_wm;
+	int32_t batch_ticks;
 	bool fifo_hires;
 	/* 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;

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 <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(ICM42688_LL, CONFIG_SENSOR_LOG_LEVEL);
@@ -61,6 +62,62 @@ int icm42688_reset(const struct device *dev)
 	return 0;
 }
 
+static uint16_t icm42688_compute_fifo_wm(const struct icm42688_cfg *cfg)
+{
+	const bool accel_enabled = cfg->accel_mode != ICM42688_ACCEL_OFF;
+	const bool gyro_enabled = cfg->gyro_mode != ICM42688_GYRO_OFF;
+	const int pkt_size = cfg->fifo_hires ? 20 : (accel_enabled && gyro_enabled ? 16 : 8);
+	int accel_modr = 0;
+	int gyro_modr = 0;
+	int64_t modr;
+
+	if (cfg->batch_ticks == 0 || (!accel_enabled && !gyro_enabled)) {
+		return 0;
+	}
+
+	if (accel_enabled) {
+		struct sensor_value val = {0};
+
+		icm42688_accel_reg_to_hz(cfg->accel_odr, &val);
+		accel_modr = sensor_value_to_micro(&val) / 1000;
+	}
+	if (gyro_enabled) {
+		struct sensor_value val = {0};
+
+		icm42688_gyro_reg_to_odr(cfg->gyro_odr, &val);
+		gyro_modr = sensor_value_to_micro(&val) / 1000;
+	}
+
+	if (accel_modr == 0) {
+		modr = gyro_modr;
+	} else if (gyro_modr == 0) {
+		modr = accel_modr;
+	} else {
+		/* Need to find the least common multiplier (LCM) */
+		int n1 = accel_modr;
+		int n2 = gyro_modr;
+
+		while (n1 != n2) {
+			if (n1 > n2) {
+				n1 -= n2;
+			} else {
+				n2 -= n1;
+			}
+		}
+		LOG_DBG("GCD=%d", n1);
+		modr = ((int64_t)accel_modr * (int64_t)gyro_modr) / n1;
+	}
+	/* At this point we have 'modr' as mHz which is 1 / msec. */
+
+	/* Convert 'modr' to bytes * batch_ticks / msec */
+	modr *= (int64_t)cfg->batch_ticks * pkt_size;
+
+	/* 'modr' = byte_ticks_per_msec / kticks_per_sec */
+	modr = DIV_ROUND_UP(modr, CONFIG_SYS_CLOCK_TICKS_PER_SEC * INT64_C(1000));
+
+	return (uint16_t)MIN(modr, 0x7ff);
+}
+
 int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg)
 {
 	struct icm42688_dev_data *dev_data = dev->data;
@@ -165,8 +222,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;
@@ -205,7 +266,8 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg)
 		}
 
 		/* Set watermark and interrupt handling first */
-		uint8_t fifo_wml = (cfg->fifo_wm) & 0xFF;
+		uint16_t fifo_wm = icm42688_compute_fifo_wm(cfg);
+		uint8_t fifo_wml = fifo_wm & 0xFF;
 
 		LOG_DBG("FIFO_CONFIG2( (0x%x)) (WM Low) 0x%x", REG_FIFO_CONFIG2, fifo_wml);
 		res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG2, fifo_wml);
@@ -214,7 +276,7 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg)
 			return -EINVAL;
 		}
 
-		uint8_t fifo_wmh = (cfg->fifo_wm >> 8) & 0x0F;
+		uint8_t fifo_wmh = (fifo_wm >> 8) & 0x0F;
 
 		LOG_DBG("FIFO_CONFIG3 (0x%x) (WM High) 0x%x", REG_FIFO_CONFIG3, fifo_wmh);
 		res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG3, fifo_wmh);