Re: [PATCH] iio: imu: st_lsm6dsx: add odr calibration feature

[Jonathan Cameron <jic23@kernel.org>]

On Mon, 7 Oct 2019 16:07:27 +0200
Lorenzo Bianconi <lorenzo@kernel.org> wrote:

> > 	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>
Hi.

The leading tabs on the commit message are a bit unusual,
but as far as I can tell the code is good.

Applied to the togreg branch of iio.git and pushed out
as testing for the autobuilders to play with it.

Thanks,

Jonathan

> 
> > ---
> >  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
> >