aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h4
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c25
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c4
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 510c4bd4a785..dc55d7dff3eb 100644
--- 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 a70b04f14187..cb536b81a1c2 100644
--- 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 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;