> 	On LSM6DSO/LSM6DSR/LSM6DSOX/ASM330LHH and ISH330DHCX
> 	devices it is possible to trim the hardware timestamp
> 	resolution through the FREQ_FINE[7:0] bits of the
> 	INTERNAL_FREQ_FINE register, which contains the difference
> 	in percentage of the effective ODR (and timestamp rate)
> 	with respect to the typical value.
> 
> 	The formula for calculating the effective ODR reported
> 	in the application notes has been linearized to the first
> 	order to simplify the calculation (pls. see note on source
> 	code).
> 
> 	This change may be useful in the outcome of CTS
> 	tests regarding the SingleSensorTests and the
> 	SensorTest#testSensorTimeStamps for high ODRs
> 
> Signed-off-by: Mario Tesi <mario.tesi@st.com>

Acked-by: Lorenzo Bianconi <lorenzo@kernel.org>

> ---
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h        |  4 ++++
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c |  5 ++---
>  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 7485423..9a40587 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;
>  };
>  
>  /**
> @@ -344,6 +346,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.
> @@ -365,6 +368,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 cabd4bf..d7cacb9 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;
>  			}
> @@ -566,7 +565,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 bacc908..2b54ce1 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),
> @@ -832,6 +834,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
>  				.addr = 0x0a,
>  				.mask = GENMASK(7, 6),
>  			},
> +			.freq_fine = 0x63,
>  		},
>  		.shub_settings = {
>  			.page_mux = {
> @@ -1020,6 +1023,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
>  				.addr = 0x0a,
>  				.mask = GENMASK(7, 6),
>  			},
> +			.freq_fine = 0x63,
>  		},
>  		.event_settings = {
>  			.enable_reg = {
> @@ -1185,6 +1189,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
>  				.addr = 0x0a,
>  				.mask = GENMASK(7, 6),
>  			},
> +			.freq_fine = 0x63,
>  		},
>  		.shub_settings = {
>  			.page_mux = {
> @@ -1874,6 +1879,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;
>  }
>  
> -- 
> 2.7.4
>