sensors: lsm6dsv16x: add RTIO async and fifo stream

Add RTIO async and RTIO stream functionalities that enables, among all the other things, the sensor data streaming from FIFO. RTIO stream is using both SENSOR_TRIG_FIFO_WATERMARK and SENSOR_TRIG_FIFO_FULL triggers. The decoder currently only supports following FIFO tags: - LSM6DSV16X_XL_NC_TAG - LSM6DSV16X_GY_NC_TAG - LSM6DSV16X_TEMP_NC_TAG Following FIFO parameters has to be defined in device tree to correctly stream sensor data: - fifo-watermark (defaults to 32) - accel-fifo-batch-rate (defaults to LSM6DSV16X_DT_XL_NOT_BATCHED) - gyro-fifo-batch-rate (defaults to LSM6DSV16X_DT_GY_NOT_BATCHED) - temp-fifo-batch-rate (defaults to LSM6DSV16X_DT_TEMP_NOT_BATCHED) Signed-off-by: Armando Visconti <armando.visconti@st.com>
zephyrproject-rtos · Nov 15, 2024 · 0c5e2cb · 0c5e2cb
1 parent 6c987e3
commit 0c5e2cb
Show file tree

Hide file tree

Showing 13 changed files with 1,389 additions and 37 deletions.
diff --git a/drivers/sensor/st/lsm6dsv16x/CMakeLists.txt b/drivers/sensor/st/lsm6dsv16x/CMakeLists.txt
@@ -9,5 +9,7 @@ zephyr_library()
 zephyr_library_sources(lsm6dsv16x.c)
 zephyr_library_sources_ifdef(CONFIG_LSM6DSV16X_SENSORHUB  lsm6dsv16x_shub.c)
 zephyr_library_sources_ifdef(CONFIG_LSM6DSV16X_TRIGGER    lsm6dsv16x_trigger.c)
+zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API lsm6dsv16x_rtio.c lsm6dsv16x_decoder.c)
+zephyr_library_sources_ifdef(CONFIG_LSM6DSV16X_STREAM lsm6dsv16x_rtio_stream.c)
 
 zephyr_library_include_directories(../stmemsc)
diff --git a/drivers/sensor/st/lsm6dsv16x/Kconfig b/drivers/sensor/st/lsm6dsv16x/Kconfig
@@ -12,12 +12,23 @@ menuconfig LSM6DSV16X
 	select SPI if $(dt_compat_on_bus,$(DT_COMPAT_ST_LSM6DSV16X),spi)
 	select HAS_STMEMSC
 	select USE_STDC_LSM6DSV16X
+	select RTIO_WORKQ if SENSOR_ASYNC_API
 	help
 	  Enable driver for LSM6DSV16X accelerometer and gyroscope
 	  sensor.
 
 if LSM6DSV16X
 
+config LSM6DSV16X_STREAM
+	bool "Use hardware FIFO to stream data"
+	select LSM6DSV16X_TRIGGER
+	default y
+	select SPI_RTIO if $(dt_compat_on_bus,$(DT_COMPAT_ST_LSM6DSV16X),spi)
+	select I2C_RTIO if $(dt_compat_on_bus,$(DT_COMPAT_ST_LSM6DSV16X),i2c)
+	depends on SENSOR_ASYNC_API
+	help
+	  Use this config option to enable streaming sensor data via RTIO subsystem.
+
 choice LSM6DSV16X_TRIGGER_MODE
 	prompt "Trigger mode"
 	help

diff --git a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.c b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.c
@@ -19,6 +19,8 @@
 #include <zephyr/logging/log.h>
 
 #include "lsm6dsv16x.h"
+#include "lsm6dsv16x_decoder.h"
+#include "lsm6dsv16x_rtio.h"
 
 LOG_MODULE_REGISTER(LSM6DSV16X, CONFIG_SENSOR_LOG_LEVEL);
 
@@ -86,6 +88,16 @@ static const uint16_t lsm6dsv16x_gyro_fs_map[] = {125, 250, 500, 1000, 2000, 0,
 						  0,   0,   0,   0,    0,    4000};
 static const uint16_t lsm6dsv16x_gyro_fs_sens[] = {1, 2, 4, 8, 16, 0, 0, 0, 0, 0, 0, 0, 32};
 
+int lsm6dsv16x_calc_accel_gain(uint8_t fs)
+{
+	return lsm6dsv16x_accel_fs_map[fs] * GAIN_UNIT_XL / 2;
+}
+
+int lsm6dsv16x_calc_gyro_gain(uint8_t fs)
+{
+	return lsm6dsv16x_gyro_fs_sens[fs] * GAIN_UNIT_G;
+}
+
 static int lsm6dsv16x_gyro_range_to_fs_val(int32_t range)
 {
 	size_t i;
@@ -205,7 +217,7 @@ static int lsm6dsv16x_accel_range_set(const struct device *dev, int32_t range)
 		return -EIO;
 	}
 
-	data->acc_gain = lsm6dsv16x_accel_fs_map[fs] * GAIN_UNIT_XL / 2;
+	data->acc_gain = lsm6dsv16x_calc_accel_gain(fs);
 	return 0;
 }
 
@@ -295,7 +307,7 @@ static int lsm6dsv16x_gyro_range_set(const struct device *dev, int32_t range)
 		return -EIO;
 	}
 
-	data->gyro_gain = (lsm6dsv16x_gyro_fs_sens[fs] * GAIN_UNIT_G);
+	data->gyro_gain = lsm6dsv16x_calc_gyro_gain(fs);
 	return 0;
 }
 
@@ -928,6 +940,10 @@ static const struct sensor_driver_api lsm6dsv16x_driver_api = {
 #endif
 	.sample_fetch = lsm6dsv16x_sample_fetch,
 	.channel_get = lsm6dsv16x_channel_get,
+#ifdef CONFIG_SENSOR_ASYNC_API
+	.get_decoder = lsm6dsv16x_get_decoder,
+	.submit = lsm6dsv16x_submit,
+#endif
 };
 
 static int lsm6dsv16x_init_chip(const struct device *dev)
@@ -974,7 +990,7 @@ static int lsm6dsv16x_init_chip(const struct device *dev)
 		LOG_ERR("failed to set accelerometer range %d", fs);
 		return -EIO;
 	}
-	lsm6dsv16x->acc_gain = lsm6dsv16x_accel_fs_map[fs] * GAIN_UNIT_XL / 2;
+	lsm6dsv16x->acc_gain = lsm6dsv16x_calc_accel_gain(fs);
 
 	odr = cfg->accel_odr;
 	LOG_DBG("accel odr is %d", odr);
@@ -989,7 +1005,7 @@ static int lsm6dsv16x_init_chip(const struct device *dev)
 		LOG_ERR("failed to set gyroscope range %d", fs);
 		return -EIO;
 	}
-	lsm6dsv16x->gyro_gain = (lsm6dsv16x_gyro_fs_sens[fs] * GAIN_UNIT_G);
+	lsm6dsv16x->gyro_gain = lsm6dsv16x_calc_gyro_gain(fs);
 
 	odr = cfg->gyro_odr;
 	LOG_DBG("gyro odr is %d", odr);
@@ -1061,10 +1077,6 @@ static int lsm6dsv16x_init(const struct device *dev)
 			    CONFIG_SENSOR_INIT_PRIORITY,		\
 			    &lsm6dsv16x_driver_api);
 
-/*
- * Instantiation macros used when a device is on a SPI bus.
- */
-
 #ifdef CONFIG_LSM6DSV16X_TRIGGER
 #define LSM6DSV16X_CFG_IRQ(inst)					\
 	.trig_enabled = true,						\
@@ -1076,19 +1088,33 @@ static int lsm6dsv16x_init(const struct device *dev)
 #define LSM6DSV16X_CFG_IRQ(inst)
 #endif /* CONFIG_LSM6DSV16X_TRIGGER */
 
+#define LSM6DSV16X_CONFIG_COMMON(inst)						\
+	.accel_odr = DT_INST_PROP(inst, accel_odr),				\
+	.accel_range = DT_INST_PROP(inst, accel_range),				\
+	.gyro_odr = DT_INST_PROP(inst, gyro_odr),				\
+	.gyro_range = DT_INST_PROP(inst, gyro_range),				\
+	IF_ENABLED(CONFIG_LSM6DSV16X_STREAM,					\
+		   (.fifo_wtm = DT_INST_PROP(inst, fifo_watermark),		\
+		    .accel_batch  = DT_INST_PROP(inst, accel_fifo_batch_rate),	\
+		    .gyro_batch  = DT_INST_PROP(inst, gyro_fifo_batch_rate),	\
+		    .temp_batch  = DT_INST_PROP(inst, temp_fifo_batch_rate),))	\
+	IF_ENABLED(UTIL_OR(DT_INST_NODE_HAS_PROP(inst, int1_gpios),		\
+			   DT_INST_NODE_HAS_PROP(inst, int2_gpios)),		\
+		   (LSM6DSV16X_CFG_IRQ(inst)))
+
+/*
+ * Instantiation macros used when a device is on a SPI bus.
+ */
+
 #define LSM6DSV16X_SPI_OP  (SPI_WORD_SET(8) |				\
 			 SPI_OP_MODE_MASTER |				\
 			 SPI_MODE_CPOL |				\
 			 SPI_MODE_CPHA)					\
 
-#define LSM6DSV16X_CONFIG_COMMON(inst)					\
-	.accel_odr = DT_INST_PROP(inst, accel_odr),			\
-	.accel_range = DT_INST_PROP(inst, accel_range),			\
-	.gyro_odr = DT_INST_PROP(inst, gyro_odr),			\
-	.gyro_range = DT_INST_PROP(inst, gyro_range),			\
-	IF_ENABLED(UTIL_OR(DT_INST_NODE_HAS_PROP(inst, int1_gpios),	\
-			   DT_INST_NODE_HAS_PROP(inst, int2_gpios)),	\
-		   (LSM6DSV16X_CFG_IRQ(inst)))
+#define LSM6DSV16X_SPI_RTIO_DEFINE(inst)				\
+	SPI_DT_IODEV_DEFINE(lsm6dsv16x_iodev_##inst,			\
+		DT_DRV_INST(inst), LSM6DSV16X_SPI_OP, 0U);		\
+	RTIO_DEFINE(lsm6dsv16x_rtio_ctx_##inst, 16, 16);
 
 #define LSM6DSV16X_CONFIG_SPI(inst)					\
 	{								\
@@ -1101,10 +1127,25 @@ static int lsm6dsv16x_init(const struct device *dev)
 		LSM6DSV16X_CONFIG_COMMON(inst)				\
 	}
 
+#define LSM6DSV16X_DEFINE_SPI(inst)					\
+	IF_ENABLED(CONFIG_LSM6DSV16X_STREAM, (LSM6DSV16X_SPI_RTIO_DEFINE(inst))); \
+	static struct lsm6dsv16x_data lsm6dsv16x_data_##inst =	{	\
+		IF_ENABLED(CONFIG_LSM6DSV16X_STREAM,			\
+			(.rtio_ctx = &lsm6dsv16x_rtio_ctx_##inst,	\
+			 .iodev = &lsm6dsv16x_iodev_##inst,		\
+			 .bus_type = BUS_SPI,))				\
+	};								\
+	static const struct lsm6dsv16x_config lsm6dsv16x_config_##inst = \
+		LSM6DSV16X_CONFIG_SPI(inst);				\
+
 /*
  * Instantiation macros used when a device is on an I2C bus.
  */
 
+#define LSM6DSV16X_I2C_RTIO_DEFINE(inst)				\
+	I2C_DT_IODEV_DEFINE(lsm6dsv16x_iodev_##inst, DT_DRV_INST(inst));\
+	RTIO_DEFINE(lsm6dsv16x_rtio_ctx_##inst, 16, 16);
+
 #define LSM6DSV16X_CONFIG_I2C(inst)					\
 	{								\
 		STMEMSC_CTX_I2C(&lsm6dsv16x_config_##inst.stmemsc_cfg),	\
@@ -1114,17 +1155,27 @@ static int lsm6dsv16x_init(const struct device *dev)
 		LSM6DSV16X_CONFIG_COMMON(inst)				\
 	}
 
+
+#define LSM6DSV16X_DEFINE_I2C(inst)					\
+	IF_ENABLED(CONFIG_LSM6DSV16X_STREAM, (LSM6DSV16X_I2C_RTIO_DEFINE(inst))); \
+	static struct lsm6dsv16x_data lsm6dsv16x_data_##inst =	{	\
+		IF_ENABLED(CONFIG_LSM6DSV16X_STREAM,			\
+			(.rtio_ctx = &lsm6dsv16x_rtio_ctx_##inst,	\
+			 .iodev = &lsm6dsv16x_iodev_##inst,		\
+			 .bus_type = BUS_I2C,))				\
+	};								\
+	static const struct lsm6dsv16x_config lsm6dsv16x_config_##inst = \
+		LSM6DSV16X_CONFIG_I2C(inst);				\
+
 /*
  * Main instantiation macro. Use of COND_CODE_1() selects the right
  * bus-specific macro at preprocessor time.
  */
 
 #define LSM6DSV16X_DEFINE(inst)						\
-	static struct lsm6dsv16x_data lsm6dsv16x_data_##inst;			\
-	static const struct lsm6dsv16x_config lsm6dsv16x_config_##inst =	\
 		COND_CODE_1(DT_INST_ON_BUS(inst, spi),			\
-			(LSM6DSV16X_CONFIG_SPI(inst)),			\
-			(LSM6DSV16X_CONFIG_I2C(inst)));			\
-	LSM6DSV16X_DEVICE_INIT(inst)
+			(LSM6DSV16X_DEFINE_SPI(inst)),			\
+			(LSM6DSV16X_DEFINE_I2C(inst)));			\
+		LSM6DSV16X_DEVICE_INIT(inst)
 
 DT_INST_FOREACH_STATUS_OKAY(LSM6DSV16X_DEFINE)
diff --git a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.h b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.h
@@ -35,6 +35,9 @@
 /* Gyro sensor sensitivity grain is 4.375 udps/LSB */
 #define GAIN_UNIT_G				(4375LL)
 
+int lsm6dsv16x_calc_accel_gain(uint8_t fs);
+int lsm6dsv16x_calc_gyro_gain(uint8_t fs);
+
 struct lsm6dsv16x_config {
 	stmdev_ctx_t ctx;
 	union {
@@ -52,6 +55,12 @@ struct lsm6dsv16x_config {
 	uint8_t gyro_odr;
 	uint8_t gyro_range;
 	uint8_t drdy_pulsed;
+#ifdef CONFIG_LSM6DSV16X_STREAM
+	uint8_t fifo_wtm;
+	uint8_t accel_batch : 4;
+	uint8_t gyro_batch : 4;
+	uint8_t temp_batch : 2;
+#endif
 #ifdef CONFIG_LSM6DSV16X_TRIGGER
 	const struct gpio_dt_spec int1_gpio;
 	const struct gpio_dt_spec int2_gpio;
@@ -98,6 +107,20 @@ struct lsm6dsv16x_data {
 	uint8_t gyro_freq;
 	uint8_t gyro_fs;
 
+#ifdef CONFIG_LSM6DSV16X_STREAM
+	struct rtio_iodev_sqe *streaming_sqe;
+	struct rtio *rtio_ctx;
+	struct rtio_iodev *iodev;
+	uint8_t fifo_status[2];
+	uint16_t fifo_count;
+	uint8_t fifo_irq;
+	uint8_t accel_batch_odr : 4;
+	uint8_t gyro_batch_odr : 4;
+	uint8_t temp_batch_odr : 2;
+	uint8_t bus_type : 1; /* I2C is 0, SPI is 1 */
+	uint8_t reserved : 5;
+#endif
+
 #ifdef CONFIG_LSM6DSV16X_TRIGGER
 	struct gpio_dt_spec *drdy_gpio;
 
@@ -119,6 +142,19 @@ struct lsm6dsv16x_data {
 #endif /* CONFIG_LSM6DSV16X_TRIGGER */
 };
 
+#ifdef CONFIG_LSM6DSV16X_STREAM
+#define BUS_I2C 0
+#define BUS_SPI 1
+
+static inline uint8_t lsm6dsv16x_bus_reg(struct lsm6dsv16x_data *data, uint8_t x)
+{
+	return (data->bus_type == BUS_SPI) ? x | 0x80 : x;
+}
+
+#define LSM6DSV16X_FIFO_ITEM_LEN 7
+#define LSM6DSV16X_FIFO_SIZE(x) (x * LSM6DSV16X_FIFO_ITEM_LEN)
+#endif
+
 #if defined(CONFIG_LSM6DSV16X_SENSORHUB)
 int lsm6dsv16x_shub_init(const struct device *dev);
 int lsm6dsv16x_shub_fetch_external_devs(const struct device *dev);