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, enable: false);
28 result = inv_mpu6050_prepare_fifo(st, enable: 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 result = regmap_write(map: st->map, reg: st->reg->int_enable,
37 INV_MPU6050_BIT_DATA_RDY_EN);
38
39 return result;
40}
41
42/*
43 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
44 */
45irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
46{
47 struct iio_poll_func *pf = p;
48 struct iio_dev *indio_dev = pf->indio_dev;
49 struct inv_mpu6050_state *st = iio_priv(indio_dev);
50 size_t bytes_per_datum;
51 int result;
52 u16 fifo_count;
53 u32 fifo_period;
54 s64 timestamp;
55 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
56 int int_status;
57 size_t i, nb;
58
59 mutex_lock(&st->lock);
60
61 /* ack interrupt and check status */
62 result = regmap_read(map: st->map, reg: st->reg->int_status, val: &int_status);
63 if (result) {
64 dev_err(regmap_get_device(st->map),
65 "failed to ack interrupt\n");
66 goto flush_fifo;
67 }
68 if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT))
69 goto end_session;
70
71 if (!(st->chip_config.accl_fifo_enable |
72 st->chip_config.gyro_fifo_enable |
73 st->chip_config.magn_fifo_enable))
74 goto end_session;
75 bytes_per_datum = 0;
76 if (st->chip_config.accl_fifo_enable)
77 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
78
79 if (st->chip_config.gyro_fifo_enable)
80 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
81
82 if (st->chip_config.temp_fifo_enable)
83 bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
84
85 if (st->chip_config.magn_fifo_enable)
86 bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
87
88 /*
89 * read fifo_count register to know how many bytes are inside the FIFO
90 * right now
91 */
92 result = regmap_bulk_read(map: st->map, reg: st->reg->fifo_count_h,
93 val: st->data, INV_MPU6050_FIFO_COUNT_BYTE);
94 if (result)
95 goto end_session;
96 fifo_count = be16_to_cpup(p: (__be16 *)&st->data[0]);
97
98 /*
99 * Handle fifo overflow by resetting fifo.
100 * Reset if there is only 3 data set free remaining to mitigate
101 * possible delay between reading fifo count and fifo data.
102 */
103 nb = 3 * bytes_per_datum;
104 if (fifo_count >= st->hw->fifo_size - nb) {
105 dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
106 goto flush_fifo;
107 }
108
109 /* compute and process only all complete datum */
110 nb = fifo_count / bytes_per_datum;
111 fifo_count = nb * bytes_per_datum;
112 if (nb == 0)
113 goto end_session;
114 /* Each FIFO data contains all sensors, so same number for FIFO and sensor data */
115 fifo_period = NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
116 inv_sensors_timestamp_interrupt(ts: &st->timestamp, fifo_period, fifo_nb: nb, sensor_nb: nb, timestamp: pf->timestamp);
117 inv_sensors_timestamp_apply_odr(ts: &st->timestamp, fifo_period, fifo_nb: nb, fifo_no: 0);
118
119 /* clear internal data buffer for avoiding kernel data leak */
120 memset(data, 0, sizeof(data));
121
122 /* read all data once and process every samples */
123 result = regmap_noinc_read(map: st->map, reg: st->reg->fifo_r_w, val: st->data, val_len: fifo_count);
124 if (result)
125 goto flush_fifo;
126 for (i = 0; i < nb; ++i) {
127 /* skip first samples if needed */
128 if (st->skip_samples) {
129 st->skip_samples--;
130 continue;
131 }
132 memcpy(data, &st->data[i * bytes_per_datum], bytes_per_datum);
133 timestamp = inv_sensors_timestamp_pop(ts: &st->timestamp);
134 iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
135 }
136
137end_session:
138 mutex_unlock(lock: &st->lock);
139 iio_trigger_notify_done(trig: indio_dev->trig);
140
141 return IRQ_HANDLED;
142
143flush_fifo:
144 /* Flush HW and SW FIFOs. */
145 inv_reset_fifo(indio_dev);
146 mutex_unlock(lock: &st->lock);
147 iio_trigger_notify_done(trig: indio_dev->trig);
148
149 return IRQ_HANDLED;
150}
151

source code of linux/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c