diff options
| author | Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> | 2018-05-22 16:18:20 +0200 |
|---|---|---|
| committer | Jonathan Cameron <Jonathan.Cameron@huawei.com> | 2018-06-10 11:52:29 +0100 |
| commit | 5cba7caaacfb842c4326959315f2817897711f53 (patch) | |
| tree | ccd391c9b2f444a6f10d03ffdb7981a040340bb0 | |
| parent | 7b782508f829d39f30a80250935974e8f77c173a (diff) | |
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 <jmaneyrol@invensense.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 24 |
1 files 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 <linux/irq.h> #include <linux/interrupt.h> #include <linux/poll.h> +#include <asm/unaligned.h> #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); |