iio: imu: st_lsm6dsx: fix decimation factor estimation

Fix decimation factor and sip estimation for LSM6DSM series
(max value for decimation factor is 32).
If gyro and accel sensors are enabled at 12.5Hz and 416Hz
respectively, decimation factor lookup will fail, producing
unaligned data.
Remove unused decimator filed in st_lsm6dsx_sensor structure.

Fixes: f8710f0357bc ("iio: imu: st_lsm6dsx: express odr in mHZ")
Signed-off-by: Lorenzo Bianconi <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 c605b153be410af4f4ad8fc4fd1aee17838989ed..510c4bd4a785460f22d9de05c232460e3217911c 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -320,7 +320,6 @@ enum st_lsm6dsx_fifo_mode {
  * @odr: Output data rate of the sensor [Hz].
  * @watermark: Sensor watermark level.
  * @sip: Number of samples in a given pattern.
- * @decimator: FIFO decimation factor.
  * @ts_ref: Sensor timestamp reference for hw one.
  * @ext_info: Sensor settings if it is connected to i2c controller
  */
@@ -334,7 +333,6 @@ struct st_lsm6dsx_sensor {
 
        u16 watermark;
        u8 sip;
-       u8 decimator;
        s64 ts_ref;
 
        struct {

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index d416990ae309d7ba6e5bdb203ee9c8a4d6f51ddf..a70b04f1418727c9684ec22230d6a24ece42c78b 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -78,14 +78,20 @@ struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
        { 32, 0x7 },
 };
 
-static int st_lsm6dsx_get_decimator_val(u8 val)
+static int
+st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
 {
        const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
+       u32 decimator =  max_odr / sensor->odr;
        int i;
 
-       for (i = 0; i < max_size; i++)
-               if (st_lsm6dsx_decimator_table[i].decimator == val)
+       if (decimator > 1)
+               decimator = round_down(decimator, 2);
+
+       for (i = 0; i < max_size; i++) {
+               if (st_lsm6dsx_decimator_table[i].decimator == decimator)
                        break;
+       }
 
        return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
 }
@@ -111,6 +117,13 @@ static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
        }
 }
 
+static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr)
+{
+       u8 sip = sensor->odr / min_odr;
+
+       return sip > 1 ? round_down(sip, 2) : sip;
+}
+
 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 {
        const struct st_lsm6dsx_reg *ts_dec_reg;
@@ -131,12 +144,10 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
                sensor = iio_priv(hw->iio_devs[i]);
                /* update fifo decimators and sample in pattern */
                if (hw->enable_mask & BIT(sensor->id)) {
-                       sensor->sip = sensor->odr / min_odr;
-                       sensor->decimator = max_odr / sensor->odr;
-                       data = st_lsm6dsx_get_decimator_val(sensor->decimator);
+                       sensor->sip = st_lsm6dsx_get_sip(sensor, min_odr);
+                       data = st_lsm6dsx_get_decimator_val(sensor, max_odr);
                } else {
                        sensor->sip = 0;
-                       sensor->decimator = 0;
                        data = 0;
                }
                ts_sip = max_t(u16, ts_sip, sensor->sip);