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
inv_reset_fifo(struct iio_dev * indio_dev)21 static 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
34 reset_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 */
inv_mpu6050_read_fifo(int irq,void * p)43 irqreturn_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
122 end_session:
123 mutex_unlock(&st->lock);
124 iio_trigger_notify_done(indio_dev->trig);
125
126 return IRQ_HANDLED;
127
128 flush_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 }
136