sensors: drivers: lsm6dsv16x: add SFLP FIFO support

Add the Sensor Fusion Low Power (SFLP) FIFO streaming capability,
using RTIO. The decode function is now aware of parsing following FIFO
tags:

    - LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG
    - LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG
    - LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG

To activate SFLP the user should put in DT the proper configuration.
For example, to activate a 60Hz SFLP FIFO batching rate of game rotation
vector, gravity vector and gbias components, the user should add in DT
the following:

  sflp-odr = <LSM6DSV16X_DT_SFLP_ODR_AT_60Hz>;
  sflp-fifo-enable = <LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY_GBIAS>;

Signed-off-by: Armando Visconti <[email protected]>

drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.c

@@ -1138,6 +1138,8 @@ static int lsm6dsv16x_init(const struct device *dev)
 		   (.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),	\
+		    .sflp_odr  = DT_INST_PROP(inst, sflp_odr),			\
+		    .sflp_fifo_en  = DT_INST_PROP(inst, sflp_fifo_enable),	\
 		    .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)),		\

drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.h

@@ -75,6 +75,8 @@ struct lsm6dsv16x_config {
 	uint8_t accel_batch : 4;
 	uint8_t gyro_batch : 4;
 	uint8_t temp_batch : 2;
+	uint8_t sflp_odr : 3;
+	uint8_t sflp_fifo_en : 3;
 #endif
 #ifdef CONFIG_LSM6DSV16X_TRIGGER
 	const struct gpio_dt_spec int1_gpio;
@@ -158,7 +160,8 @@ struct lsm6dsv16x_data {
 	uint8_t gyro_batch_odr : 4;
 	uint8_t temp_batch_odr : 2;
 	uint8_t bus_type : 2; /* I2C is 0, SPI is 1, I3C is 2 */
-	uint8_t reserved : 4;
+	uint8_t sflp_batch_odr : 3;
+	uint8_t reserved : 1;
 #endif
 
 #ifdef CONFIG_LSM6DSV16X_TRIGGER

drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.c

@@ -51,8 +51,23 @@ static const uint32_t temp_period_ns[] = {
 	[LSM6DSV16X_TEMP_BATCHED_AT_60Hz] = UINT32_C(1000000000) / 60,
 };
 #endif
+
+static const uint32_t sflp_period_ns[] = {
+	[LSM6DSV16X_DT_SFLP_ODR_AT_15Hz] = UINT32_C(1000000000) / 15,
+	[LSM6DSV16X_DT_SFLP_ODR_AT_30Hz] = UINT32_C(1000000000) / 30,
+	[LSM6DSV16X_DT_SFLP_ODR_AT_60Hz] = UINT32_C(1000000000) / 60,
+	[LSM6DSV16X_DT_SFLP_ODR_AT_120Hz] = UINT32_C(1000000000) / 120,
+	[LSM6DSV16X_DT_SFLP_ODR_AT_240Hz] = UINT32_C(1000000000) / 240,
+	[LSM6DSV16X_DT_SFLP_ODR_AT_480Hz] = UINT32_C(1000000000) / 480,
+};
 #endif /* CONFIG_LSM6DSV16X_STREAM */
 
+/*
+ * Expand val to q31_t according to its range; this is achieved multiplying by 2^31/2^range.
+ */
+#define Q31_SHIFT_VAL(val, range) \
+	(q31_t) (round((val) * ((int64_t)1 << (31 - (range)))))
+
 /*
  * Expand micro_val (a generic micro unit) to q31_t according to its range; this is achieved
  * multiplying by 2^31/2^range. Then transform it to val.
@@ -157,6 +172,7 @@ static int lsm6dsv16x_decoder_get_frame_count(const uint8_t *buffer,
 	const uint8_t *buffer_end;
 	uint8_t fifo_tag;
 	uint8_t tot_accel_fifo_words = 0, tot_gyro_fifo_words = 0;
+	uint8_t tot_sflp_gbias = 0, tot_sflp_gravity = 0, tot_sflp_game_rotation = 0;
 
 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
 	uint8_t tot_temp_fifo_words = 0;
@@ -181,6 +197,15 @@ static int lsm6dsv16x_decoder_get_frame_count(const uint8_t *buffer,
 			tot_temp_fifo_words++;
 			break;
 #endif
+		case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
+			tot_sflp_gbias++;
+			break;
+		case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG:
+			tot_sflp_gravity++;
+			break;
+		case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG:
+			tot_sflp_game_rotation++;
+			break;
 		default:
 			break;
 		}
@@ -208,11 +233,21 @@ static int lsm6dsv16x_decoder_get_frame_count(const uint8_t *buffer,
 		*frame_count = tot_temp_fifo_words;
 		break;
 #endif
+	case SENSOR_CHAN_GAME_ROTATION_VECTOR:
+		*frame_count = tot_sflp_game_rotation;
+		break;
+	case SENSOR_CHAN_GRAVITY_VECTOR:
+		*frame_count = tot_sflp_gravity;
+		break;
+	case SENSOR_CHAN_GBIAS_XYZ:
+		*frame_count = tot_sflp_gbias;
+		break;
 	default:
 		*frame_count = 0;
 		break;
 	}
 #endif
+
 	return 0;
 }
 
@@ -231,6 +266,7 @@ static int lsm6dsv16x_decode_fifo(const uint8_t *buffer, struct sensor_chan_spec
 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
 	uint16_t temp_count = 0;
 #endif
+	uint16_t game_rot_count = 0, gravity_count = 0, gbias_count = 0;
 	int ret;
 
 	/* count total FIFO word for each tag */
@@ -261,6 +297,12 @@ static int lsm6dsv16x_decode_fifo(const uint8_t *buffer, struct sensor_chan_spec
 			edata->header.timestamp -
 			(tot_chan_fifo_words - 1) * temp_period_ns[edata->temp_batch_odr];
 #endif
+	} else if (chan_spec.chan_type == SENSOR_CHAN_GRAVITY_VECTOR ||
+		   chan_spec.chan_type == SENSOR_CHAN_GAME_ROTATION_VECTOR ||
+		   chan_spec.chan_type == SENSOR_CHAN_GBIAS_XYZ) {
+		((struct sensor_data_header *)data_out)->base_timestamp_ns =
+			edata->header.timestamp -
+			(tot_chan_fifo_words - 1) * sflp_period_ns[edata->sflp_batch_odr];
 	}
 
 	while (count < max_count && buffer < buffer_end) {
@@ -365,6 +407,126 @@ static int lsm6dsv16x_decode_fifo(const uint8_t *buffer, struct sensor_chan_spec
 			break;
 		}
 #endif
+		case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: {
+			struct sensor_game_rotation_vector_data *out = data_out;
+			union { float32_t f; uint32_t i; } x, y, z;
+			float32_t w, sumsq;
+
+			game_rot_count++;
+			if ((uintptr_t)buffer < *fit) {
+				/* This frame was already decoded, move on to the next frame */
+				buffer = frame_end;
+				continue;
+			}
+
+			if (chan_spec.chan_type != SENSOR_CHAN_GAME_ROTATION_VECTOR) {
+				buffer = frame_end;
+				continue;
+			}
+
+			out->readings[count].timestamp_delta =
+				(game_rot_count - 1) * sflp_period_ns[edata->sflp_batch_odr];
+
+			x.i = lsm6dsv16x_from_f16_to_f32(buffer[1] | (buffer[2] << 8));
+			y.i = lsm6dsv16x_from_f16_to_f32(buffer[3] | (buffer[4] << 8));
+			z.i = lsm6dsv16x_from_f16_to_f32(buffer[5] | (buffer[6] << 8));
+
+			sumsq = powf(x.f, 2) + powf(y.f, 2) + powf(z.f, 2);
+
+			/*
+			 * Theoretically sumsq should never be greater than 1, but due to
+			 * lack of precision it might happen. So, add a software correction
+			 * which consists in normalizing the (x, y, z) vector.
+			 */
+			if (sumsq > 1.0f) {
+				float n = sqrtf(sumsq);
+
+				x.f /= n;
+				y.f /= n;
+				z.f /= n;
+				sumsq = 1.0f;
+			}
+
+			/* unity vector quaternions */
+			w = sqrtf(1.0f - sumsq);
+
+			/*
+			 * Quaternions are numbers between -1 and 1. So let's select the signed
+			 * Q0.31 format (m = 0, n (fractional bits) == 31)
+			 */
+			out->shift = 0;
+
+			out->readings[count].x = Q31_SHIFT_VAL(x.f, out->shift);
+			out->readings[count].y = Q31_SHIFT_VAL(y.f, out->shift);
+			out->readings[count].z = Q31_SHIFT_VAL(z.f, out->shift);
+			out->readings[count].w = Q31_SHIFT_VAL(w, out->shift);
+
+			break;
+		}
+
+		case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: {
+			struct sensor_three_axis_data *out = data_out;
+			int16_t x, y, z;
+			const int32_t scale = gyro_scaler[LSM6DSV16X_DT_FS_125DPS];
+
+			gbias_count++;
+			if ((uintptr_t)buffer < *fit) {
+				/* This frame was already decoded, move on to the next frame */
+				buffer = frame_end;
+				continue;
+			}
+
+			if (chan_spec.chan_type != SENSOR_CHAN_GBIAS_XYZ) {
+				buffer = frame_end;
+				continue;
+			}
+
+			out->readings[count].timestamp_delta =
+				(gbias_count - 1) * sflp_period_ns[edata->sflp_batch_odr];
+
+			x = buffer[1] | (buffer[2] << 8);
+			y = buffer[3] | (buffer[4] << 8);
+			z = buffer[5] | (buffer[6] << 8);
+
+			out->shift = gyro_range[LSM6DSV16X_DT_FS_125DPS];
+
+			out->readings[count].x = Q31_SHIFT_MICROVAL(scale * x, out->shift);
+			out->readings[count].y = Q31_SHIFT_MICROVAL(scale * y, out->shift);
+			out->readings[count].z = Q31_SHIFT_MICROVAL(scale * z, out->shift);
+			break;
+		}
+
+		case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: {
+			struct sensor_three_axis_data *out = data_out;
+			float32_t x, y, z;
+
+			gravity_count++;
+			if ((uintptr_t)buffer < *fit) {
+				/* This frame was already decoded, move on to the next frame */
+				buffer = frame_end;
+				continue;
+			}
+
+			if (chan_spec.chan_type != SENSOR_CHAN_GRAVITY_VECTOR) {
+				buffer = frame_end;
+				continue;
+			}
+
+			out->readings[count].timestamp_delta =
+				(gravity_count - 1) * sflp_period_ns[edata->sflp_batch_odr];
+
+			x = lsm6dsv16x_from_sflp_to_mg(buffer[1] | (buffer[2] << 8));
+			y = lsm6dsv16x_from_sflp_to_mg(buffer[3] | (buffer[4] << 8));
+			z = lsm6dsv16x_from_sflp_to_mg(buffer[5] | (buffer[6] << 8));
+
+			out->shift = 12;
+
+			out->readings[count].x = Q31_SHIFT_VAL(x, out->shift);
+			out->readings[count].y = Q31_SHIFT_VAL(y, out->shift);
+			out->readings[count].z = Q31_SHIFT_VAL(z, out->shift);
+			break;
+		}
+
 		default:
 			/* skip unhandled FIFO tag */
 			buffer = frame_end;

drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.h

@@ -30,7 +30,8 @@ struct lsm6dsv16x_fifo_data {
 	uint16_t gyro_batch_odr: 4;
 	uint16_t accel_batch_odr: 4;
 	uint16_t temp_batch_odr: 4;
-	uint16_t reserved_2: 4;
+	uint16_t sflp_batch_odr: 3;
+	uint16_t reserved_2: 1;
 } __attribute__((__packed__));
 
 struct lsm6dsv16x_rtio_data {

drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_rtio_stream.c

@@ -31,6 +31,8 @@ static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
 	lsm6dsv16x_fifo_gy_batch_t gy_batch = LSM6DSV16X_DT_GY_NOT_BATCHED;
 	lsm6dsv16x_fifo_temp_batch_t temp_batch = LSM6DSV16X_DT_TEMP_NOT_BATCHED;
 	lsm6dsv16x_fifo_mode_t fifo_mode = LSM6DSV16X_BYPASS_MODE;
+	lsm6dsv16x_sflp_data_rate_t sflp_odr = LSM6DSV16X_SFLP_120Hz;
+	lsm6dsv16x_fifo_sflp_raw_t sflp_fifo = { 0 };
 
 	/* disable FIFO as first thing */
 	lsm6dsv16x_fifo_mode_set(ctx, LSM6DSV16X_BYPASS_MODE);
@@ -48,6 +50,20 @@ static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
 
 		fifo_mode = LSM6DSV16X_STREAM_MODE;
 		fifo_wtm = config->fifo_wtm;
+
+		if (config->sflp_fifo_en & LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION) {
+			sflp_fifo.game_rotation = 1;
+		}
+
+		if (config->sflp_fifo_en & LSM6DSV16X_DT_SFLP_FIFO_GRAVITY) {
+			sflp_fifo.gravity = 1;
+		}
+
+		if (config->sflp_fifo_en & LSM6DSV16X_DT_SFLP_FIFO_GBIAS) {
+			sflp_fifo.gbias = 1;
+		}
+
+		sflp_odr = config->sflp_odr;
 	}
 
 	/*
@@ -69,6 +85,11 @@ static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
 	lsm6dsv16x->temp_batch_odr = temp_batch;
 #endif
 
+	lsm6dsv16x_sflp_data_rate_set(ctx, sflp_odr);
+	lsm6dsv16x->sflp_batch_odr = sflp_odr;
+	lsm6dsv16x_fifo_sflp_batch_set(ctx, sflp_fifo);
+	lsm6dsv16x_sflp_game_rotation_set(ctx, PROPERTY_ENABLE);
+
 	/* Set pin interrupt (fifo_th could be on or off) */
 	if ((config->drdy_pin == 1) || (ON_I3C_BUS(config) && (!I3C_INT_PIN(config)))) {
 		lsm6dsv16x_pin_int1_route_set(ctx, &pin_int);
@@ -306,6 +327,7 @@ static void lsm6dsv16x_read_fifo_cb(struct rtio *r, const struct rtio_sqe *sqe,
 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
 		.temp_batch_odr = lsm6dsv16x->temp_batch_odr,
 #endif
+		.sflp_batch_odr = lsm6dsv16x->sflp_batch_odr,
 	};
 
 	memcpy(buf, &hdr, sizeof(hdr));

dts/bindings/sensor/st,lsm6dsv16x-common.yaml

@@ -258,3 +258,41 @@ properties:
       - 0x3 # LSM6DSV16X_DT_TEMP_BATCHED_AT_60Hz
 
     enum: [0x00, 0x01, 0x02, 0x03]
+
+  sflp-odr:
+    type: int
+    default: 0x3
+    description: |
+      Specify the Sensor Fusion Low Power output data rate expressed in samples per second (Hz).
+      The values are taken in accordance to lsm6dsv16x_sflp_data_rate_t enumerative in hal/st
+      module.
+      Default is power-up configuration.
+
+      - 0x0 # LSM6DSV16X_DT_SFLP_ODR_AT_15Hz
+      - 0x1 # LSM6DSV16X_DT_SFLP_ODR_AT_30Hz
+      - 0x2 # LSM6DSV16X_DT_SFLP_ODR_AT_60Hz
+      - 0x3 # LSM6DSV16X_DT_SFLP_ODR_AT_120Hz
+      - 0x4 # LSM6DSV16X_DT_SFLP_ODR_AT_240Hz
+      - 0x5 # LSM6DSV16X_DT_SFLP_ODR_AT_480Hz
+
+    enum: [0x00, 0x01, 0x02, 0x03, 0x04, 0x05]
+
+  sflp-fifo-enable:
+    type: int
+    default: 0x0
+    description: |
+      Specify what Sensor Fusion Low Power component has to be batched in FIFO.
+      The values are taken in accordance to lsm6dsv16x_fifo_sflp_raw_t enumerative in hal/st
+      module.
+      Default is power-up configuration.
+
+      - 0x0 # LSM6DSV16X_DT_SFLP_FIFO_OFF
+      - 0x1 # LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION
+      - 0x2 # LSM6DSV16X_DT_SFLP_FIFO_GRAVITY
+      - 0x3 # LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY
+      - 0x4 # LSM6DSV16X_DT_SFLP_FIFO_GBIAS
+      - 0x5 # LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GBIAS
+      - 0x6 # LSM6DSV16X_DT_SFLP_FIFO_GRAVITY_GBIAS
+      - 0x7 # LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY_GBIAS
+
+    enum: [0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07]

include/zephyr/dt-bindings/sensor/lsm6dsv16x.h

@@ -91,4 +91,23 @@
 #define LSM6DSV16X_DT_TEMP_BATCHED_AT_15Hz	0x2
 #define LSM6DSV16X_DT_TEMP_BATCHED_AT_60Hz	0x3
 
+/* Sensor Fusion Low Power Data rates */
+#define LSM6DSV16X_DT_SFLP_ODR_AT_15Hz		0x0
+#define LSM6DSV16X_DT_SFLP_ODR_AT_30Hz		0x1
+#define LSM6DSV16X_DT_SFLP_ODR_AT_60Hz		0x2
+#define LSM6DSV16X_DT_SFLP_ODR_AT_120Hz		0x3
+#define LSM6DSV16X_DT_SFLP_ODR_AT_240Hz		0x4
+#define LSM6DSV16X_DT_SFLP_ODR_AT_480Hz		0x5
+
+/* Sensor Fusion Low Power FIFO enable defs */
+#define LSM6DSV16X_DT_SFLP_FIFO_OFF				0x0
+#define LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION			0x1
+#define LSM6DSV16X_DT_SFLP_FIFO_GRAVITY				0x2
+#define LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY		0x3
+#define LSM6DSV16X_DT_SFLP_FIFO_GBIAS				0x4
+#define LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GBIAS		0x5
+#define LSM6DSV16X_DT_SFLP_FIFO_GRAVITY_GBIAS			0x6
+#define LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY_GBIAS	0x7
+
+
 #endif /* ZEPHYR_INCLUDE_DT_BINDINGS_ST_LSM6DSV16X_H_ */