WIP streaming

Signed-off-by: Yuval Peress <[email protected]>

drivers/sensor/Kconfig

@@ -36,6 +36,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

drivers/sensor/default_rtio_sensor.c

@@ -22,8 +22,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);
 	}
 }
 

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

@@ -50,6 +50,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

@@ -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;

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;

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);
@@ -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;

drivers/sensor/icm42688/icm42688_decoder.c

@@ -214,6 +214,98 @@ int icm42688_encode(const struct device *dev, const enum sensor_channel *const c
 	return 0;
 }
 
+static int icm42688_fifo_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)
+{
+	const struct icm42688_fifo_data *edata = (const struct icm42688_fifo_data *)buffer;
+
+	if (edata->fifo_count <= *fit) {
+		return 0;
+	}
+
+	/* Jump to the frame start */
+	buffer += sizeof(struct icm42688_fifo_data) + *fit;
+
+	const bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]);
+	const bool has_accel = FIELD_GET(FIFO_HEADER_ACCEL, buffer[0]);
+	const bool has_gyro = FIELD_GET(FIFO_HEADER_GYRO, buffer[0]);
+	const int channel_count = 1 + (has_accel ? 3 : 0) + (has_gyro ? 3 : 0);
+	const uint8_t *frame_end = buffer + /* header */ 1 + /* accel */ (has_accel ? 6 : 0) +
+				   /* gyro */ (has_gyro ? 6 : 0) +
+				   /* timestamp */ (has_accel && has_gyro ? 2 : 0) +
+				   /* temperature */ (is_20b ? 2 : 1);
+	int header_channels[7];
+	int count = 0;
+
+	if (*cit >= channel_count) {
+		return -1;
+	}
+
+	if (has_accel) {
+		header_channels[count++] = SENSOR_CHAN_ACCEL_X;
+		header_channels[count++] = SENSOR_CHAN_ACCEL_Y;
+		header_channels[count++] = SENSOR_CHAN_ACCEL_Z;
+	}
+	if (has_gyro) {
+		header_channels[count++] = SENSOR_CHAN_GYRO_X;
+		header_channels[count++] = SENSOR_CHAN_GYRO_Y;
+		header_channels[count++] = SENSOR_CHAN_GYRO_Z;
+	}
+	header_channels[count] = SENSOR_CHAN_DIE_TEMP;
+	count = 0;
+
+	while (count < max_count && *cit < channel_count) {
+		/*
+		 * [0]@0 - accel_x
+		 * [1]@2 - accel_y
+		 * [2]@4 - accel_z
+		 * [3]@6 - gyro_x
+		 * [4]@8 - gyro_y
+		 * [5]@a - gyro_z
+		 * [6]@c - temp
+		 */
+		int offset = 1 + *cit * 2;
+
+		channels[count] = header_channels[*cit];
+		int32_t value;
+		int32_t value_max = INT16_MAX;
+
+		if (channels[count] == SENSOR_CHAN_DIE_TEMP) {
+			value = buffer[offset];
+			if (is_20b) {
+				value |= (buffer[offset + 1] << 8);
+			} else {
+				value_max = INT8_MAX;
+			}
+		} else {
+			value = (buffer[offset] << 8) | buffer[offset + 1];
+			if (is_20b) {
+				int mask = channels[count] < SENSOR_CHAN_ACCEL_XYZ ? GENMASK(7, 4)
+										   : GENMASK(3, 0);
+				/* Get the offset after the end of the frame data */
+				int ext_offset =
+					(channels[count] - (channels[count] < SENSOR_CHAN_ACCEL_XYZ
+								    ? SENSOR_CHAN_ACCEL_X
+								    : SENSOR_CHAN_GYRO_X));
+
+				value = (value << 4) | FIELD_GET(mask, frame_end[ext_offset]);
+				value_max = (1 << 19) - 1;
+			}
+		}
+		values[count] = (q31_t)((int64_t)value * INT32_MAX / value_max);
+		count++;
+		*cit += 1;
+	}
+
+	if (*cit == channel_count) {
+		/* Reached the end of the frame */
+		*fit += (uintptr_t)frame_end - (uintptr_t)buffer + (is_20b ? 3 : 0);
+		*cit = 0;
+	}
+	return count;
+}
+
 static int icm42688_one_shot_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)
@@ -267,13 +359,53 @@ static int icm42688_decoder_decode(const uint8_t *buffer, sensor_frame_iterator_
 				   sensor_channel_iterator_t *cit, enum sensor_channel *channels,
 				   q31_t *values, uint8_t max_count)
 {
+	const struct icm42688_decoder_header *header =
+		(const struct icm42688_decoder_header *)buffer;
+
+	if (header->is_fifo) {
+		return icm42688_fifo_decode(buffer, fit, cit, channels, values, max_count);
+	}
 	return icm42688_one_shot_decode(buffer, fit, cit, channels, values, max_count);
 }
 
 static int icm42688_decoder_get_frame_count(const uint8_t *buffer, uint16_t *frame_count)
 {
-	ARG_UNUSED(buffer);
-	*frame_count = 1;
+	const struct icm42688_fifo_data *data = (const struct icm42688_fifo_data *)buffer;
+	const struct icm42688_decoder_header *header = &data->header;
+
+	if (!header->is_fifo) {
+		*frame_count = 1;
+		return 0;
+	}
+
+	/* 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;
 }
 
@@ -286,6 +418,26 @@ static int icm42688_decoder_get_timestamp(const uint8_t *buffer, uint64_t *times
 	return 0;
 }
 
+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_THRESHOLD:
+		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;
+	}
+}
+
 static int icm42688_decoder_get_shift(const uint8_t *buffer, enum sensor_channel channel_type,
 				      int8_t *shift)
 {
@@ -299,6 +451,7 @@ SENSOR_DECODER_API_DT_DEFINE() = {
 	.get_frame_count = icm42688_decoder_get_frame_count,
 	.get_timestamp = icm42688_decoder_get_timestamp,
 	.get_shift = icm42688_decoder_get_shift,
+	.has_trigger = icm24688_decoder_has_trigger,
 	.decode = icm42688_decoder_decode,
 };
 

drivers/sensor/icm42688/icm42688_decoder.h

@@ -18,6 +18,15 @@ struct icm42688_decoder_header {
 	uint64_t timestamp;
 } __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 {

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_ */