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 | |
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, enable: false); |
28 | result = inv_mpu6050_prepare_fifo(st, enable: 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 | 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 | */ |
45 | irqreturn_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 | |
137 | end_session: |
138 | mutex_unlock(lock: &st->lock); |
139 | iio_trigger_notify_done(trig: indio_dev->trig); |
140 | |
141 | return IRQ_HANDLED; |
142 | |
143 | flush_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 | |