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>

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;