diff options
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 4 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 5 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 23 |
3 files changed, 29 insertions, 3 deletions
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h index b766c2218f4b..3dfd21f5d2a4 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h @@ -153,12 +153,14 @@ struct st_lsm6dsx_fifo_ops { * @hr_timer: Hw timer resolution register info (addr + mask). * @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. */ struct st_lsm6dsx_hw_ts_settings { struct st_lsm6dsx_reg timer_en; struct st_lsm6dsx_reg hr_timer; struct st_lsm6dsx_reg fifo_en; struct st_lsm6dsx_reg decimator; + u8 freq_fine; }; /** @@ -346,6 +348,7 @@ struct st_lsm6dsx_sensor { * @fifo_mode: FIFO operating mode supported by the device. * @suspend_mask: Suspended sensor bitmask. * @enable_mask: Enabled sensor bitmask. + * @ts_gain: Hw timestamp rate after internal calibration. * @ts_sip: Total number of timestamp samples in a given pattern. * @sip: Total number of samples (acc/gyro/ts) in a given pattern. * @buff: Device read buffer. @@ -367,6 +370,7 @@ struct st_lsm6dsx_hw { enum st_lsm6dsx_fifo_mode fifo_mode; u8 suspend_mask; u8 enable_mask; + s64 ts_gain; u8 ts_sip; 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 1f6873441aa0..ffeb2596b97b 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c @@ -50,7 +50,6 @@ #define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08 -#define ST_LSM6DSX_TS_SENSITIVITY 25000UL /* 25us */ #define ST_LSM6DSX_TS_RESET_VAL 0xaa struct st_lsm6dsx_decimator_entry { @@ -423,7 +422,7 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) */ if (!reset_ts && ts >= 0xff0000) reset_ts = true; - ts *= ST_LSM6DSX_TS_SENSITIVITY; + ts *= hw->ts_gain; offset += ST_LSM6DSX_SAMPLE_SIZE; } @@ -572,7 +571,7 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw) */ if (!reset_ts && ts >= 0xffff0000) reset_ts = true; - ts *= ST_LSM6DSX_TS_SENSITIVITY; + ts *= hw->ts_gain; } else { st_lsm6dsx_push_tagged_data(hw, tag, iio_buff, ts); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c index 4f2a787f63f4..b0f525066237 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -63,6 +63,8 @@ #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), @@ -843,6 +845,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { .addr = 0x0a, .mask = GENMASK(7, 6), }, + .freq_fine = 0x63, }, .shub_settings = { .page_mux = { @@ -1037,6 +1040,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { .addr = 0x0a, .mask = GENMASK(7, 6), }, + .freq_fine = 0x63, }, .event_settings = { .enable_reg = { @@ -1208,6 +1212,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { .addr = 0x0a, .mask = GENMASK(7, 6), }, + .freq_fine = 0x63, }, .shub_settings = { .page_mux = { @@ -1893,6 +1898,24 @@ static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw) if (err < 0) return err; } + + /* calibrate timestamp sensitivity */ + hw->ts_gain = ST_LSM6DSX_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; + } + return 0; } |