iio: imu: st_lsm6dsx: configure full scale on slave device if supported

Introduce st_lsm6dsx_shub_set_full_scale routine in order to configure
the i2c slave device sensitivity

Tested-by: Jimmy Assarsson <jimmyassarsson@gmail.com>
Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
index 95ddd19d1aa7c369df3cb488805eed726fc33eb8..a9d3f0adcaa18130ded046c9cd4c27d23ce1a4f2 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
@@ -517,6 +517,36 @@ st_lsm6dsx_shub_read_raw(struct iio_dev *iio_dev,
        return ret;
 }
 
+static int
+st_lsm6dsx_shub_set_full_scale(struct st_lsm6dsx_sensor *sensor,
+                              u32 gain)
+{
+       const struct st_lsm6dsx_fs_table_entry *fs_table;
+       int i, err;
+
+       fs_table = &sensor->ext_info.settings->fs_table;
+       if (!fs_table->reg.addr)
+               return -ENOTSUPP;
+
+       for (i = 0; i < fs_table->fs_len; i++) {
+               if (fs_table->fs_avl[i].gain == gain)
+                       break;
+       }
+
+       if (i == fs_table->fs_len)
+               return -EINVAL;
+
+       err = st_lsm6dsx_shub_write_with_mask(sensor, fs_table->reg.addr,
+                                             fs_table->reg.mask,
+                                             fs_table->fs_avl[i].val);
+       if (err < 0)
+               return err;
+
+       sensor->gain = gain;
+
+       return 0;
+}
+
 static int
 st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev,
                          struct iio_chan_spec const *chan,
@@ -539,6 +569,9 @@ st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev,
                        sensor->odr = val;
                break;
        }
+       case IIO_CHAN_INFO_SCALE:
+               err = st_lsm6dsx_shub_set_full_scale(sensor, val2);
+               break;
        default:
                err = -EINVAL;
                break;