[6/6] iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm

Commit Message

Lorenzo Bianconi Dec. 14, 2019, 4:52 p.m. UTC

Enabled i2c master controller support for LSM6DSM sensor. Enable
ext_sensor0 for lsm6dsm. This series has been tested using LIS2MDL as
slave device connected to the i2c controller of the LSM6DSM

Signed-off-by: Lorenzo Bianconi <[e-mail]>
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  4 ++
 .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 27 ++++++++---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  | 47 +++++++++++++++++++
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c  | 31 ++++++++----
 4 files changed, 94 insertions(+), 15 deletions(-)

Comments

Jonathan Cameron Dec. 23, 2019, 4:49 p.m. UTC | #1

On Sat, 14 Dec 2019 17:52:59 +0100
Lorenzo Bianconi <[e-mail]> wrote:

> Enabled i2c master controller support for LSM6DSM sensor. Enable
> ext_sensor0 for lsm6dsm. This series has been tested using LIS2MDL as
> slave device connected to the i2c controller of the LSM6DSM
> 
> Signed-off-by: Lorenzo Bianconi <[e-mail]>
One question inline.  However I'm happy enough to take it with that
outstanding on basis I am not going to push out as non rebasing for
a while yet.

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 ++
>  .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 27 ++++++++---
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  | 47 +++++++++++++++++++
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c  | 31 ++++++++----
>  4 files changed, 94 insertions(+), 15 deletions(-)
> 
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> index c2d00c3dd044..1c528b7aefdf 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> @@ -176,11 +176,13 @@ struct st_lsm6dsx_hw_ts_settings {
>   * @pullup_en: i2c controller pull-up register info (addr + mask).
>   * @aux_sens: aux sensor register info (addr + mask).
>   * @wr_once: write_once register info (addr + mask).
> + * @emb_func:  embedded function register info (addr + mask).
>   * @num_ext_dev: max number of slave devices.
>   * @shub_out: sensor hub first output register info.
>   * @slv0_addr: slave0 address in secondary page.
>   * @dw_slv0_addr: slave0 write register address in secondary page.
>   * @batch_en: Enable/disable FIFO batching.
> + * @pause: controller pause value.
>   */
>  struct st_lsm6dsx_shub_settings {
>  	struct st_lsm6dsx_reg page_mux;
> @@ -196,6 +198,7 @@ struct st_lsm6dsx_shub_settings {
>  	} pullup_en;
>  	struct st_lsm6dsx_reg aux_sens;
>  	struct st_lsm6dsx_reg wr_once;
> +	struct st_lsm6dsx_reg emb_func;
>  	u8 num_ext_dev;
>  	struct {
>  		bool sec_page;
> @@ -204,6 +207,7 @@ struct st_lsm6dsx_shub_settings {
>  	u8 slv0_addr;
>  	u8 dw_slv0_addr;
>  	u8 batch_en;
> +	u8 pause;
>  };
>  
>  struct st_lsm6dsx_event_settings {
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> index d416990ae309..cc2b19b60d61 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> @@ -332,12 +332,13 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
>   */
>  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
>  {
> +	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
> +	int err, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
>  	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
>  	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
> -	int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
> -	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
>  	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
>  	u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
> +	u8 ext_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
>  	bool reset_ts = false;
>  	__le16 fifo_status;
>  	s64 ts = 0;
> @@ -360,6 +361,8 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
>  
>  	acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
>  	gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
> +	if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
> +		ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
>  
>  	for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
>  		err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
> @@ -387,12 +390,13 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
>  		 * following pattern is repeated every 9 samples:
>  		 *   - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
>  		 */
> +		ext_sip = ext_sensor ? ext_sensor->sip : 0;
>  		gyro_sip = gyro_sensor->sip;
>  		acc_sip = acc_sensor->sip;
>  		ts_sip = hw->ts_sip;
>  		offset = 0;
>  
> -		while (acc_sip > 0 || gyro_sip > 0) {
> +		while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
>  			if (gyro_sip > 0) {
>  				memcpy(gyro_buff, &hw->buff[offset],
>  				       ST_LSM6DSX_SAMPLE_SIZE);
> @@ -403,6 +407,11 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
>  				       ST_LSM6DSX_SAMPLE_SIZE);
>  				offset += ST_LSM6DSX_SAMPLE_SIZE;
>  			}
> +			if (ext_sip > 0) {
> +				memcpy(ext_buff, &hw->buff[offset],
> +				       ST_LSM6DSX_SAMPLE_SIZE);
> +				offset += ST_LSM6DSX_SAMPLE_SIZE;
> +			}
>  
>  			if (ts_sip-- > 0) {
>  				u8 data[ST_LSM6DSX_SAMPLE_SIZE];
> @@ -436,6 +445,10 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
>  				iio_push_to_buffers_with_timestamp(
>  					hw->iio_devs[ST_LSM6DSX_ID_ACC],
>  					acc_buff, acc_sensor->ts_ref + ts);
> +			if (ext_sip-- > 0)
> +				iio_push_to_buffers_with_timestamp(
> +					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
> +					ext_buff, ext_sensor->ts_ref + ts);
>  		}
>  	}
>  
> @@ -628,12 +641,12 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
>  		err = st_lsm6dsx_sensor_set_enable(sensor, enable);
>  		if (err < 0)
>  			goto out;
> -
> -		err = st_lsm6dsx_set_fifo_odr(sensor, enable);
> -		if (err < 0)
> -			goto out;
>  	}
>  

Why this change?  I'm not sure what the original logic was so hard to follow ;)

> +	err = st_lsm6dsx_set_fifo_odr(sensor, enable);
> +	if (err < 0)
> +		goto out;
> +
>  	err = st_lsm6dsx_update_decimators(hw);
>  	if (err < 0)
>  		goto out;
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> index 679831d84d78..1f00b0d74b25 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> @@ -655,6 +655,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
>  				.addr = 0x08,
>  				.mask = GENMASK(5, 3),
>  			},
> +			[ST_LSM6DSX_ID_EXT0] = {
> +				.addr = 0x09,
> +				.mask = GENMASK(2, 0),
> +			},
>  		},
>  		.fifo_ops = {
>  			.update_fifo = st_lsm6dsx_update_fifo,
> @@ -687,6 +691,39 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
>  				.mask = GENMASK(5, 3),
>  			},
>  		},
> +		.shub_settings = {
> +			.page_mux = {
> +				.addr = 0x01,
> +				.mask = BIT(7),
> +			},
> +			.master_en = {
> +				.addr = 0x1a,
> +				.mask = BIT(0),
> +			},
> +			.pullup_en = {
> +				.addr = 0x1a,
> +				.mask = BIT(3),
> +			},
> +			.aux_sens = {
> +				.addr = 0x04,
> +				.mask = GENMASK(5, 4),
> +			},
> +			.wr_once = {
> +				.addr = 0x07,
> +				.mask = BIT(5),
> +			},
> +			.emb_func = {
> +				.addr = 0x19,
> +				.mask = BIT(2),
> +			},
> +			.num_ext_dev = 1,
> +			.shub_out = {
> +				.addr = 0x2e,
> +			},
> +			.slv0_addr = 0x02,
> +			.dw_slv0_addr = 0x0e,
> +			.pause = 0x7,
> +		},
>  		.event_settings = {
>  			.enable_reg = {
>  				.addr = 0x58,
> @@ -1901,6 +1938,16 @@ static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
>  					 hub_settings->aux_sens.mask, data);
>  
>  		st_lsm6dsx_set_page(hw, false);
> +
> +		if (err < 0)
> +			return err;
> +	}
> +
> +	if (hub_settings->emb_func.addr) {
> +		data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->emb_func.mask);
> +		err = regmap_update_bits(hw->regmap,
> +					 hub_settings->emb_func.addr,
> +					 hub_settings->emb_func.mask, data);
>  	}
>  
>  	return err;
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
> index dc739dfb36f2..eea555617d4a 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
> @@ -221,16 +221,21 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
>  		     u8 *data, int len)
>  {
>  	const struct st_lsm6dsx_shub_settings *hub_settings;
> +	u8 config[3], slv_addr, slv_config = 0;
>  	struct st_lsm6dsx_hw *hw = sensor->hw;
> -	u8 config[3], slv_addr;
> +	const struct st_lsm6dsx_reg *aux_sens;
>  	int err;
>  
>  	hub_settings = &hw->settings->shub_settings;
>  	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
> +	aux_sens = &hw->settings->shub_settings.aux_sens;
> +	/* do not overwrite aux_sens */
> +	if (slv_addr + 2 == aux_sens->addr)
> +		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
>  
>  	config[0] = (sensor->ext_info.addr << 1) | 1;
>  	config[1] = addr;
> -	config[2] = len & ST_LS6DSX_READ_OP_MASK;
> +	config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config;
>  
>  	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
>  					sizeof(config));
> @@ -248,7 +253,9 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
>  
>  	st_lsm6dsx_shub_master_enable(sensor, false);
>  
> -	memset(config, 0, sizeof(config));
> +	config[0] = hub_settings->pause;
> +	config[1] = 0;
> +	config[2] = slv_config;
>  	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
>  					 sizeof(config));
>  }
> @@ -305,7 +312,8 @@ st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
>  		st_lsm6dsx_shub_master_enable(sensor, false);
>  	}
>  
> -	memset(config, 0, sizeof(config));
> +	config[0] = hub_settings->pause;
> +	config[1] = 0;
>  	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
>  }
>  
> @@ -697,14 +705,19 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
>  			  const struct st_lsm6dsx_ext_dev_settings *settings)
>  {
>  	const struct st_lsm6dsx_shub_settings *hub_settings;
> +	u8 config[3], data, slv_addr, slv_config = 0;
> +	const struct st_lsm6dsx_reg *aux_sens;
>  	struct st_lsm6dsx_sensor *sensor;
> -	u8 config[3], data, slv_addr;
>  	bool found = false;
>  	int i, err;
>  
> +	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
>  	hub_settings = &hw->settings->shub_settings;
> +	aux_sens = &hw->settings->shub_settings.aux_sens;
>  	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
> -	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
> +	/* do not overwrite aux_sens */
> +	if (slv_addr + 2 == aux_sens->addr)
> +		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
>  
>  	for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
>  		if (!settings->i2c_addr[i])
> @@ -713,7 +726,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
>  		/* read wai slave register */
>  		config[0] = (settings->i2c_addr[i] << 1) | 0x1;
>  		config[1] = settings->wai.addr;
> -		config[2] = 0x1;
> +		config[2] = 0x1 | slv_config;
>  
>  		err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
>  						sizeof(config));
> @@ -742,7 +755,9 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
>  	}
>  
>  	/* reset SLV0 channel */
> -	memset(config, 0, sizeof(config));
> +	config[0] = hub_settings->pause;
> +	config[1] = 0;
> +	config[2] = slv_config;
>  	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
>  					sizeof(config));
>  	if (err < 0)

Lorenzo Bianconi Dec. 23, 2019, 5:25 p.m. UTC | #2

> On Sat, 14 Dec 2019 17:52:59 +0100
> Lorenzo Bianconi <[e-mail]> wrote:
> 
> > Enabled i2c master controller support for LSM6DSM sensor. Enable
> > ext_sensor0 for lsm6dsm. This series has been tested using LIS2MDL as
> > slave device connected to the i2c controller of the LSM6DSM
> > 
> > Signed-off-by: Lorenzo Bianconi <[e-mail]>
> One question inline.  However I'm happy enough to take it with that
> outstanding on basis I am not going to push out as non rebasing for
> a while yet.
> 
> Applied to the togreg branch of iio.git and pushed out as testing for
> the autobuilders to play with it.
> 
> Thanks,
> 
> Jonathan
>  
> > ---

[...]

> >  				iio_push_to_buffers_with_timestamp(
> >  					hw->iio_devs[ST_LSM6DSX_ID_ACC],
> >  					acc_buff, acc_sensor->ts_ref + ts);
> > +			if (ext_sip-- > 0)
> > +				iio_push_to_buffers_with_timestamp(
> > +					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
> > +					ext_buff, ext_sensor->ts_ref + ts);
> >  		}
> >  	}
> >  
> > @@ -628,12 +641,12 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
> >  		err = st_lsm6dsx_sensor_set_enable(sensor, enable);
> >  		if (err < 0)
> >  			goto out;
> > -
> > -		err = st_lsm6dsx_set_fifo_odr(sensor, enable);
> > -		if (err < 0)
> > -			goto out;
> >  	}
> >  
> 
> Why this change?  I'm not sure what the original logic was so hard to follow ;)

Hi Jonathan,

we need to set FIFO odr to 1.66KHz for LSM6DSM even if we enable just the sensor hub
(and not other sensors).

Regards,
Lorenzo

> 
> > +	err = st_lsm6dsx_set_fifo_odr(sensor, enable);
> > +	if (err < 0)
> > +		goto out;
> > +
> >  	err = st_lsm6dsx_update_decimators(hw);
> >  	if (err < 0)
> >  		goto out;
> > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> > index 679831d84d78..1f00b0d74b25 100644
> > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> > @@ -655,6 +655,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
> >  				.addr = 0x08,
> >  				.mask = GENMASK(5, 3),
> >  			},
> > +			[ST_LSM6DSX_ID_EXT0] = {
> > +				.addr = 0x09,
> > +				.mask = GENMASK(2, 0),
> > +			},
> >  		},
> >  		.fifo_ops = {
> >  			.update_fifo = st_lsm6dsx_update_fifo,
> > @@ -687,6 +691,39 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
> >  				.mask = GENMASK(5, 3),
> >  			},
> >  		},
> > +		.shub_settings = {
> > +			.page_mux = {
> > +				.addr = 0x01,
> > +				.mask = BIT(7),
> > +			},
> > +			.master_en = {
> > +				.addr = 0x1a,
> > +				.mask = BIT(0),
> > +			},
> > +			.pullup_en = {
> > +				.addr = 0x1a,
> > +				.mask = BIT(3),
> > +			},
> > +			.aux_sens = {
> > +				.addr = 0x04,
> > +				.mask = GENMASK(5, 4),
> > +			},
> > +			.wr_once = {
> > +				.addr = 0x07,
> > +				.mask = BIT(5),
> > +			},
> > +			.emb_func = {
> > +				.addr = 0x19,
> > +				.mask = BIT(2),
> > +			},
> > +			.num_ext_dev = 1,
> > +			.shub_out = {
> > +				.addr = 0x2e,
> > +			},
> > +			.slv0_addr = 0x02,
> > +			.dw_slv0_addr = 0x0e,
> > +			.pause = 0x7,
> > +		},
> >  		.event_settings = {
> >  			.enable_reg = {
> >  				.addr = 0x58,
> > @@ -1901,6 +1938,16 @@ static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
> >  					 hub_settings->aux_sens.mask, data);
> >  
> >  		st_lsm6dsx_set_page(hw, false);
> > +
> > +		if (err < 0)
> > +			return err;
> > +	}
> > +
> > +	if (hub_settings->emb_func.addr) {
> > +		data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->emb_func.mask);
> > +		err = regmap_update_bits(hw->regmap,
> > +					 hub_settings->emb_func.addr,
> > +					 hub_settings->emb_func.mask, data);
> >  	}
> >  
> >  	return err;
> > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
> > index dc739dfb36f2..eea555617d4a 100644
> > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
> > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
> > @@ -221,16 +221,21 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
> >  		     u8 *data, int len)
> >  {
> >  	const struct st_lsm6dsx_shub_settings *hub_settings;
> > +	u8 config[3], slv_addr, slv_config = 0;
> >  	struct st_lsm6dsx_hw *hw = sensor->hw;
> > -	u8 config[3], slv_addr;
> > +	const struct st_lsm6dsx_reg *aux_sens;
> >  	int err;
> >  
> >  	hub_settings = &hw->settings->shub_settings;
> >  	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
> > +	aux_sens = &hw->settings->shub_settings.aux_sens;
> > +	/* do not overwrite aux_sens */
> > +	if (slv_addr + 2 == aux_sens->addr)
> > +		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
> >  
> >  	config[0] = (sensor->ext_info.addr << 1) | 1;
> >  	config[1] = addr;
> > -	config[2] = len & ST_LS6DSX_READ_OP_MASK;
> > +	config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config;
> >  
> >  	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
> >  					sizeof(config));
> > @@ -248,7 +253,9 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
> >  
> >  	st_lsm6dsx_shub_master_enable(sensor, false);
> >  
> > -	memset(config, 0, sizeof(config));
> > +	config[0] = hub_settings->pause;
> > +	config[1] = 0;
> > +	config[2] = slv_config;
> >  	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
> >  					 sizeof(config));
> >  }
> > @@ -305,7 +312,8 @@ st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
> >  		st_lsm6dsx_shub_master_enable(sensor, false);
> >  	}
> >  
> > -	memset(config, 0, sizeof(config));
> > +	config[0] = hub_settings->pause;
> > +	config[1] = 0;
> >  	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
> >  }
> >  
> > @@ -697,14 +705,19 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
> >  			  const struct st_lsm6dsx_ext_dev_settings *settings)
> >  {
> >  	const struct st_lsm6dsx_shub_settings *hub_settings;
> > +	u8 config[3], data, slv_addr, slv_config = 0;
> > +	const struct st_lsm6dsx_reg *aux_sens;
> >  	struct st_lsm6dsx_sensor *sensor;
> > -	u8 config[3], data, slv_addr;
> >  	bool found = false;
> >  	int i, err;
> >  
> > +	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
> >  	hub_settings = &hw->settings->shub_settings;
> > +	aux_sens = &hw->settings->shub_settings.aux_sens;
> >  	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
> > -	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
> > +	/* do not overwrite aux_sens */
> > +	if (slv_addr + 2 == aux_sens->addr)
> > +		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
> >  
> >  	for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
> >  		if (!settings->i2c_addr[i])
> > @@ -713,7 +726,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
> >  		/* read wai slave register */
> >  		config[0] = (settings->i2c_addr[i] << 1) | 0x1;
> >  		config[1] = settings->wai.addr;
> > -		config[2] = 0x1;
> > +		config[2] = 0x1 | slv_config;
> >  
> >  		err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
> >  						sizeof(config));
> > @@ -742,7 +755,9 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
> >  	}
> >  
> >  	/* reset SLV0 channel */
> > -	memset(config, 0, sizeof(config));
> > +	config[0] = hub_settings->pause;
> > +	config[1] = 0;
> > +	config[2] = slv_config;
> >  	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
> >  					sizeof(config));
> >  	if (err < 0)
>

Patch

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index c2d00c3dd044..1c528b7aefdf 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -176,11 +176,13 @@  struct st_lsm6dsx_hw_ts_settings {
  * @pullup_en: i2c controller pull-up register info (addr + mask).
  * @aux_sens: aux sensor register info (addr + mask).
  * @wr_once: write_once register info (addr + mask).
+ * @emb_func:  embedded function register info (addr + mask).
  * @num_ext_dev: max number of slave devices.
  * @shub_out: sensor hub first output register info.
  * @slv0_addr: slave0 address in secondary page.
  * @dw_slv0_addr: slave0 write register address in secondary page.
  * @batch_en: Enable/disable FIFO batching.
+ * @pause: controller pause value.
  */
 struct st_lsm6dsx_shub_settings {
 	struct st_lsm6dsx_reg page_mux;
@@ -196,6 +198,7 @@  struct st_lsm6dsx_shub_settings {
 	} pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
+	struct st_lsm6dsx_reg emb_func;
 	u8 num_ext_dev;
 	struct {
 		bool sec_page;
@@ -204,6 +207,7 @@  struct st_lsm6dsx_shub_settings {
 	u8 slv0_addr;
 	u8 dw_slv0_addr;
 	u8 batch_en;
+	u8 pause;
 };
 
 struct st_lsm6dsx_event_settings {
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index d416990ae309..cc2b19b60d61 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -332,12 +332,13 @@  static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
  */
 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 {
+	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
+	int err, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
 	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
 	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
-	int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
-	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
 	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
 	u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
+	u8 ext_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
 	bool reset_ts = false;
 	__le16 fifo_status;
 	s64 ts = 0;
@@ -360,6 +361,8 @@  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 
 	acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 	gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
+	if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
+		ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
 
 	for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
 		err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
@@ -387,12 +390,13 @@  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 		 * following pattern is repeated every 9 samples:
 		 *   - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
 		 */
+		ext_sip = ext_sensor ? ext_sensor->sip : 0;
 		gyro_sip = gyro_sensor->sip;
 		acc_sip = acc_sensor->sip;
 		ts_sip = hw->ts_sip;
 		offset = 0;
 
-		while (acc_sip > 0 || gyro_sip > 0) {
+		while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
 			if (gyro_sip > 0) {
 				memcpy(gyro_buff, &hw->buff[offset],
 				       ST_LSM6DSX_SAMPLE_SIZE);
@@ -403,6 +407,11 @@  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 				       ST_LSM6DSX_SAMPLE_SIZE);
 				offset += ST_LSM6DSX_SAMPLE_SIZE;
 			}
+			if (ext_sip > 0) {
+				memcpy(ext_buff, &hw->buff[offset],
+				       ST_LSM6DSX_SAMPLE_SIZE);
+				offset += ST_LSM6DSX_SAMPLE_SIZE;
+			}
 
 			if (ts_sip-- > 0) {
 				u8 data[ST_LSM6DSX_SAMPLE_SIZE];
@@ -436,6 +445,10 @@  int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 				iio_push_to_buffers_with_timestamp(
 					hw->iio_devs[ST_LSM6DSX_ID_ACC],
 					acc_buff, acc_sensor->ts_ref + ts);
+			if (ext_sip-- > 0)
+				iio_push_to_buffers_with_timestamp(
+					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
+					ext_buff, ext_sensor->ts_ref + ts);
 		}
 	}
 
@@ -628,12 +641,12 @@  int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 		err = st_lsm6dsx_sensor_set_enable(sensor, enable);
 		if (err < 0)
 			goto out;
-
-		err = st_lsm6dsx_set_fifo_odr(sensor, enable);
-		if (err < 0)
-			goto out;
 	}
 
+	err = st_lsm6dsx_set_fifo_odr(sensor, enable);
+	if (err < 0)
+		goto out;
+
 	err = st_lsm6dsx_update_decimators(hw);
 	if (err < 0)
 		goto out;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 679831d84d78..1f00b0d74b25 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -655,6 +655,10 @@  static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.addr = 0x08,
 				.mask = GENMASK(5, 3),
 			},
+			[ST_LSM6DSX_ID_EXT0] = {
+				.addr = 0x09,
+				.mask = GENMASK(2, 0),
+			},
 		},
 		.fifo_ops = {
 			.update_fifo = st_lsm6dsx_update_fifo,
@@ -687,6 +691,39 @@  static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = GENMASK(5, 3),
 			},
 		},
+		.shub_settings = {
+			.page_mux = {
+				.addr = 0x01,
+				.mask = BIT(7),
+			},
+			.master_en = {
+				.addr = 0x1a,
+				.mask = BIT(0),
+			},
+			.pullup_en = {
+				.addr = 0x1a,
+				.mask = BIT(3),
+			},
+			.aux_sens = {
+				.addr = 0x04,
+				.mask = GENMASK(5, 4),
+			},
+			.wr_once = {
+				.addr = 0x07,
+				.mask = BIT(5),
+			},
+			.emb_func = {
+				.addr = 0x19,
+				.mask = BIT(2),
+			},
+			.num_ext_dev = 1,
+			.shub_out = {
+				.addr = 0x2e,
+			},
+			.slv0_addr = 0x02,
+			.dw_slv0_addr = 0x0e,
+			.pause = 0x7,
+		},
 		.event_settings = {
 			.enable_reg = {
 				.addr = 0x58,
@@ -1901,6 +1938,16 @@  static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
 					 hub_settings->aux_sens.mask, data);
 
 		st_lsm6dsx_set_page(hw, false);
+
+		if (err < 0)
+			return err;
+	}
+
+	if (hub_settings->emb_func.addr) {
+		data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->emb_func.mask);
+		err = regmap_update_bits(hw->regmap,
+					 hub_settings->emb_func.addr,
+					 hub_settings->emb_func.mask, data);
 	}
 
 	return err;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
index dc739dfb36f2..eea555617d4a 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
@@ -221,16 +221,21 @@  st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 		     u8 *data, int len)
 {
 	const struct st_lsm6dsx_shub_settings *hub_settings;
+	u8 config[3], slv_addr, slv_config = 0;
 	struct st_lsm6dsx_hw *hw = sensor->hw;
-	u8 config[3], slv_addr;
+	const struct st_lsm6dsx_reg *aux_sens;
 	int err;
 
 	hub_settings = &hw->settings->shub_settings;
 	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
+	aux_sens = &hw->settings->shub_settings.aux_sens;
+	/* do not overwrite aux_sens */
+	if (slv_addr + 2 == aux_sens->addr)
+		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
 
 	config[0] = (sensor->ext_info.addr << 1) | 1;
 	config[1] = addr;
-	config[2] = len & ST_LS6DSX_READ_OP_MASK;
+	config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config;
 
 	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					sizeof(config));
@@ -248,7 +253,9 @@  st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 
 	st_lsm6dsx_shub_master_enable(sensor, false);
 
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
+	config[2] = slv_config;
 	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					 sizeof(config));
 }
@@ -305,7 +312,8 @@  st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
 		st_lsm6dsx_shub_master_enable(sensor, false);
 	}
 
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
 	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
 }
 
@@ -697,14 +705,19 @@  st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 			  const struct st_lsm6dsx_ext_dev_settings *settings)
 {
 	const struct st_lsm6dsx_shub_settings *hub_settings;
+	u8 config[3], data, slv_addr, slv_config = 0;
+	const struct st_lsm6dsx_reg *aux_sens;
 	struct st_lsm6dsx_sensor *sensor;
-	u8 config[3], data, slv_addr;
 	bool found = false;
 	int i, err;
 
+	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 	hub_settings = &hw->settings->shub_settings;
+	aux_sens = &hw->settings->shub_settings.aux_sens;
 	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
-	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+	/* do not overwrite aux_sens */
+	if (slv_addr + 2 == aux_sens->addr)
+		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
 
 	for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
 		if (!settings->i2c_addr[i])
@@ -713,7 +726,7 @@  st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 		/* read wai slave register */
 		config[0] = (settings->i2c_addr[i] << 1) | 0x1;
 		config[1] = settings->wai.addr;
-		config[2] = 0x1;
+		config[2] = 0x1 | slv_config;
 
 		err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 						sizeof(config));
@@ -742,7 +755,9 @@  st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 	}
 
 	/* reset SLV0 channel */
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
+	config[2] = slv_config;
 	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					sizeof(config));
 	if (err < 0)