iio: imu: st_lsm6dsx: discard samples during filters settling time

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>
Link: https://lore.kernel.org/r/21dcd94935c147ef9b1da4984b3da6264ee9609e.1677496295.git.lorenzo@kernel.org
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 499fcf8..8e119d7 100644 (file)
--- 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 7dd5205..f6c11d6 100644 (file)
--- 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 3f6060c..966df6f 100644 (file)
--- 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,