iio: imu: st_lsm6dsx: introduce st_lsm6dsx_device_set_enable routine

Introduce st_lsm6dsx_device_set_enable utility routine and remove
duplicated code used to enable/disable sensors

Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
Link: https://lore.kernel.org/r/e3fbe5d4a3bed41130908669f745f78c8505cf47.1665399959.git.lorenzo@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 6b57d47..d3a4e21 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -509,6 +509,17 @@ st_lsm6dsx_get_mount_matrix(const struct iio_dev *iio_dev,
        return &hw->orientation;
 }
 
+static inline int
+st_lsm6dsx_device_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)
+{
+       if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
+           sensor->id == ST_LSM6DSX_ID_EXT1 ||
+           sensor->id == ST_LSM6DSX_ID_EXT2)
+               return st_lsm6dsx_shub_set_enable(sensor, enable);
+
+       return st_lsm6dsx_sensor_set_enable(sensor, enable);
+}
+
 static const
 struct iio_chan_spec_ext_info __maybe_unused st_lsm6dsx_accel_ext_info[] = {
        IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, st_lsm6dsx_get_mount_matrix),

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index e49f2d1..48fe6a4 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -673,17 +673,9 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
                        goto out;
        }
 
-       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, enable);
-               if (err < 0)
-                       goto out;
-       } else {
-               err = st_lsm6dsx_sensor_set_enable(sensor, enable);
-               if (err < 0)
-                       goto out;
-       }
+       err = st_lsm6dsx_device_set_enable(sensor, enable);
+       if (err < 0)
+               goto out;
 
        err = st_lsm6dsx_set_fifo_odr(sensor, enable);
        if (err < 0)

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index f8bbb00..6af2e90 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -2317,12 +2317,7 @@ static int st_lsm6dsx_suspend(struct device *dev)
                        continue;
                }
 
-               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);
+               err = st_lsm6dsx_device_set_enable(sensor, false);
                if (err < 0)
                        return err;
 
@@ -2353,12 +2348,7 @@ static int st_lsm6dsx_resume(struct device *dev)
                if (!(hw->suspend_mask & BIT(sensor->id)))
                        continue;
 
-               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);
+               err = st_lsm6dsx_device_set_enable(sensor, true);
                if (err < 0)
                        return err;