Commit db3c4905 authored by Lorenzo Bianconi's avatar Lorenzo Bianconi Committed by Jonathan Cameron
Browse files

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: default avatarPhilippe De Muyter <phdm@macqel.be>
Tested-by: default avatarPhilippe De Muyter <phdm@macqel.be>
Signed-off-by: default avatarLorenzo Bianconi <lorenzo@kernel.org>
Link: https://lore.kernel.org/r/21dcd94935c147ef9b1da4984b3da6264ee9609e.1677496295.git.lorenzo@kernel.org


Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent accb9d05

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h

+11 −0
Original line number Diff line number Diff line
@@ -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;

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

+49 −8
Original line number Diff line number Diff line
@@ -457,6 +457,13 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 
			}



			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {

				/*

				 * 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],
@@ -464,6 +471,13 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 
				gyro_sip--;

			}

			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {

				/*

				 * 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],
@@ -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;

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c

+18 −0
Original line number Diff line number Diff line
@@ -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,