Commit 4ab717d9 authored by Lorenzo Bianconi's avatar Lorenzo Bianconi Committed by Jonathan Cameron
Browse files

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: default avatarJimmy Assarsson <jimmyassarsson@gmail.com>
Signed-off-by: default avatarLorenzo Bianconi <lorenzo@kernel.org>
Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent 9601c94d

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c

+33 −0
Original line number Diff line number Diff line
@@ -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;