diff mbox series

[1/2] iio: imu: st_lsm6dsx: fix read misalignment on untagged FIFO

Message ID f9cdf1a1f69013417a5aaaf11a548e9585bdac20.1584121851.git.lorenzo@kernel.org (mailing list archive)
State New, archived
Headers show
Series fix read misalignment on LSM6DSM SensorHub | expand

Commit Message

Lorenzo Bianconi March 13, 2020, 5:54 p.m. UTC
st_lsm6dsx suffers of a read misalignment on untagged FIFO when
all 3 supported sensors (accel, gyro and ext device) are running
at different ODRs (the use-case is reported in the LSM6DSM Application
Note at pag 100).
Fix the issue taking into account decimation factor reading the FIFO
pattern.

Fixes: e485e2a2cfd6 ("iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm")
Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  2 ++
 .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 23 +++++++++++++------
 2 files changed, 18 insertions(+), 7 deletions(-)

Comments

Jonathan Cameron March 15, 2020, 11:11 a.m. UTC | #1
On Fri, 13 Mar 2020 18:54:41 +0100
Lorenzo Bianconi <lorenzo@kernel.org> wrote:

> st_lsm6dsx suffers of a read misalignment on untagged FIFO when
> all 3 supported sensors (accel, gyro and ext device) are running
> at different ODRs (the use-case is reported in the LSM6DSM Application
> Note at pag 100).
> Fix the issue taking into account decimation factor reading the FIFO
> pattern.
> 
> Fixes: e485e2a2cfd6 ("iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm")
> Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
Queued up.  Not totally sure which branch this will through, but may
well be in next merge window because of timing.

Thanks,

Jonathan

> ---
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  2 ++
>  .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 23 +++++++++++++------
>  2 files changed, 18 insertions(+), 7 deletions(-)
> 
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> index f2113a63721a..e9e756b1e92f 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> @@ -337,6 +337,7 @@ enum st_lsm6dsx_fifo_mode {
>   * @gain: Configured sensor sensitivity.
>   * @odr: Output data rate of the sensor [Hz].
>   * @watermark: Sensor watermark level.
> + * @decimator: Sensor decimation factor.
>   * @sip: Number of samples in a given pattern.
>   * @ts_ref: Sensor timestamp reference for hw one.
>   * @ext_info: Sensor settings if it is connected to i2c controller
> @@ -350,6 +351,7 @@ struct st_lsm6dsx_sensor {
>  	u32 odr;
>  
>  	u16 watermark;
> +	u8 decimator;
>  	u8 sip;
>  	s64 ts_ref;
>  
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> index bb899345f2bb..afd00daeefb2 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> @@ -93,6 +93,7 @@ st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
>  			break;
>  	}
>  
> +	sensor->decimator = decimator;
>  	return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
>  }
>  
> @@ -337,7 +338,7 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
>  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
>  {
>  	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
> -	int err, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
> +	int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
>  	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
>  	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
>  	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
> @@ -399,19 +400,20 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
>  		acc_sip = acc_sensor->sip;
>  		ts_sip = hw->ts_sip;
>  		offset = 0;
> +		sip = 0;
>  
>  		while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
> -			if (gyro_sip > 0) {
> +			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
>  				memcpy(gyro_buff, &hw->buff[offset],
>  				       ST_LSM6DSX_SAMPLE_SIZE);
>  				offset += ST_LSM6DSX_SAMPLE_SIZE;
>  			}
> -			if (acc_sip > 0) {
> +			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
>  				memcpy(acc_buff, &hw->buff[offset],
>  				       ST_LSM6DSX_SAMPLE_SIZE);
>  				offset += ST_LSM6DSX_SAMPLE_SIZE;
>  			}
> -			if (ext_sip > 0) {
> +			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
>  				memcpy(ext_buff, &hw->buff[offset],
>  				       ST_LSM6DSX_SAMPLE_SIZE);
>  				offset += ST_LSM6DSX_SAMPLE_SIZE;
> @@ -441,18 +443,25 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
>  				offset += ST_LSM6DSX_SAMPLE_SIZE;
>  			}
>  
> -			if (gyro_sip-- > 0)
> +			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
>  				iio_push_to_buffers_with_timestamp(
>  					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
>  					gyro_buff, gyro_sensor->ts_ref + ts);
> -			if (acc_sip-- > 0)
> +				gyro_sip--;
> +			}
> +			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
>  				iio_push_to_buffers_with_timestamp(
>  					hw->iio_devs[ST_LSM6DSX_ID_ACC],
>  					acc_buff, acc_sensor->ts_ref + ts);
> -			if (ext_sip-- > 0)
> +				acc_sip--;
> +			}
> +			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
>  				iio_push_to_buffers_with_timestamp(
>  					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
>  					ext_buff, ext_sensor->ts_ref + ts);
> +				ext_sip--;
> +			}
> +			sip++;
>  		}
>  	}
>
Lorenzo Bianconi March 15, 2020, 12:05 p.m. UTC | #2
On Mar 15, Jonathan Cameron wrote:
> On Fri, 13 Mar 2020 18:54:41 +0100
> Lorenzo Bianconi <lorenzo@kernel.org> wrote:
> 
> > st_lsm6dsx suffers of a read misalignment on untagged FIFO when
> > all 3 supported sensors (accel, gyro and ext device) are running
> > at different ODRs (the use-case is reported in the LSM6DSM Application
> > Note at pag 100).
> > Fix the issue taking into account decimation factor reading the FIFO
> > pattern.
> > 
> > Fixes: e485e2a2cfd6 ("iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm")
> > Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
> Queued up.  Not totally sure which branch this will through, but may
> well be in next merge window because of timing.

I guess on the stable branch since e485e2a2cfd6 is already in Linus's tree

Regards,
Lorenzo

> 
> Thanks,
> 
> Jonathan
> 
> > ---
> >  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  2 ++
> >  .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 23 +++++++++++++------
> >  2 files changed, 18 insertions(+), 7 deletions(-)
> > 
> > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > index f2113a63721a..e9e756b1e92f 100644
> > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > @@ -337,6 +337,7 @@ enum st_lsm6dsx_fifo_mode {
> >   * @gain: Configured sensor sensitivity.
> >   * @odr: Output data rate of the sensor [Hz].
> >   * @watermark: Sensor watermark level.
> > + * @decimator: Sensor decimation factor.
> >   * @sip: Number of samples in a given pattern.
> >   * @ts_ref: Sensor timestamp reference for hw one.
> >   * @ext_info: Sensor settings if it is connected to i2c controller
> > @@ -350,6 +351,7 @@ struct st_lsm6dsx_sensor {
> >  	u32 odr;
> >  
> >  	u16 watermark;
> > +	u8 decimator;
> >  	u8 sip;
> >  	s64 ts_ref;
> >  
> > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > index bb899345f2bb..afd00daeefb2 100644
> > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > @@ -93,6 +93,7 @@ st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
> >  			break;
> >  	}
> >  
> > +	sensor->decimator = decimator;
> >  	return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
> >  }
> >  
> > @@ -337,7 +338,7 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
> >  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
> >  {
> >  	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
> > -	int err, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
> > +	int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
> >  	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
> >  	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
> >  	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
> > @@ -399,19 +400,20 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
> >  		acc_sip = acc_sensor->sip;
> >  		ts_sip = hw->ts_sip;
> >  		offset = 0;
> > +		sip = 0;
> >  
> >  		while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
> > -			if (gyro_sip > 0) {
> > +			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
> >  				memcpy(gyro_buff, &hw->buff[offset],
> >  				       ST_LSM6DSX_SAMPLE_SIZE);
> >  				offset += ST_LSM6DSX_SAMPLE_SIZE;
> >  			}
> > -			if (acc_sip > 0) {
> > +			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
> >  				memcpy(acc_buff, &hw->buff[offset],
> >  				       ST_LSM6DSX_SAMPLE_SIZE);
> >  				offset += ST_LSM6DSX_SAMPLE_SIZE;
> >  			}
> > -			if (ext_sip > 0) {
> > +			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
> >  				memcpy(ext_buff, &hw->buff[offset],
> >  				       ST_LSM6DSX_SAMPLE_SIZE);
> >  				offset += ST_LSM6DSX_SAMPLE_SIZE;
> > @@ -441,18 +443,25 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
> >  				offset += ST_LSM6DSX_SAMPLE_SIZE;
> >  			}
> >  
> > -			if (gyro_sip-- > 0)
> > +			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
> >  				iio_push_to_buffers_with_timestamp(
> >  					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
> >  					gyro_buff, gyro_sensor->ts_ref + ts);
> > -			if (acc_sip-- > 0)
> > +				gyro_sip--;
> > +			}
> > +			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
> >  				iio_push_to_buffers_with_timestamp(
> >  					hw->iio_devs[ST_LSM6DSX_ID_ACC],
> >  					acc_buff, acc_sensor->ts_ref + ts);
> > -			if (ext_sip-- > 0)
> > +				acc_sip--;
> > +			}
> > +			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
> >  				iio_push_to_buffers_with_timestamp(
> >  					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
> >  					ext_buff, ext_sensor->ts_ref + ts);
> > +				ext_sip--;
> > +			}
> > +			sip++;
> >  		}
> >  	}
> >  
>
Jonathan Cameron March 15, 2020, 12:12 p.m. UTC | #3
On Sun, 15 Mar 2020 13:05:13 +0100
Lorenzo Bianconi <lorenzo@kernel.org> wrote:

> On Mar 15, Jonathan Cameron wrote:
> > On Fri, 13 Mar 2020 18:54:41 +0100
> > Lorenzo Bianconi <lorenzo@kernel.org> wrote:
> >   
> > > st_lsm6dsx suffers of a read misalignment on untagged FIFO when
> > > all 3 supported sensors (accel, gyro and ext device) are running
> > > at different ODRs (the use-case is reported in the LSM6DSM Application
> > > Note at pag 100).
> > > Fix the issue taking into account decimation factor reading the FIFO
> > > pattern.
> > > 
> > > Fixes: e485e2a2cfd6 ("iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm")
> > > Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>  
> > Queued up.  Not totally sure which branch this will through, but may
> > well be in next merge window because of timing.  
> 
> I guess on the stable branch since e485e2a2cfd6 is already in Linus's tree

Agreed, but fastest path to there might be via the merge window depending
on when I get a pull request out.

We are only rc5 I think. rc6 will be today most likely.  Only leaves a week
before we are too close to the possible merge window opening.

As I have an outstanding pull request with Greg, that may not be enough time.

If this goes in the merge window, it'll hit first or second stable release so before
any distros etc pick up the new kernel anyway.

Jonathan

> 
> Regards,
> Lorenzo
> 
> > 
> > Thanks,
> > 
> > Jonathan
> >   
> > > ---
> > >  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  2 ++
> > >  .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 23 +++++++++++++------
> > >  2 files changed, 18 insertions(+), 7 deletions(-)
> > > 
> > > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > > index f2113a63721a..e9e756b1e92f 100644
> > > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > > @@ -337,6 +337,7 @@ enum st_lsm6dsx_fifo_mode {
> > >   * @gain: Configured sensor sensitivity.
> > >   * @odr: Output data rate of the sensor [Hz].
> > >   * @watermark: Sensor watermark level.
> > > + * @decimator: Sensor decimation factor.
> > >   * @sip: Number of samples in a given pattern.
> > >   * @ts_ref: Sensor timestamp reference for hw one.
> > >   * @ext_info: Sensor settings if it is connected to i2c controller
> > > @@ -350,6 +351,7 @@ struct st_lsm6dsx_sensor {
> > >  	u32 odr;
> > >  
> > >  	u16 watermark;
> > > +	u8 decimator;
> > >  	u8 sip;
> > >  	s64 ts_ref;
> > >  
> > > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > > index bb899345f2bb..afd00daeefb2 100644
> > > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > > @@ -93,6 +93,7 @@ st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
> > >  			break;
> > >  	}
> > >  
> > > +	sensor->decimator = decimator;
> > >  	return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
> > >  }
> > >  
> > > @@ -337,7 +338,7 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
> > >  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
> > >  {
> > >  	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
> > > -	int err, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
> > > +	int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
> > >  	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
> > >  	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
> > >  	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
> > > @@ -399,19 +400,20 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
> > >  		acc_sip = acc_sensor->sip;
> > >  		ts_sip = hw->ts_sip;
> > >  		offset = 0;
> > > +		sip = 0;
> > >  
> > >  		while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
> > > -			if (gyro_sip > 0) {
> > > +			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
> > >  				memcpy(gyro_buff, &hw->buff[offset],
> > >  				       ST_LSM6DSX_SAMPLE_SIZE);
> > >  				offset += ST_LSM6DSX_SAMPLE_SIZE;
> > >  			}
> > > -			if (acc_sip > 0) {
> > > +			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
> > >  				memcpy(acc_buff, &hw->buff[offset],
> > >  				       ST_LSM6DSX_SAMPLE_SIZE);
> > >  				offset += ST_LSM6DSX_SAMPLE_SIZE;
> > >  			}
> > > -			if (ext_sip > 0) {
> > > +			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
> > >  				memcpy(ext_buff, &hw->buff[offset],
> > >  				       ST_LSM6DSX_SAMPLE_SIZE);
> > >  				offset += ST_LSM6DSX_SAMPLE_SIZE;
> > > @@ -441,18 +443,25 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
> > >  				offset += ST_LSM6DSX_SAMPLE_SIZE;
> > >  			}
> > >  
> > > -			if (gyro_sip-- > 0)
> > > +			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
> > >  				iio_push_to_buffers_with_timestamp(
> > >  					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
> > >  					gyro_buff, gyro_sensor->ts_ref + ts);
> > > -			if (acc_sip-- > 0)
> > > +				gyro_sip--;
> > > +			}
> > > +			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
> > >  				iio_push_to_buffers_with_timestamp(
> > >  					hw->iio_devs[ST_LSM6DSX_ID_ACC],
> > >  					acc_buff, acc_sensor->ts_ref + ts);
> > > -			if (ext_sip-- > 0)
> > > +				acc_sip--;
> > > +			}
> > > +			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
> > >  				iio_push_to_buffers_with_timestamp(
> > >  					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
> > >  					ext_buff, ext_sensor->ts_ref + ts);
> > > +				ext_sip--;
> > > +			}
> > > +			sip++;
> > >  		}
> > >  	}
> > >    
> >
diff mbox series

Patch

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index f2113a63721a..e9e756b1e92f 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -337,6 +337,7 @@  enum st_lsm6dsx_fifo_mode {
  * @gain: Configured sensor sensitivity.
  * @odr: Output data rate of the sensor [Hz].
  * @watermark: Sensor watermark level.
+ * @decimator: Sensor decimation factor.
  * @sip: Number of samples in a given pattern.
  * @ts_ref: Sensor timestamp reference for hw one.
  * @ext_info: Sensor settings if it is connected to i2c controller
@@ -350,6 +351,7 @@  struct st_lsm6dsx_sensor {
 	u32 odr;
 
 	u16 watermark;
+	u8 decimator;
 	u8 sip;
 	s64 ts_ref;
 
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index bb899345f2bb..afd00daeefb2 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -93,6 +93,7 @@  st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
 			break;
 	}
 
+	sensor->decimator = decimator;
 	return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
 }
 
@@ -337,7 +338,7 @@  static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 {
 	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
-	int err, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
+	int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
 	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
 	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
 	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
@@ -399,19 +400,20 @@  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 		acc_sip = acc_sensor->sip;
 		ts_sip = hw->ts_sip;
 		offset = 0;
+		sip = 0;
 
 		while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
-			if (gyro_sip > 0) {
+			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
 				memcpy(gyro_buff, &hw->buff[offset],
 				       ST_LSM6DSX_SAMPLE_SIZE);
 				offset += ST_LSM6DSX_SAMPLE_SIZE;
 			}
-			if (acc_sip > 0) {
+			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
 				memcpy(acc_buff, &hw->buff[offset],
 				       ST_LSM6DSX_SAMPLE_SIZE);
 				offset += ST_LSM6DSX_SAMPLE_SIZE;
 			}
-			if (ext_sip > 0) {
+			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
 				memcpy(ext_buff, &hw->buff[offset],
 				       ST_LSM6DSX_SAMPLE_SIZE);
 				offset += ST_LSM6DSX_SAMPLE_SIZE;
@@ -441,18 +443,25 @@  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 				offset += ST_LSM6DSX_SAMPLE_SIZE;
 			}
 
-			if (gyro_sip-- > 0)
+			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
 				iio_push_to_buffers_with_timestamp(
 					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
 					gyro_buff, gyro_sensor->ts_ref + ts);
-			if (acc_sip-- > 0)
+				gyro_sip--;
+			}
+			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
 				iio_push_to_buffers_with_timestamp(
 					hw->iio_devs[ST_LSM6DSX_ID_ACC],
 					acc_buff, acc_sensor->ts_ref + ts);
-			if (ext_sip-- > 0)
+				acc_sip--;
+			}
+			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
 				iio_push_to_buffers_with_timestamp(
 					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
 					ext_buff, ext_sensor->ts_ref + ts);
+				ext_sip--;
+			}
+			sip++;
 		}
 	}