From 5cba7caaacfb842c4326959315f2817897711f53 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Maneyrol Date: Tue, 22 May 2018 16:18:20 +0200 Subject: iio: imu: inv_mpu6050: fix fifo count reading Use unaligned access since buffer is a bytes table. Truncate fifo count to read only complete datum. Signed-off-by: Jean-Baptiste Maneyrol Signed-off-by: Jonathan Cameron --- drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 24 +++++++++++------------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c index 5436d181f2dc..7724888a4696 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c @@ -20,6 +20,7 @@ #include #include #include +#include #include "inv_mpu_iio.h" int inv_reset_fifo(struct iio_dev *indio_dev) @@ -98,6 +99,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) u16 fifo_count; s64 timestamp = pf->timestamp; int int_status; + size_t i, nb; mutex_lock(&st->lock); @@ -132,27 +134,23 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) INV_MPU6050_FIFO_COUNT_BYTE); if (result) goto end_session; - fifo_count = be16_to_cpup((__be16 *)(&data[0])); - if (fifo_count < bytes_per_datum) - goto end_session; - /* fifo count can't be an odd number. If it is odd, reset the FIFO. */ - if (fifo_count & 1) - goto flush_fifo; + fifo_count = get_unaligned_be16(&data[0]); if (fifo_count > INV_MPU6050_FIFO_THRESHOLD) goto flush_fifo; - do { + /* compute and process all complete datum */ + nb = fifo_count / bytes_per_datum; + for (i = 0; i < nb; ++i) { result = regmap_bulk_read(st->map, st->reg->fifo_r_w, data, bytes_per_datum); if (result) goto flush_fifo; /* skip first samples if needed */ - if (st->skip_samples) + if (st->skip_samples) { st->skip_samples--; - else - iio_push_to_buffers_with_timestamp(indio_dev, data, - timestamp); - fifo_count -= bytes_per_datum; - } while (fifo_count >= bytes_per_datum); + continue; + } + iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp); + } end_session: mutex_unlock(&st->lock); -- cgit v1.2.3-55-g7522