inv_mpu_ring.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3* Copyright (C) 2012 Invensense, Inc.
  4*/
  5
  6#include <linux/module.h>
  7#include <linux/slab.h>
  8#include <linux/err.h>
  9#include <linux/delay.h>
 10#include <linux/sysfs.h>
 11#include <linux/jiffies.h>
 12#include <linux/irq.h>
 13#include <linux/interrupt.h>
 14#include <linux/poll.h>
 15#include <linux/math64.h>
 16
 17#include <linux/iio/common/inv_sensors_timestamp.h>
 18
 19#include "inv_mpu_iio.h"
 20
 21static int inv_reset_fifo(struct iio_dev *indio_dev)
 22{
 23	int result;
 24	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 25
 26	/* disable fifo and reenable it */
 27	inv_mpu6050_prepare_fifo(st, false);
 28	result = inv_mpu6050_prepare_fifo(st, true);
 29	if (result)
 30		goto reset_fifo_fail;
 31
 32	return 0;
 33
 34reset_fifo_fail:
 35	dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
 36	return regmap_update_bits(st->map, st->reg->int_enable,
 37			INV_MPU6050_BIT_DATA_RDY_EN, INV_MPU6050_BIT_DATA_RDY_EN);
 38}
 39
 40/*
 41 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
 42 */
 43irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 44{
 45	struct iio_poll_func *pf = p;
 46	struct iio_dev *indio_dev = pf->indio_dev;
 47	struct inv_mpu6050_state *st = iio_priv(indio_dev);
 48	size_t bytes_per_datum;
 49	int result;
 50	u16 fifo_count;
 51	u32 fifo_period;
 52	s64 timestamp;
 53	/* clear internal data buffer for avoiding kernel data leak */
 54	u8 data[INV_MPU6050_OUTPUT_DATA_SIZE] __aligned(8) = { };
 55	size_t i, nb;
 56
 57	mutex_lock(&st->lock);
 58
 59	if (!(st->chip_config.accl_fifo_enable |
 60		st->chip_config.gyro_fifo_enable |
 61		st->chip_config.magn_fifo_enable))
 62		goto end_session;
 63	bytes_per_datum = 0;
 64	if (st->chip_config.accl_fifo_enable)
 65		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
 66
 67	if (st->chip_config.gyro_fifo_enable)
 68		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
 69
 70	if (st->chip_config.temp_fifo_enable)
 71		bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
 72
 73	if (st->chip_config.magn_fifo_enable)
 74		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
 75
 76	/*
 77	 * read fifo_count register to know how many bytes are inside the FIFO
 78	 * right now
 79	 */
 80	result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
 81				  st->data, INV_MPU6050_FIFO_COUNT_BYTE);
 82	if (result)
 83		goto end_session;
 84	fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
 85
 86	/*
 87	 * Handle fifo overflow by resetting fifo.
 88	 * Reset if there is only 3 data set free remaining to mitigate
 89	 * possible delay between reading fifo count and fifo data.
 90	 */
 91	nb = 3 * bytes_per_datum;
 92	if (fifo_count >= st->hw->fifo_size - nb) {
 93		dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
 94		goto flush_fifo;
 95	}
 96
 97	/* compute and process only all complete datum */
 98	nb = fifo_count / bytes_per_datum;
 99	fifo_count = nb * bytes_per_datum;
100	if (nb == 0)
101		goto end_session;
102	/* Each FIFO data contains all sensors, so same number for FIFO and sensor data */
103	fifo_period = NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
104	inv_sensors_timestamp_interrupt(&st->timestamp, 1, pf->timestamp);
105	inv_sensors_timestamp_apply_odr(&st->timestamp, fifo_period, 1, 0);
106
107	/* read all data once and process every samples */
108	result = regmap_noinc_read(st->map, st->reg->fifo_r_w, st->data, fifo_count);
109	if (result)
110		goto flush_fifo;
111	for (i = 0; i < nb; ++i) {
112		/* skip first samples if needed */
113		if (st->skip_samples) {
114			st->skip_samples--;
115			continue;
116		}
117		memcpy(data, &st->data[i * bytes_per_datum], bytes_per_datum);
118		timestamp = inv_sensors_timestamp_pop(&st->timestamp);
119		iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
120	}
121
122end_session:
123	mutex_unlock(&st->lock);
124	iio_trigger_notify_done(indio_dev->trig);
125
126	return IRQ_HANDLED;
127
128flush_fifo:
129	/* Flush HW and SW FIFOs. */
130	inv_reset_fifo(indio_dev);
131	mutex_unlock(&st->lock);
132	iio_trigger_notify_done(indio_dev->trig);
133
134	return IRQ_HANDLED;
135}