Use unaligned access since buffer is a bytes table. Truncate fifo
count to read only complete datum.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx>
---
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 5436d18..7724888 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);
--
2.7.4
--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
