Commit 5cba7caa authored by Jean-Baptiste Maneyrol's avatar Jean-Baptiste Maneyrol Committed by Jonathan Cameron

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: default avatarJean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent 7b782508
...@@ -20,6 +20,7 @@ ...@@ -20,6 +20,7 @@
#include <linux/irq.h> #include <linux/irq.h>
#include <linux/interrupt.h> #include <linux/interrupt.h>
#include <linux/poll.h> #include <linux/poll.h>
#include <asm/unaligned.h>
#include "inv_mpu_iio.h" #include "inv_mpu_iio.h"
int inv_reset_fifo(struct iio_dev *indio_dev) int inv_reset_fifo(struct iio_dev *indio_dev)
...@@ -98,6 +99,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) ...@@ -98,6 +99,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
u16 fifo_count; u16 fifo_count;
s64 timestamp = pf->timestamp; s64 timestamp = pf->timestamp;
int int_status; int int_status;
size_t i, nb;
mutex_lock(&st->lock); mutex_lock(&st->lock);
...@@ -132,27 +134,23 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) ...@@ -132,27 +134,23 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
INV_MPU6050_FIFO_COUNT_BYTE); INV_MPU6050_FIFO_COUNT_BYTE);
if (result) if (result)
goto end_session; goto end_session;
fifo_count = be16_to_cpup((__be16 *)(&data[0])); fifo_count = get_unaligned_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;
if (fifo_count > INV_MPU6050_FIFO_THRESHOLD) if (fifo_count > INV_MPU6050_FIFO_THRESHOLD)
goto flush_fifo; 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, result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
data, bytes_per_datum); data, bytes_per_datum);
if (result) if (result)
goto flush_fifo; goto flush_fifo;
/* skip first samples if needed */ /* skip first samples if needed */
if (st->skip_samples) if (st->skip_samples) {
st->skip_samples--; st->skip_samples--;
else continue;
iio_push_to_buffers_with_timestamp(indio_dev, data, }
timestamp); iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
fifo_count -= bytes_per_datum; }
} while (fifo_count >= bytes_per_datum);
end_session: end_session:
mutex_unlock(&st->lock); mutex_unlock(&st->lock);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment