From 4ab717d9e8bdc14ce9f76d1417bc5873c4f493a1 Mon Sep 17 00:00:00 2001 From: Lorenzo Bianconi Date: Thu, 19 Mar 2020 22:02:45 +0100 Subject: [PATCH] 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 Signed-off-by: Lorenzo Bianconi Signed-off-by: Jonathan Cameron --- drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 33 ++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c index 95ddd19d1aa7..a9d3f0adcaa1 100644 --- 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;