diff mbox series

iio: imu: st_lsm6dsx: track hw FIFO buffering with fifo_mask

Message ID 844cd314df8ff354859bad1e43b13778f60b08bb.1574954314.git.lorenzo@kernel.org (mailing list archive)
State New, archived
Headers show
Series iio: imu: st_lsm6dsx: track hw FIFO buffering with fifo_mask | expand

Commit Message

Lorenzo Bianconi Nov. 28, 2019, 3:42 p.m. UTC
Track hw FIFO state introducing fifo_mask since now the accel sensor
can be enabled during suspend/resume in order to trigger the wake-up
enabling the FIFO in st_lsm6dsx_resume even if it was disabled before
the suspend

Fixes: 4c997dfa692d ("iio: imu: st_lsm6dsx: add wakeup-source option")
Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  4 +--
 .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 25 +++++++++++--------
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  |  4 +--
 3 files changed, 18 insertions(+), 15 deletions(-)

Comments

Sean Nyekjaer Nov. 29, 2019, 9:13 a.m. UTC | #1
On 28/11/2019 16.42, Lorenzo Bianconi wrote:
> Track hw FIFO state introducing fifo_mask since now the accel sensor
> can be enabled during suspend/resume in order to trigger the wake-up
> enabling the FIFO in st_lsm6dsx_resume even if it was disabled before
> the suspend
> 
> Fixes: 4c997dfa692d ("iio: imu: st_lsm6dsx: add wakeup-source option")
> Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
Tested-by: Sean Nyekjaer <sean@geanix.com>
> ---
>   drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  4 +--
>   .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 25 +++++++++++--------
>   drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  |  4 +--
>   3 files changed, 18 insertions(+), 15 deletions(-)
> 
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> index c605b153be41..b54aefcdaad4 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> @@ -351,9 +351,9 @@ struct st_lsm6dsx_sensor {
>    * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
>    * @conf_lock: Mutex to prevent concurrent FIFO configuration update.
>    * @page_lock: Mutex to prevent concurrent memory page configuration.
> - * @fifo_mode: FIFO operating mode supported by the device.
>    * @suspend_mask: Suspended sensor bitmask.
>    * @enable_mask: Enabled sensor bitmask.
> + * @fifo_mask: Enabled hw FIFO 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.
> @@ -373,9 +373,9 @@ struct st_lsm6dsx_hw {
>   	struct mutex conf_lock;
>   	struct mutex page_lock;
>   
> -	enum st_lsm6dsx_fifo_mode fifo_mode;
>   	u8 suspend_mask;
>   	u8 enable_mask;
> +	u8 fifo_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 d416990ae309..bfd4c6306c0b 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> @@ -176,17 +176,10 @@ int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
>   			     enum st_lsm6dsx_fifo_mode fifo_mode)
>   {
>   	unsigned int data;
> -	int err;
>   
>   	data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
> -	err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
> -					    ST_LSM6DSX_FIFO_MODE_MASK, data);
> -	if (err < 0)
> -		return err;
> -
> -	hw->fifo_mode = fifo_mode;
> -
> -	return 0;
> +	return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
> +					     ST_LSM6DSX_FIFO_MODE_MASK, data);
>   }
>   
>   static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
> @@ -608,11 +601,17 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
>   int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
>   {
>   	struct st_lsm6dsx_hw *hw = sensor->hw;
> +	u8 fifo_mask;
>   	int err;
>   
>   	mutex_lock(&hw->conf_lock);
>   
> -	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
> +	if (enable)
> +		fifo_mask = hw->fifo_mask | BIT(sensor->id);
> +	else
> +		fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
> +
> +	if (hw->fifo_mask) {
>   		err = st_lsm6dsx_flush_fifo(hw);
>   		if (err < 0)
>   			goto out;
> @@ -642,15 +641,19 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
>   	if (err < 0)
>   		goto out;
>   
> -	if (hw->enable_mask) {
> +	if (fifo_mask) {
>   		/* reset hw ts counter */
>   		err = st_lsm6dsx_reset_hw_ts(hw);
>   		if (err < 0)
>   			goto out;
>   
>   		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
> +		if (err < 0)
> +			goto out;
>   	}
>   
> +	hw->fifo_mask = fifo_mask;
> +
>   out:
>   	mutex_unlock(&hw->conf_lock);
>   
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> index 11b2c7bc8041..6f628c3cd133 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> @@ -2300,7 +2300,7 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
>   		hw->suspend_mask |= BIT(sensor->id);
>   	}
>   
> -	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
> +	if (hw->fifo_mask)
>   		err = st_lsm6dsx_flush_fifo(hw);
>   
>   	return err;
> @@ -2336,7 +2336,7 @@ static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
>   		hw->suspend_mask &= ~BIT(sensor->id);
>   	}
>   
> -	if (hw->enable_mask)
> +	if (hw->fifo_mask)
>   		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
>   
>   	return err;
>
Jonathan Cameron Dec. 1, 2019, 8:49 p.m. UTC | #2
On Thu, 28 Nov 2019 17:42:30 +0200
Lorenzo Bianconi <lorenzo@kernel.org> wrote:

> Track hw FIFO state introducing fifo_mask since now the accel sensor
> can be enabled during suspend/resume in order to trigger the wake-up
> enabling the FIFO in st_lsm6dsx_resume even if it was disabled before
> the suspend

What is the user visible result of not having this patch?

Otherwise looks fine to me.

Jonathan

> 
> Fixes: 4c997dfa692d ("iio: imu: st_lsm6dsx: add wakeup-source option")
> Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
> ---
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  4 +--
>  .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 25 +++++++++++--------
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  |  4 +--
>  3 files changed, 18 insertions(+), 15 deletions(-)
> 
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> index c605b153be41..b54aefcdaad4 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> @@ -351,9 +351,9 @@ struct st_lsm6dsx_sensor {
>   * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
>   * @conf_lock: Mutex to prevent concurrent FIFO configuration update.
>   * @page_lock: Mutex to prevent concurrent memory page configuration.
> - * @fifo_mode: FIFO operating mode supported by the device.
>   * @suspend_mask: Suspended sensor bitmask.
>   * @enable_mask: Enabled sensor bitmask.
> + * @fifo_mask: Enabled hw FIFO 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.
> @@ -373,9 +373,9 @@ struct st_lsm6dsx_hw {
>  	struct mutex conf_lock;
>  	struct mutex page_lock;
>  
> -	enum st_lsm6dsx_fifo_mode fifo_mode;
>  	u8 suspend_mask;
>  	u8 enable_mask;
> +	u8 fifo_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 d416990ae309..bfd4c6306c0b 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> @@ -176,17 +176,10 @@ int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
>  			     enum st_lsm6dsx_fifo_mode fifo_mode)
>  {
>  	unsigned int data;
> -	int err;
>  
>  	data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
> -	err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
> -					    ST_LSM6DSX_FIFO_MODE_MASK, data);
> -	if (err < 0)
> -		return err;
> -
> -	hw->fifo_mode = fifo_mode;
> -
> -	return 0;
> +	return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
> +					     ST_LSM6DSX_FIFO_MODE_MASK, data);
>  }
>  
>  static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
> @@ -608,11 +601,17 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
>  int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
>  {
>  	struct st_lsm6dsx_hw *hw = sensor->hw;
> +	u8 fifo_mask;
>  	int err;
>  
>  	mutex_lock(&hw->conf_lock);
>  
> -	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
> +	if (enable)
> +		fifo_mask = hw->fifo_mask | BIT(sensor->id);
> +	else
> +		fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
> +
> +	if (hw->fifo_mask) {
>  		err = st_lsm6dsx_flush_fifo(hw);
>  		if (err < 0)
>  			goto out;
> @@ -642,15 +641,19 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
>  	if (err < 0)
>  		goto out;
>  
> -	if (hw->enable_mask) {
> +	if (fifo_mask) {
>  		/* reset hw ts counter */
>  		err = st_lsm6dsx_reset_hw_ts(hw);
>  		if (err < 0)
>  			goto out;
>  
>  		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
> +		if (err < 0)
> +			goto out;
>  	}
>  
> +	hw->fifo_mask = fifo_mask;
> +
>  out:
>  	mutex_unlock(&hw->conf_lock);
>  
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> index 11b2c7bc8041..6f628c3cd133 100644
> --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> @@ -2300,7 +2300,7 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
>  		hw->suspend_mask |= BIT(sensor->id);
>  	}
>  
> -	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
> +	if (hw->fifo_mask)
>  		err = st_lsm6dsx_flush_fifo(hw);
>  
>  	return err;
> @@ -2336,7 +2336,7 @@ static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
>  		hw->suspend_mask &= ~BIT(sensor->id);
>  	}
>  
> -	if (hw->enable_mask)
> +	if (hw->fifo_mask)
>  		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
>  
>  	return err;
Lorenzo Bianconi Dec. 1, 2019, 9:04 p.m. UTC | #3
> On Thu, 28 Nov 2019 17:42:30 +0200
> Lorenzo Bianconi <lorenzo@kernel.org> wrote:
> 
> > Track hw FIFO state introducing fifo_mask since now the accel sensor
> > can be enabled during suspend/resume in order to trigger the wake-up
> > enabling the FIFO in st_lsm6dsx_resume even if it was disabled before
> > the suspend
> 
> What is the user visible result of not having this patch?
> 
> Otherwise looks fine to me.

Hi Jonathan,

I discovered the issue looking at the code. The main point here is after commit
4c997dfa692d enable_mask can be modified even by event code and not just by
FIFO one, so it can't be used to track FIFO status (e.g during suspend-resume).

Regards,
Lorenzo

> 
> Jonathan
> 
> > 
> > Fixes: 4c997dfa692d ("iio: imu: st_lsm6dsx: add wakeup-source option")
> > Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
> > ---
> >  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  4 +--
> >  .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 25 +++++++++++--------
> >  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  |  4 +--
> >  3 files changed, 18 insertions(+), 15 deletions(-)
> > 
> > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > index c605b153be41..b54aefcdaad4 100644
> > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > @@ -351,9 +351,9 @@ struct st_lsm6dsx_sensor {
> >   * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
> >   * @conf_lock: Mutex to prevent concurrent FIFO configuration update.
> >   * @page_lock: Mutex to prevent concurrent memory page configuration.
> > - * @fifo_mode: FIFO operating mode supported by the device.
> >   * @suspend_mask: Suspended sensor bitmask.
> >   * @enable_mask: Enabled sensor bitmask.
> > + * @fifo_mask: Enabled hw FIFO 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.
> > @@ -373,9 +373,9 @@ struct st_lsm6dsx_hw {
> >  	struct mutex conf_lock;
> >  	struct mutex page_lock;
> >  
> > -	enum st_lsm6dsx_fifo_mode fifo_mode;
> >  	u8 suspend_mask;
> >  	u8 enable_mask;
> > +	u8 fifo_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 d416990ae309..bfd4c6306c0b 100644
> > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > @@ -176,17 +176,10 @@ int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
> >  			     enum st_lsm6dsx_fifo_mode fifo_mode)
> >  {
> >  	unsigned int data;
> > -	int err;
> >  
> >  	data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
> > -	err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
> > -					    ST_LSM6DSX_FIFO_MODE_MASK, data);
> > -	if (err < 0)
> > -		return err;
> > -
> > -	hw->fifo_mode = fifo_mode;
> > -
> > -	return 0;
> > +	return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
> > +					     ST_LSM6DSX_FIFO_MODE_MASK, data);
> >  }
> >  
> >  static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
> > @@ -608,11 +601,17 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
> >  int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
> >  {
> >  	struct st_lsm6dsx_hw *hw = sensor->hw;
> > +	u8 fifo_mask;
> >  	int err;
> >  
> >  	mutex_lock(&hw->conf_lock);
> >  
> > -	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
> > +	if (enable)
> > +		fifo_mask = hw->fifo_mask | BIT(sensor->id);
> > +	else
> > +		fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
> > +
> > +	if (hw->fifo_mask) {
> >  		err = st_lsm6dsx_flush_fifo(hw);
> >  		if (err < 0)
> >  			goto out;
> > @@ -642,15 +641,19 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
> >  	if (err < 0)
> >  		goto out;
> >  
> > -	if (hw->enable_mask) {
> > +	if (fifo_mask) {
> >  		/* reset hw ts counter */
> >  		err = st_lsm6dsx_reset_hw_ts(hw);
> >  		if (err < 0)
> >  			goto out;
> >  
> >  		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
> > +		if (err < 0)
> > +			goto out;
> >  	}
> >  
> > +	hw->fifo_mask = fifo_mask;
> > +
> >  out:
> >  	mutex_unlock(&hw->conf_lock);
> >  
> > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> > index 11b2c7bc8041..6f628c3cd133 100644
> > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> > @@ -2300,7 +2300,7 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
> >  		hw->suspend_mask |= BIT(sensor->id);
> >  	}
> >  
> > -	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
> > +	if (hw->fifo_mask)
> >  		err = st_lsm6dsx_flush_fifo(hw);
> >  
> >  	return err;
> > @@ -2336,7 +2336,7 @@ static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
> >  		hw->suspend_mask &= ~BIT(sensor->id);
> >  	}
> >  
> > -	if (hw->enable_mask)
> > +	if (hw->fifo_mask)
> >  		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
> >  
> >  	return err;
>
Jonathan Cameron Dec. 1, 2019, 9:16 p.m. UTC | #4
On Sun, 1 Dec 2019 23:04:26 +0200
Lorenzo Bianconi <lorenzo@kernel.org> wrote:

> > On Thu, 28 Nov 2019 17:42:30 +0200
> > Lorenzo Bianconi <lorenzo@kernel.org> wrote:
> >   
> > > Track hw FIFO state introducing fifo_mask since now the accel sensor
> > > can be enabled during suspend/resume in order to trigger the wake-up
> > > enabling the FIFO in st_lsm6dsx_resume even if it was disabled before
> > > the suspend  
> > 
> > What is the user visible result of not having this patch?
> > 
> > Otherwise looks fine to me.  
> 
> Hi Jonathan,
> 
> I discovered the issue looking at the code. The main point here is after commit
> 4c997dfa692d enable_mask can be modified even by event code and not just by
> FIFO one, so it can't be used to track FIFO status (e.g during suspend-resume).
Ah got it now. I tweaked the explanation a tiny bit and applied to the fixes-togreg
branch of iio.git.

I guess I might be able to apply that other fix now as well. Will check.

Thanks,

Jonathan

> 
> Regards,
> Lorenzo
> 
> > 
> > Jonathan
> >   
> > > 
> > > Fixes: 4c997dfa692d ("iio: imu: st_lsm6dsx: add wakeup-source option")
> > > Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
> > > ---
> > >  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  4 +--
> > >  .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 25 +++++++++++--------
> > >  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  |  4 +--
> > >  3 files changed, 18 insertions(+), 15 deletions(-)
> > > 
> > > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > > index c605b153be41..b54aefcdaad4 100644
> > > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> > > @@ -351,9 +351,9 @@ struct st_lsm6dsx_sensor {
> > >   * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
> > >   * @conf_lock: Mutex to prevent concurrent FIFO configuration update.
> > >   * @page_lock: Mutex to prevent concurrent memory page configuration.
> > > - * @fifo_mode: FIFO operating mode supported by the device.
> > >   * @suspend_mask: Suspended sensor bitmask.
> > >   * @enable_mask: Enabled sensor bitmask.
> > > + * @fifo_mask: Enabled hw FIFO 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.
> > > @@ -373,9 +373,9 @@ struct st_lsm6dsx_hw {
> > >  	struct mutex conf_lock;
> > >  	struct mutex page_lock;
> > >  
> > > -	enum st_lsm6dsx_fifo_mode fifo_mode;
> > >  	u8 suspend_mask;
> > >  	u8 enable_mask;
> > > +	u8 fifo_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 d416990ae309..bfd4c6306c0b 100644
> > > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> > > @@ -176,17 +176,10 @@ int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
> > >  			     enum st_lsm6dsx_fifo_mode fifo_mode)
> > >  {
> > >  	unsigned int data;
> > > -	int err;
> > >  
> > >  	data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
> > > -	err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
> > > -					    ST_LSM6DSX_FIFO_MODE_MASK, data);
> > > -	if (err < 0)
> > > -		return err;
> > > -
> > > -	hw->fifo_mode = fifo_mode;
> > > -
> > > -	return 0;
> > > +	return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
> > > +					     ST_LSM6DSX_FIFO_MODE_MASK, data);
> > >  }
> > >  
> > >  static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
> > > @@ -608,11 +601,17 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
> > >  int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
> > >  {
> > >  	struct st_lsm6dsx_hw *hw = sensor->hw;
> > > +	u8 fifo_mask;
> > >  	int err;
> > >  
> > >  	mutex_lock(&hw->conf_lock);
> > >  
> > > -	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
> > > +	if (enable)
> > > +		fifo_mask = hw->fifo_mask | BIT(sensor->id);
> > > +	else
> > > +		fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
> > > +
> > > +	if (hw->fifo_mask) {
> > >  		err = st_lsm6dsx_flush_fifo(hw);
> > >  		if (err < 0)
> > >  			goto out;
> > > @@ -642,15 +641,19 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
> > >  	if (err < 0)
> > >  		goto out;
> > >  
> > > -	if (hw->enable_mask) {
> > > +	if (fifo_mask) {
> > >  		/* reset hw ts counter */
> > >  		err = st_lsm6dsx_reset_hw_ts(hw);
> > >  		if (err < 0)
> > >  			goto out;
> > >  
> > >  		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
> > > +		if (err < 0)
> > > +			goto out;
> > >  	}
> > >  
> > > +	hw->fifo_mask = fifo_mask;
> > > +
> > >  out:
> > >  	mutex_unlock(&hw->conf_lock);
> > >  
> > > diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> > > index 11b2c7bc8041..6f628c3cd133 100644
> > > --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> > > +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> > > @@ -2300,7 +2300,7 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
> > >  		hw->suspend_mask |= BIT(sensor->id);
> > >  	}
> > >  
> > > -	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
> > > +	if (hw->fifo_mask)
> > >  		err = st_lsm6dsx_flush_fifo(hw);
> > >  
> > >  	return err;
> > > @@ -2336,7 +2336,7 @@ static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
> > >  		hw->suspend_mask &= ~BIT(sensor->id);
> > >  	}
> > >  
> > > -	if (hw->enable_mask)
> > > +	if (hw->fifo_mask)
> > >  		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
> > >  
> > >  	return err;  
> >
diff mbox series

Patch

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index c605b153be41..b54aefcdaad4 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -351,9 +351,9 @@  struct st_lsm6dsx_sensor {
  * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
  * @conf_lock: Mutex to prevent concurrent FIFO configuration update.
  * @page_lock: Mutex to prevent concurrent memory page configuration.
- * @fifo_mode: FIFO operating mode supported by the device.
  * @suspend_mask: Suspended sensor bitmask.
  * @enable_mask: Enabled sensor bitmask.
+ * @fifo_mask: Enabled hw FIFO 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.
@@ -373,9 +373,9 @@  struct st_lsm6dsx_hw {
 	struct mutex conf_lock;
 	struct mutex page_lock;
 
-	enum st_lsm6dsx_fifo_mode fifo_mode;
 	u8 suspend_mask;
 	u8 enable_mask;
+	u8 fifo_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 d416990ae309..bfd4c6306c0b 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -176,17 +176,10 @@  int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
 			     enum st_lsm6dsx_fifo_mode fifo_mode)
 {
 	unsigned int data;
-	int err;
 
 	data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
-	err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
-					    ST_LSM6DSX_FIFO_MODE_MASK, data);
-	if (err < 0)
-		return err;
-
-	hw->fifo_mode = fifo_mode;
-
-	return 0;
+	return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
+					     ST_LSM6DSX_FIFO_MODE_MASK, data);
 }
 
 static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
@@ -608,11 +601,17 @@  int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 {
 	struct st_lsm6dsx_hw *hw = sensor->hw;
+	u8 fifo_mask;
 	int err;
 
 	mutex_lock(&hw->conf_lock);
 
-	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
+	if (enable)
+		fifo_mask = hw->fifo_mask | BIT(sensor->id);
+	else
+		fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
+
+	if (hw->fifo_mask) {
 		err = st_lsm6dsx_flush_fifo(hw);
 		if (err < 0)
 			goto out;
@@ -642,15 +641,19 @@  int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 	if (err < 0)
 		goto out;
 
-	if (hw->enable_mask) {
+	if (fifo_mask) {
 		/* reset hw ts counter */
 		err = st_lsm6dsx_reset_hw_ts(hw);
 		if (err < 0)
 			goto out;
 
 		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
+		if (err < 0)
+			goto out;
 	}
 
+	hw->fifo_mask = fifo_mask;
+
 out:
 	mutex_unlock(&hw->conf_lock);
 
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 11b2c7bc8041..6f628c3cd133 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -2300,7 +2300,7 @@  static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
 		hw->suspend_mask |= BIT(sensor->id);
 	}
 
-	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
+	if (hw->fifo_mask)
 		err = st_lsm6dsx_flush_fifo(hw);
 
 	return err;
@@ -2336,7 +2336,7 @@  static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
 		hw->suspend_mask &= ~BIT(sensor->id);
 	}
 
-	if (hw->enable_mask)
+	if (hw->fifo_mask)
 		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
 
 	return err;