Generalize st_lsm6dsx_shub_read_oneshot in order to not use a fixed read length and take into account iio channel realbits for single read operations Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@xxxxxxxxxx> --- drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 31 +++++++++++++------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c index ee59b0cac84f..b908df0380b3 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c @@ -27,6 +27,7 @@ #include <linux/iio/iio.h> #include <linux/iio/sysfs.h> #include <linux/bitfield.h> +#include <linux/slab.h> #include "st_lsm6dsx.h" @@ -432,31 +433,39 @@ st_lsm6dsx_shub_read_oneshot(struct st_lsm6dsx_sensor *sensor, struct iio_chan_spec const *ch, int *val) { - int err, delay, len = ch->scan_type.realbits >> 3; - __le16 data; + int ret, delay, len = ch->scan_type.realbits >> 3; + u8 *data; - err = st_lsm6dsx_shub_set_enable(sensor, true); - if (err < 0) - return err; + ret = st_lsm6dsx_shub_set_enable(sensor, true); + if (ret < 0) + return ret; + + data = kmalloc(len, GFP_KERNEL); + if (!data) + return -ENOMEM; delay = 1000000 / sensor->odr; usleep_range(delay, 2 * delay); - err = st_lsm6dsx_shub_read(sensor, ch->address, (u8 *)&data, len); - if (err < 0) - return err; + ret = st_lsm6dsx_shub_read(sensor, ch->address, data, len); + if (ret < 0) + goto out; st_lsm6dsx_shub_set_enable(sensor, false); switch (len) { case 2: - *val = (s16)le16_to_cpu(data); + *val = (s16)le16_to_cpu(*((__le16 *)data)); + ret = IIO_VAL_INT; break; default: - return -EINVAL; + ret = -EINVAL; + break; } - return IIO_VAL_INT; +out: + kfree(data); + return ret; } static int -- 2.19.1