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

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.  Hence we must separately track the fifo state.

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>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 510c4bd..dc55d7d 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -349,9 +349,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.
@@ -371,9 +371,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 a70b04f..cb536b8 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -187,17 +187,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,
@@ -619,11 +612,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;
@@ -653,15 +652,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 11b2c7b..6f628c3 100644 (file)
--- 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;