Signed-off-by: Gregor Boirie <gregor.boirie@xxxxxxxxxx>
---
drivers/iio/common/st_sensors/st_sensors_buffer.c | 38 ++++++++++-------------
drivers/iio/common/st_sensors/st_sensors_core.c | 2 +-
2 files changed, 18 insertions(+), 22 deletions(-)
diff --git a/drivers/iio/common/st_sensors/st_sensors_buffer.c b/drivers/iio/common/st_sensors/st_sensors_buffer.c
index c558985..a7f1ced 100644
--- a/drivers/iio/common/st_sensors/st_sensors_buffer.c
+++ b/drivers/iio/common/st_sensors/st_sensors_buffer.c
@@ -21,33 +21,29 @@
#include <linux/iio/common/st_sensors.h>
-
int st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf)
{
- int i, len;
- int total = 0;
+ unsigned int cid;
struct st_sensor_data *sdata = iio_priv(indio_dev);
- unsigned int num_data_channels = sdata->num_data_channels;
-
- for (i = 0; i < num_data_channels; i++) {
- unsigned int bytes_to_read;
-
- if (test_bit(i, indio_dev->active_scan_mask)) {
- bytes_to_read = indio_dev->channels[i].scan_type.storagebits >> 3;
- len = sdata->tf->read_multiple_byte(&sdata->tb,
- sdata->dev, indio_dev->channels[i].address,
- bytes_to_read,
- buf + total, sdata->multiread_bit);
-
- if (len < bytes_to_read)
- return -EIO;
- /* Advance the buffer pointer */
- total += len;
- }
+ for_each_set_bit(cid, indio_dev->active_scan_mask,
+ sdata->num_data_channels) {
+ const struct iio_chan_spec *chan = &indio_dev->channels[cid];
+ int sb = chan->scan_type.storagebits / 8;
+ int rb = chan->scan_type.realbits / 8;
+ int err;
+
+ buf = PTR_ALIGN(buf, sb);
+ err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
+ chan->address, rb, buf,
+ sdata->multiread_bit);
+ if (err != rb)
+ return (err < 0) ? err : -EIO;
+
+ buf += sb;
}
- return total;
+ return 0;
}
EXPORT_SYMBOL(st_sensors_get_buffer_element);
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index dffe006..6901c7f 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -463,7 +463,7 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
int err;
u8 *outdata;
struct st_sensor_data *sdata = iio_priv(indio_dev);
- unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;
+ unsigned int byte_for_channel = ch->scan_type.realbits >> 3;
outdata = kmalloc(byte_for_channel, GFP_KERNEL);
if (!outdata)