iio: st_lsm6dsx: Fixed calibrated timestamp calculation

The calibrated timestamp is calculated from the nominal value using the
formula:
  ts_gain[ns] ≈ ts_sensitivity - (ts_trim_coeff * val) / 1000.

The values of ts_sensitivity and ts_trim_coeff are not the same for all
devices, so it is necessary to differentiate them based on the part name.
For the correct values please consult the relevant AN.

Fixes: cb3b6b8e1bc0 ("iio: imu: st_lsm6dsx: add odr calibration feature")
Signed-off-by: Mario Tesi <mario.tesi@st.com>
Acked-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 bca136392e99affaf402dadc7486abe645f667dc..381b016fa524344842ea61b303727de1b9fa5f95 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -192,6 +192,22 @@ struct st_lsm6dsx_fifo_ops {
  * @fifo_en: Hw timer FIFO enable register info (addr + mask).
  * @decimator: Hw timer FIFO decimator register info (addr + mask).
  * @freq_fine: Difference in % of ODR with respect to the typical.
+ * @ts_sensitivity: Nominal timestamp sensitivity.
+ * @ts_trim_coeff: Coefficient for calculating the calibrated timestamp gain.
+ *                 This coefficient comes into play when linearizing the formula
+ *                 used to calculate the calibrated timestamp (please see the
+ *                 relevant formula in the AN for the specific IMU).
+ *                 For example, in the case of LSM6DSO we have:
+ *
+ *                  1 / (1 + x) ~= 1 - x (Taylor’s Series)
+ *                  ttrim[s] = 1 / (40000 * (1 + 0.0015 * val)) (from AN5192)
+ *                  ttrim[ns] ~= 25000 - 37.5 * val
+ *                  ttrim[ns] ~= 25000 - (37500 * val) / 1000
+ *
+ *                  so, replacing ts_sensitivity = 25000 and
+ *                  ts_trim_coeff = 37500
+ *
+ *                  ttrim[ns] ~= ts_sensitivity - (ts_trim_coeff * val) / 1000
  */
 struct st_lsm6dsx_hw_ts_settings {
        struct st_lsm6dsx_reg timer_en;
@@ -199,6 +215,8 @@ struct st_lsm6dsx_hw_ts_settings {
        struct st_lsm6dsx_reg fifo_en;
        struct st_lsm6dsx_reg decimator;
        u8 freq_fine;
+       u16 ts_sensitivity;
+       u16 ts_trim_coeff;
 };
 
 /**

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index d8cb4b0218d53002caf35b3fc775049208a7018e..a2daf0c14d965d6bf70d93e92262ce74306db532 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -94,8 +94,6 @@
 
 #define ST_LSM6DSX_REG_WHOAMI_ADDR             0x0f
 
-#define ST_LSM6DSX_TS_SENSITIVITY              25000UL /* 25us */
-
 static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
        ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x28, IIO_MOD_X, 0),
        ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x2a, IIO_MOD_Y, 1),
@@ -983,6 +981,8 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .mask = GENMASK(7, 6),
                        },
                        .freq_fine = 0x63,
+                       .ts_sensitivity = 25000,
+                       .ts_trim_coeff = 37500,
                },
                .shub_settings = {
                        .page_mux = {
@@ -1196,6 +1196,8 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .mask = GENMASK(7, 6),
                        },
                        .freq_fine = 0x63,
+                       .ts_sensitivity = 25000,
+                       .ts_trim_coeff = 37500,
                },
                .event_settings = {
                        .enable_reg = {
@@ -1371,6 +1373,8 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .mask = GENMASK(7, 6),
                        },
                        .freq_fine = 0x4f,
+                       .ts_sensitivity = 21701,
+                       .ts_trim_coeff = 28212,
                },
                .shub_settings = {
                        .page_mux = {
@@ -2248,20 +2252,13 @@ static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw)
        }
 
        /* calibrate timestamp sensitivity */
-       hw->ts_gain = ST_LSM6DSX_TS_SENSITIVITY;
+       hw->ts_gain = ts_settings->ts_sensitivity;
        if (ts_settings->freq_fine) {
                err = regmap_read(hw->regmap, ts_settings->freq_fine, &val);
                if (err < 0)
                        return err;
 
-               /*
-                * linearize the AN5192 formula:
-                * 1 / (1 + x) ~= 1 - x (Taylor’s Series)
-                * ttrim[s] = 1 / (40000 * (1 + 0.0015 * val))
-                * ttrim[ns] ~= 25000 - 37.5 * val
-                * ttrim[ns] ~= 25000 - (37500 * val) / 1000
-                */
-               hw->ts_gain -= ((s8)val * 37500) / 1000;
+               hw->ts_gain -= ((s8)val * ts_settings->ts_trim_coeff) / 1000;
        }
 
        return 0;