iio: imu: st_lsm6dsx: fix PM support for st_lsm6dsx i2c controller

Properly suspend/resume i2c slaves connected to st_lsm6dsx master
controller if the CPU goes in suspended state

Fixes: c91c1c844ebd ("imu: st_lsm6dsx: add i2c embedded controller support")
Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
Cc: <Stable@vger.kernel.org>
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 004a8a1a002793ffaae348e13d14d4208ad24b5e..6ef3577234a00688720b705c5f1e9095a8aee4a2 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -271,6 +271,7 @@ struct st_lsm6dsx_sensor {
  * @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.
  * @ts_sip: Total number of timestamp samples in a given pattern.
  * @sip: Total number of samples (acc/gyro/ts) in a given pattern.
@@ -288,6 +289,7 @@ struct st_lsm6dsx_hw {
        struct mutex page_lock;
 
        enum st_lsm6dsx_fifo_mode fifo_mode;
+       u8 suspend_mask;
        u8 enable_mask;
        u8 ts_sip;
        u8 sip;

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index cf82c90499451ddd1df6861a737957f74d88111e..be5c87c8266da15125876e46fe46997a14f97cee 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -1110,8 +1110,6 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
 {
        struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
        struct st_lsm6dsx_sensor *sensor;
-       const struct st_lsm6dsx_reg *reg;
-       unsigned int data;
        int i, err = 0;
 
        for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
@@ -1122,12 +1120,16 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
                if (!(hw->enable_mask & BIT(sensor->id)))
                        continue;
 
-               reg = &st_lsm6dsx_odr_table[sensor->id].reg;
-               data = ST_LSM6DSX_SHIFT_VAL(0, reg->mask);
-               err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask,
-                                                   data);
+               if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
+                   sensor->id == ST_LSM6DSX_ID_EXT1 ||
+                   sensor->id == ST_LSM6DSX_ID_EXT2)
+                       err = st_lsm6dsx_shub_set_enable(sensor, false);
+               else
+                       err = st_lsm6dsx_sensor_set_enable(sensor, false);
                if (err < 0)
                        return err;
+
+               hw->suspend_mask |= BIT(sensor->id);
        }
 
        if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
@@ -1147,12 +1149,19 @@ static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
                        continue;
 
                sensor = iio_priv(hw->iio_devs[i]);
-               if (!(hw->enable_mask & BIT(sensor->id)))
+               if (!(hw->suspend_mask & BIT(sensor->id)))
                        continue;
 
-               err = st_lsm6dsx_set_odr(sensor, sensor->odr);
+               if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
+                   sensor->id == ST_LSM6DSX_ID_EXT1 ||
+                   sensor->id == ST_LSM6DSX_ID_EXT2)
+                       err = st_lsm6dsx_shub_set_enable(sensor, true);
+               else
+                       err = st_lsm6dsx_sensor_set_enable(sensor, true);
                if (err < 0)
                        return err;
+
+               hw->suspend_mask &= ~BIT(sensor->id);
        }
 
        if (hw->enable_mask)