icm42688: Implement streaming APIs

Add streaming implementation for icm42688 using both threshold and full FIFO triggers. Signed-off-by: Yuval Peress <peress@google.com> topic#sensor_stream
zephyrproject-rtos · Nov 3, 2023 · a4e6294 · a4e6294
1 parent 552c078
commit a4e6294
Show file tree

Hide file tree

Showing 13 changed files with 773 additions and 29 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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);
@@ -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;