iio: imu: inv_mpu6050: fix fifo count reading (5cba7caa) · Commits · 戴 / test

drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c

+11 −13

Original line number	Diff line number	Diff line
		@@ -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);