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