[PATCH v2] iio: imu: st_lsm6dsx: discard samples during filters settling time

[PATCH v2] iio: imu: st_lsm6dsx: discard samples during filters settling time

[Lorenzo Bianconi]

During digital filters settling time the driver is expected to drop
samples since they can be corrupted. Introduce the capability to drop
a given number of samples according to the configured ODR.
Add sample_to_discard for LSM6DSM-like sensors since new generation
devices (e.g. LSM6DSO) support DRDY mask where corrupted samples are
masked in hw with values greather than 0x7ffd so the driver can easily
discard them.
I have not added sample_to_discard support for LSM6DS3 or LSM6DS3H since
I do not have any sample for testing at the moment.

Reported-by: Philippe De Muyter <phdm@macqel.be>
Tested-by: Philippe De Muyter <phdm@macqel.be>
Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
---
Changes since v1:
- improve commit log
- fix comment style
- remove check for LSM6DSO-like sensors since they already support drdy-mask
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       | 11 ++++
 .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 57 ++++++++++++++++---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  | 18 ++++++
 3 files changed, 78 insertions(+), 8 deletions(-)

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 499fcf8875b4..8e119d78730b 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -137,6 +137,13 @@ struct st_lsm6dsx_odr_table_entry {
 	int odr_len;
 };
 
+struct st_lsm6dsx_samples_to_discard {
+	struct {
+		u32 milli_hz;
+		u16 samples;
+	} val[ST_LSM6DSX_ODR_LIST_SIZE];
+};
+
 struct st_lsm6dsx_fs {
 	u32 gain;
 	u8 val;
@@ -291,6 +298,7 @@ struct st_lsm6dsx_ext_dev_settings {
  * @irq_config: interrupts related registers.
  * @drdy_mask: register info for data-ready mask (addr + mask).
  * @odr_table: Hw sensors odr table (Hz + val).
+ * @samples_to_discard: Number of samples to discard for filters settling time.
  * @fs_table: Hw sensors gain table (gain + val).
  * @decimator: List of decimator register info (addr + mask).
  * @batch: List of FIFO batching register info (addr + mask).
@@ -323,6 +331,7 @@ struct st_lsm6dsx_settings {
 	} irq_config;
 	struct st_lsm6dsx_reg drdy_mask;
 	struct st_lsm6dsx_odr_table_entry odr_table[2];
+	struct st_lsm6dsx_samples_to_discard samples_to_discard[2];
 	struct st_lsm6dsx_fs_table_entry fs_table[2];
 	struct st_lsm6dsx_reg decimator[ST_LSM6DSX_MAX_ID];
 	struct st_lsm6dsx_reg batch[ST_LSM6DSX_MAX_ID];
@@ -353,6 +362,7 @@ enum st_lsm6dsx_fifo_mode {
  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
  * @gain: Configured sensor sensitivity.
  * @odr: Output data rate of the sensor [Hz].
+ * @samples_to_discard: Number of samples to discard for filters settling time.
  * @watermark: Sensor watermark level.
  * @decimator: Sensor decimation factor.
  * @sip: Number of samples in a given pattern.
@@ -367,6 +377,7 @@ struct st_lsm6dsx_sensor {
 	u32 gain;
 	u32 odr;
 
+	u16 samples_to_discard;
 	u16 watermark;
 	u8 decimator;
 	u8 sip;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index 7dd5205aea5b..f6c11d6fb0b0 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -457,17 +457,31 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 			}
 
 			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
-				iio_push_to_buffers_with_timestamp(
-					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
-					&hw->scan[ST_LSM6DSX_ID_GYRO],
-					gyro_sensor->ts_ref + ts);
+				/*
+				 * We need to discards gyro samples during
+				 * filters settling time
+				 */
+				if (gyro_sensor->samples_to_discard > 0)
+					gyro_sensor->samples_to_discard--;
+				else
+					iio_push_to_buffers_with_timestamp(
+						hw->iio_devs[ST_LSM6DSX_ID_GYRO],
+						&hw->scan[ST_LSM6DSX_ID_GYRO],
+						gyro_sensor->ts_ref + ts);
 				gyro_sip--;
 			}
 			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
-				iio_push_to_buffers_with_timestamp(
-					hw->iio_devs[ST_LSM6DSX_ID_ACC],
-					&hw->scan[ST_LSM6DSX_ID_ACC],
-					acc_sensor->ts_ref + ts);
+				/*
+				 * We need to discards accel samples during
+				 * filters settling time
+				 */
+				if (acc_sensor->samples_to_discard > 0)
+					acc_sensor->samples_to_discard--;
+				else
+					iio_push_to_buffers_with_timestamp(
+						hw->iio_devs[ST_LSM6DSX_ID_ACC],
+						&hw->scan[ST_LSM6DSX_ID_ACC],
+						acc_sensor->ts_ref + ts);
 				acc_sip--;
 			}
 			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
@@ -654,6 +668,30 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
 	return err;
 }
 
+static void
+st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor)
+{
+	const struct st_lsm6dsx_samples_to_discard *data;
+	struct st_lsm6dsx_hw *hw = sensor->hw;
+	int i;
+
+	if (sensor->id != ST_LSM6DSX_ID_GYRO &&
+	    sensor->id != ST_LSM6DSX_ID_ACC)
+		return;
+
+	/* check if drdy mask is supported in hw */
+	if (hw->settings->drdy_mask.addr)
+		return;
+
+	data = &hw->settings->samples_to_discard[sensor->id];
+	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
+		if (data->val[i].milli_hz == sensor->odr) {
+			sensor->samples_to_discard = data->val[i].samples;
+			return;
+		}
+	}
+}
+
 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 {
 	struct st_lsm6dsx_hw *hw = sensor->hw;
@@ -673,6 +711,9 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 			goto out;
 	}
 
+	if (enable)
+		st_lsm6dsx_update_samples_to_discard(sensor);
+
 	err = st_lsm6dsx_device_set_enable(sensor, enable);
 	if (err < 0)
 		goto out;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 3f6060c64f32..966df6ffe874 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -634,6 +634,24 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.fs_len = 4,
 			},
 		},
+		.samples_to_discard = {
+			[ST_LSM6DSX_ID_ACC] = {
+				.val[0] = {  12500, 1 },
+				.val[1] = {  26000, 1 },
+				.val[2] = {  52000, 1 },
+				.val[3] = { 104000, 2 },
+				.val[4] = { 208000, 2 },
+				.val[5] = { 416000, 2 },
+			},
+			[ST_LSM6DSX_ID_GYRO] = {
+				.val[0] = {  12500,  2 },
+				.val[1] = {  26000,  5 },
+				.val[2] = {  52000,  7 },
+				.val[3] = { 104000, 12 },
+				.val[4] = { 208000, 20 },
+				.val[5] = { 416000, 36 },
+			},
+		},
 		.irq_config = {
 			.irq1 = {
 				.addr = 0x0d,
-- 
2.39.2

Re: [PATCH v2] iio: imu: st_lsm6dsx: discard samples during filters settling time

[Jonathan Cameron]

On Mon, 27 Feb 2023 12:14:56 +0100
Lorenzo Bianconi <lorenzo@kernel.org> wrote:

> During digital filters settling time the driver is expected to drop
> samples since they can be corrupted. Introduce the capability to drop
> a given number of samples according to the configured ODR.
> Add sample_to_discard for LSM6DSM-like sensors since new generation
> devices (e.g. LSM6DSO) support DRDY mask where corrupted samples are
> masked in hw with values greather than 0x7ffd so the driver can easily
> discard them.
> I have not added sample_to_discard support for LSM6DS3 or LSM6DS3H since
> I do not have any sample for testing at the moment.
> 
> Reported-by: Philippe De Muyter <phdm@macqel.be>
> Tested-by: Philippe De Muyter <phdm@macqel.be>
> Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
Applied to the togreg branch of iio.git.

I'm not rushing this in because any existing userspace code must coping
with the bad data by doing it's own start up filtering - or in some other
way coping.

Anyhow, if anyone feels strongly that we should rush this in as a fix
then let me know.

Thanks,

Jonathan


> ---
> Changes since v1:
> - improve commit log
> - fix comment style
> - remove check for LSM6DSO-like sensors since they already support drdy-mask
> ---
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       | 11 ++++
>  .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 57 ++++++++++++++++---
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  | 18 ++++++
>  3 files changed, 78 insertions(+), 8 deletions(-)
> 
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> index 499fcf8875b4..8e119d78730b 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> @@ -137,6 +137,13 @@ struct st_lsm6dsx_odr_table_entry {
>  	int odr_len;
>  };
>  
> +struct st_lsm6dsx_samples_to_discard {
> +	struct {
> +		u32 milli_hz;
> +		u16 samples;
> +	} val[ST_LSM6DSX_ODR_LIST_SIZE];
> +};
> +
>  struct st_lsm6dsx_fs {
>  	u32 gain;
>  	u8 val;
> @@ -291,6 +298,7 @@ struct st_lsm6dsx_ext_dev_settings {
>   * @irq_config: interrupts related registers.
>   * @drdy_mask: register info for data-ready mask (addr + mask).
>   * @odr_table: Hw sensors odr table (Hz + val).
> + * @samples_to_discard: Number of samples to discard for filters settling time.
>   * @fs_table: Hw sensors gain table (gain + val).
>   * @decimator: List of decimator register info (addr + mask).
>   * @batch: List of FIFO batching register info (addr + mask).
> @@ -323,6 +331,7 @@ struct st_lsm6dsx_settings {
>  	} irq_config;
>  	struct st_lsm6dsx_reg drdy_mask;
>  	struct st_lsm6dsx_odr_table_entry odr_table[2];
> +	struct st_lsm6dsx_samples_to_discard samples_to_discard[2];
>  	struct st_lsm6dsx_fs_table_entry fs_table[2];
>  	struct st_lsm6dsx_reg decimator[ST_LSM6DSX_MAX_ID];
>  	struct st_lsm6dsx_reg batch[ST_LSM6DSX_MAX_ID];
> @@ -353,6 +362,7 @@ enum st_lsm6dsx_fifo_mode {
>   * @hw: Pointer to instance of struct st_lsm6dsx_hw.
>   * @gain: Configured sensor sensitivity.
>   * @odr: Output data rate of the sensor [Hz].
> + * @samples_to_discard: Number of samples to discard for filters settling time.
>   * @watermark: Sensor watermark level.
>   * @decimator: Sensor decimation factor.
>   * @sip: Number of samples in a given pattern.
> @@ -367,6 +377,7 @@ struct st_lsm6dsx_sensor {
>  	u32 gain;
>  	u32 odr;
>  
> +	u16 samples_to_discard;
>  	u16 watermark;
>  	u8 decimator;
>  	u8 sip;
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> index 7dd5205aea5b..f6c11d6fb0b0 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> @@ -457,17 +457,31 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
>  			}
>  
>  			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
> -				iio_push_to_buffers_with_timestamp(
> -					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
> -					&hw->scan[ST_LSM6DSX_ID_GYRO],
> -					gyro_sensor->ts_ref + ts);
> +				/*
> +				 * We need to discards gyro samples during
> +				 * filters settling time
> +				 */
> +				if (gyro_sensor->samples_to_discard > 0)
> +					gyro_sensor->samples_to_discard--;
> +				else
> +					iio_push_to_buffers_with_timestamp(
> +						hw->iio_devs[ST_LSM6DSX_ID_GYRO],
> +						&hw->scan[ST_LSM6DSX_ID_GYRO],
> +						gyro_sensor->ts_ref + ts);
>  				gyro_sip--;
>  			}
>  			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
> -				iio_push_to_buffers_with_timestamp(
> -					hw->iio_devs[ST_LSM6DSX_ID_ACC],
> -					&hw->scan[ST_LSM6DSX_ID_ACC],
> -					acc_sensor->ts_ref + ts);
> +				/*
> +				 * We need to discards accel samples during
> +				 * filters settling time
> +				 */
> +				if (acc_sensor->samples_to_discard > 0)
> +					acc_sensor->samples_to_discard--;
> +				else
> +					iio_push_to_buffers_with_timestamp(
> +						hw->iio_devs[ST_LSM6DSX_ID_ACC],
> +						&hw->scan[ST_LSM6DSX_ID_ACC],
> +						acc_sensor->ts_ref + ts);
>  				acc_sip--;
>  			}
>  			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
> @@ -654,6 +668,30 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
>  	return err;
>  }
>  
> +static void
> +st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor)
> +{
> +	const struct st_lsm6dsx_samples_to_discard *data;
> +	struct st_lsm6dsx_hw *hw = sensor->hw;
> +	int i;
> +
> +	if (sensor->id != ST_LSM6DSX_ID_GYRO &&
> +	    sensor->id != ST_LSM6DSX_ID_ACC)
> +		return;
> +
> +	/* check if drdy mask is supported in hw */
> +	if (hw->settings->drdy_mask.addr)
> +		return;
> +
> +	data = &hw->settings->samples_to_discard[sensor->id];
> +	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
> +		if (data->val[i].milli_hz == sensor->odr) {
> +			sensor->samples_to_discard = data->val[i].samples;
> +			return;
> +		}
> +	}
> +}
> +
>  int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
>  {
>  	struct st_lsm6dsx_hw *hw = sensor->hw;
> @@ -673,6 +711,9 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
>  			goto out;
>  	}
>  
> +	if (enable)
> +		st_lsm6dsx_update_samples_to_discard(sensor);
> +
>  	err = st_lsm6dsx_device_set_enable(sensor, enable);
>  	if (err < 0)
>  		goto out;
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> index 3f6060c64f32..966df6ffe874 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> @@ -634,6 +634,24 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
>  				.fs_len = 4,
>  			},
>  		},
> +		.samples_to_discard = {
> +			[ST_LSM6DSX_ID_ACC] = {
> +				.val[0] = {  12500, 1 },
> +				.val[1] = {  26000, 1 },
> +				.val[2] = {  52000, 1 },
> +				.val[3] = { 104000, 2 },
> +				.val[4] = { 208000, 2 },
> +				.val[5] = { 416000, 2 },
> +			},
> +			[ST_LSM6DSX_ID_GYRO] = {
> +				.val[0] = {  12500,  2 },
> +				.val[1] = {  26000,  5 },
> +				.val[2] = {  52000,  7 },
> +				.val[3] = { 104000, 12 },
> +				.val[4] = { 208000, 20 },
> +				.val[5] = { 416000, 36 },
> +			},
> +		},
>  		.irq_config = {
>  			.irq1 = {
>  				.addr = 0x0d,