Commit 09a642b78523e9f4c5970c806ad218aa3de31551
Committed by
Jonathan Cameron
1 parent
8ce4a56a52
Exists in
master
and in
20 other branches
Invensense MPU6050 Device Driver.
This the basic functional Invensense MPU6050 Device driver. Signed-off-by: Ge Gao <ggao@invensense.com> Reviewed-by: Lars-Peter Clausen <lars@metafoo.de> Signed-off-by: Jonathan Cameron <jic23@kernel.org>
Showing 10 changed files with 1459 additions and 0 deletions Side-by-side Diff
- Documentation/ABI/testing/sysfs-bus-iio-mpu6050
- drivers/iio/imu/Kconfig
- drivers/iio/imu/Makefile
- drivers/iio/imu/inv_mpu6050/Kconfig
- drivers/iio/imu/inv_mpu6050/Makefile
- drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
- drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
- drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
- drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
- include/linux/platform_data/invensense_mpu6050.h
Documentation/ABI/testing/sysfs-bus-iio-mpu6050
1 | +What: /sys/bus/iio/devices/iio:deviceX/in_gyro_matrix | |
2 | +What: /sys/bus/iio/devices/iio:deviceX/in_accel_matrix | |
3 | +What: /sys/bus/iio/devices/iio:deviceX/in_magn_matrix | |
4 | +KernelVersion: 3.4.0 | |
5 | +Contact: linux-iio@vger.kernel.org | |
6 | +Description: | |
7 | + This is mounting matrix for motion sensors. Mounting matrix | |
8 | + is a 3x3 unitary matrix. A typical mounting matrix would look like | |
9 | + [0, 1, 0; 1, 0, 0; 0, 0, -1]. Using this information, it would be | |
10 | + easy to tell the relative positions among sensors as well as their | |
11 | + positions relative to the board that holds these sensors. Identity matrix | |
12 | + [1, 0, 0; 0, 1, 0; 0, 0, 1] means sensor chip and device are perfectly | |
13 | + aligned with each other. All axes are exactly the same. |
drivers/iio/imu/Kconfig
drivers/iio/imu/Makefile
drivers/iio/imu/inv_mpu6050/Kconfig
1 | +# | |
2 | +# inv-mpu6050 drivers for Invensense MPU devices and combos | |
3 | +# | |
4 | + | |
5 | +config INV_MPU6050_IIO | |
6 | + tristate "Invensense MPU6050 devices" | |
7 | + depends on I2C && SYSFS | |
8 | + select IIO_TRIGGERED_BUFFER | |
9 | + help | |
10 | + This driver supports the Invensense MPU6050 devices. | |
11 | + It is a gyroscope/accelerometer combo device. | |
12 | + This driver can be built as a module. The module will be called | |
13 | + inv-mpu6050. |
drivers/iio/imu/inv_mpu6050/Makefile
drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
1 | +/* | |
2 | +* Copyright (C) 2012 Invensense, Inc. | |
3 | +* | |
4 | +* This software is licensed under the terms of the GNU General Public | |
5 | +* License version 2, as published by the Free Software Foundation, and | |
6 | +* may be copied, distributed, and modified under those terms. | |
7 | +* | |
8 | +* This program is distributed in the hope that it will be useful, | |
9 | +* but WITHOUT ANY WARRANTY; without even the implied warranty of | |
10 | +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
11 | +* GNU General Public License for more details. | |
12 | +*/ | |
13 | + | |
14 | +#include <linux/module.h> | |
15 | +#include <linux/init.h> | |
16 | +#include <linux/slab.h> | |
17 | +#include <linux/i2c.h> | |
18 | +#include <linux/err.h> | |
19 | +#include <linux/delay.h> | |
20 | +#include <linux/sysfs.h> | |
21 | +#include <linux/jiffies.h> | |
22 | +#include <linux/irq.h> | |
23 | +#include <linux/interrupt.h> | |
24 | +#include <linux/kfifo.h> | |
25 | +#include <linux/spinlock.h> | |
26 | +#include "inv_mpu_iio.h" | |
27 | + | |
28 | +/* | |
29 | + * this is the gyro scale translated from dynamic range plus/minus | |
30 | + * {250, 500, 1000, 2000} to rad/s | |
31 | + */ | |
32 | +static const int gyro_scale_6050[] = {133090, 266181, 532362, 1064724}; | |
33 | + | |
34 | +/* | |
35 | + * this is the accel scale translated from dynamic range plus/minus | |
36 | + * {2, 4, 8, 16} to m/s^2 | |
37 | + */ | |
38 | +static const int accel_scale[] = {598, 1196, 2392, 4785}; | |
39 | + | |
40 | +static const struct inv_mpu6050_reg_map reg_set_6050 = { | |
41 | + .sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV, | |
42 | + .lpf = INV_MPU6050_REG_CONFIG, | |
43 | + .user_ctrl = INV_MPU6050_REG_USER_CTRL, | |
44 | + .fifo_en = INV_MPU6050_REG_FIFO_EN, | |
45 | + .gyro_config = INV_MPU6050_REG_GYRO_CONFIG, | |
46 | + .accl_config = INV_MPU6050_REG_ACCEL_CONFIG, | |
47 | + .fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H, | |
48 | + .fifo_r_w = INV_MPU6050_REG_FIFO_R_W, | |
49 | + .raw_gyro = INV_MPU6050_REG_RAW_GYRO, | |
50 | + .raw_accl = INV_MPU6050_REG_RAW_ACCEL, | |
51 | + .temperature = INV_MPU6050_REG_TEMPERATURE, | |
52 | + .int_enable = INV_MPU6050_REG_INT_ENABLE, | |
53 | + .pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1, | |
54 | + .pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2, | |
55 | +}; | |
56 | + | |
57 | +static const struct inv_mpu6050_chip_config chip_config_6050 = { | |
58 | + .fsr = INV_MPU6050_FSR_2000DPS, | |
59 | + .lpf = INV_MPU6050_FILTER_20HZ, | |
60 | + .fifo_rate = INV_MPU6050_INIT_FIFO_RATE, | |
61 | + .gyro_fifo_enable = false, | |
62 | + .accl_fifo_enable = false, | |
63 | + .accl_fs = INV_MPU6050_FS_02G, | |
64 | +}; | |
65 | + | |
66 | +static const struct inv_mpu6050_hw hw_info[INV_NUM_PARTS] = { | |
67 | + { | |
68 | + .num_reg = 117, | |
69 | + .name = "MPU6050", | |
70 | + .reg = ®_set_6050, | |
71 | + .config = &chip_config_6050, | |
72 | + }, | |
73 | +}; | |
74 | + | |
75 | +int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 d) | |
76 | +{ | |
77 | + return i2c_smbus_write_i2c_block_data(st->client, reg, 1, &d); | |
78 | +} | |
79 | + | |
80 | +int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask) | |
81 | +{ | |
82 | + u8 d, mgmt_1; | |
83 | + int result; | |
84 | + | |
85 | + /* switch clock needs to be careful. Only when gyro is on, can | |
86 | + clock source be switched to gyro. Otherwise, it must be set to | |
87 | + internal clock */ | |
88 | + if (INV_MPU6050_BIT_PWR_GYRO_STBY == mask) { | |
89 | + result = i2c_smbus_read_i2c_block_data(st->client, | |
90 | + st->reg->pwr_mgmt_1, 1, &mgmt_1); | |
91 | + if (result != 1) | |
92 | + return result; | |
93 | + | |
94 | + mgmt_1 &= ~INV_MPU6050_BIT_CLK_MASK; | |
95 | + } | |
96 | + | |
97 | + if ((INV_MPU6050_BIT_PWR_GYRO_STBY == mask) && (!en)) { | |
98 | + /* turning off gyro requires switch to internal clock first. | |
99 | + Then turn off gyro engine */ | |
100 | + mgmt_1 |= INV_CLK_INTERNAL; | |
101 | + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, mgmt_1); | |
102 | + if (result) | |
103 | + return result; | |
104 | + } | |
105 | + | |
106 | + result = i2c_smbus_read_i2c_block_data(st->client, | |
107 | + st->reg->pwr_mgmt_2, 1, &d); | |
108 | + if (result != 1) | |
109 | + return result; | |
110 | + if (en) | |
111 | + d &= ~mask; | |
112 | + else | |
113 | + d |= mask; | |
114 | + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_2, d); | |
115 | + if (result) | |
116 | + return result; | |
117 | + | |
118 | + if (en) { | |
119 | + /* Wait for output stablize */ | |
120 | + msleep(INV_MPU6050_TEMP_UP_TIME); | |
121 | + if (INV_MPU6050_BIT_PWR_GYRO_STBY == mask) { | |
122 | + /* switch internal clock to PLL */ | |
123 | + mgmt_1 |= INV_CLK_PLL; | |
124 | + result = inv_mpu6050_write_reg(st, | |
125 | + st->reg->pwr_mgmt_1, mgmt_1); | |
126 | + if (result) | |
127 | + return result; | |
128 | + } | |
129 | + } | |
130 | + | |
131 | + return 0; | |
132 | +} | |
133 | + | |
134 | +int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on) | |
135 | +{ | |
136 | + int result; | |
137 | + | |
138 | + if (power_on) | |
139 | + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, 0); | |
140 | + else | |
141 | + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, | |
142 | + INV_MPU6050_BIT_SLEEP); | |
143 | + if (result) | |
144 | + return result; | |
145 | + | |
146 | + if (power_on) | |
147 | + msleep(INV_MPU6050_REG_UP_TIME); | |
148 | + | |
149 | + return 0; | |
150 | +} | |
151 | + | |
152 | +/** | |
153 | + * inv_mpu6050_init_config() - Initialize hardware, disable FIFO. | |
154 | + * | |
155 | + * Initial configuration: | |
156 | + * FSR: ยฑ 2000DPS | |
157 | + * DLPF: 20Hz | |
158 | + * FIFO rate: 50Hz | |
159 | + * Clock source: Gyro PLL | |
160 | + */ | |
161 | +static int inv_mpu6050_init_config(struct iio_dev *indio_dev) | |
162 | +{ | |
163 | + int result; | |
164 | + u8 d; | |
165 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
166 | + | |
167 | + result = inv_mpu6050_set_power_itg(st, true); | |
168 | + if (result) | |
169 | + return result; | |
170 | + d = (INV_MPU6050_FSR_2000DPS << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); | |
171 | + result = inv_mpu6050_write_reg(st, st->reg->gyro_config, d); | |
172 | + if (result) | |
173 | + return result; | |
174 | + | |
175 | + d = INV_MPU6050_FILTER_20HZ; | |
176 | + result = inv_mpu6050_write_reg(st, st->reg->lpf, d); | |
177 | + if (result) | |
178 | + return result; | |
179 | + | |
180 | + d = INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE - 1; | |
181 | + result = inv_mpu6050_write_reg(st, st->reg->sample_rate_div, d); | |
182 | + if (result) | |
183 | + return result; | |
184 | + | |
185 | + d = (INV_MPU6050_FS_02G << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); | |
186 | + result = inv_mpu6050_write_reg(st, st->reg->accl_config, d); | |
187 | + if (result) | |
188 | + return result; | |
189 | + | |
190 | + memcpy(&st->chip_config, hw_info[st->chip_type].config, | |
191 | + sizeof(struct inv_mpu6050_chip_config)); | |
192 | + result = inv_mpu6050_set_power_itg(st, false); | |
193 | + | |
194 | + return result; | |
195 | +} | |
196 | + | |
197 | +static int inv_mpu6050_sensor_show(struct inv_mpu6050_state *st, int reg, | |
198 | + int axis, int *val) | |
199 | +{ | |
200 | + int ind, result; | |
201 | + __be16 d; | |
202 | + | |
203 | + ind = (axis - IIO_MOD_X) * 2; | |
204 | + result = i2c_smbus_read_i2c_block_data(st->client, reg + ind, 2, | |
205 | + (u8 *)&d); | |
206 | + if (result != 2) | |
207 | + return -EINVAL; | |
208 | + *val = (short)be16_to_cpup(&d); | |
209 | + | |
210 | + return IIO_VAL_INT; | |
211 | +} | |
212 | + | |
213 | +static int inv_mpu6050_read_raw(struct iio_dev *indio_dev, | |
214 | + struct iio_chan_spec const *chan, | |
215 | + int *val, | |
216 | + int *val2, | |
217 | + long mask) { | |
218 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
219 | + | |
220 | + switch (mask) { | |
221 | + case IIO_CHAN_INFO_RAW: | |
222 | + { | |
223 | + int ret, result; | |
224 | + | |
225 | + ret = IIO_VAL_INT; | |
226 | + result = 0; | |
227 | + mutex_lock(&indio_dev->mlock); | |
228 | + if (!st->chip_config.enable) { | |
229 | + result = inv_mpu6050_set_power_itg(st, true); | |
230 | + if (result) | |
231 | + goto error_read_raw; | |
232 | + } | |
233 | + /* when enable is on, power is already on */ | |
234 | + switch (chan->type) { | |
235 | + case IIO_ANGL_VEL: | |
236 | + if (!st->chip_config.gyro_fifo_enable || | |
237 | + !st->chip_config.enable) { | |
238 | + result = inv_mpu6050_switch_engine(st, true, | |
239 | + INV_MPU6050_BIT_PWR_GYRO_STBY); | |
240 | + if (result) | |
241 | + goto error_read_raw; | |
242 | + } | |
243 | + ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro, | |
244 | + chan->channel2, val); | |
245 | + if (!st->chip_config.gyro_fifo_enable || | |
246 | + !st->chip_config.enable) { | |
247 | + result = inv_mpu6050_switch_engine(st, false, | |
248 | + INV_MPU6050_BIT_PWR_GYRO_STBY); | |
249 | + if (result) | |
250 | + goto error_read_raw; | |
251 | + } | |
252 | + break; | |
253 | + case IIO_ACCEL: | |
254 | + if (!st->chip_config.accl_fifo_enable || | |
255 | + !st->chip_config.enable) { | |
256 | + result = inv_mpu6050_switch_engine(st, true, | |
257 | + INV_MPU6050_BIT_PWR_ACCL_STBY); | |
258 | + if (result) | |
259 | + goto error_read_raw; | |
260 | + } | |
261 | + ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl, | |
262 | + chan->channel2, val); | |
263 | + if (!st->chip_config.accl_fifo_enable || | |
264 | + !st->chip_config.enable) { | |
265 | + result = inv_mpu6050_switch_engine(st, false, | |
266 | + INV_MPU6050_BIT_PWR_ACCL_STBY); | |
267 | + if (result) | |
268 | + goto error_read_raw; | |
269 | + } | |
270 | + break; | |
271 | + case IIO_TEMP: | |
272 | + /* wait for stablization */ | |
273 | + msleep(INV_MPU6050_SENSOR_UP_TIME); | |
274 | + inv_mpu6050_sensor_show(st, st->reg->temperature, | |
275 | + IIO_MOD_X, val); | |
276 | + break; | |
277 | + default: | |
278 | + ret = -EINVAL; | |
279 | + break; | |
280 | + } | |
281 | +error_read_raw: | |
282 | + if (!st->chip_config.enable) | |
283 | + result |= inv_mpu6050_set_power_itg(st, false); | |
284 | + mutex_unlock(&indio_dev->mlock); | |
285 | + if (result) | |
286 | + return result; | |
287 | + | |
288 | + return ret; | |
289 | + } | |
290 | + case IIO_CHAN_INFO_SCALE: | |
291 | + switch (chan->type) { | |
292 | + case IIO_ANGL_VEL: | |
293 | + *val = 0; | |
294 | + *val2 = gyro_scale_6050[st->chip_config.fsr]; | |
295 | + | |
296 | + return IIO_VAL_INT_PLUS_NANO; | |
297 | + case IIO_ACCEL: | |
298 | + *val = 0; | |
299 | + *val2 = accel_scale[st->chip_config.accl_fs]; | |
300 | + | |
301 | + return IIO_VAL_INT_PLUS_MICRO; | |
302 | + case IIO_TEMP: | |
303 | + *val = 0; | |
304 | + *val2 = INV_MPU6050_TEMP_SCALE; | |
305 | + | |
306 | + return IIO_VAL_INT_PLUS_MICRO; | |
307 | + default: | |
308 | + return -EINVAL; | |
309 | + } | |
310 | + case IIO_CHAN_INFO_OFFSET: | |
311 | + switch (chan->type) { | |
312 | + case IIO_TEMP: | |
313 | + *val = INV_MPU6050_TEMP_OFFSET; | |
314 | + | |
315 | + return IIO_VAL_INT; | |
316 | + default: | |
317 | + return -EINVAL; | |
318 | + } | |
319 | + default: | |
320 | + return -EINVAL; | |
321 | + } | |
322 | +} | |
323 | + | |
324 | +static int inv_mpu6050_write_fsr(struct inv_mpu6050_state *st, int fsr) | |
325 | +{ | |
326 | + int result; | |
327 | + u8 d; | |
328 | + | |
329 | + if (fsr < 0 || fsr > INV_MPU6050_MAX_GYRO_FS_PARAM) | |
330 | + return -EINVAL; | |
331 | + if (fsr == st->chip_config.fsr) | |
332 | + return 0; | |
333 | + | |
334 | + d = (fsr << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); | |
335 | + result = inv_mpu6050_write_reg(st, st->reg->gyro_config, d); | |
336 | + if (result) | |
337 | + return result; | |
338 | + st->chip_config.fsr = fsr; | |
339 | + | |
340 | + return 0; | |
341 | +} | |
342 | + | |
343 | +static int inv_mpu6050_write_accel_fs(struct inv_mpu6050_state *st, int fs) | |
344 | +{ | |
345 | + int result; | |
346 | + u8 d; | |
347 | + | |
348 | + if (fs < 0 || fs > INV_MPU6050_MAX_ACCL_FS_PARAM) | |
349 | + return -EINVAL; | |
350 | + if (fs == st->chip_config.accl_fs) | |
351 | + return 0; | |
352 | + | |
353 | + d = (fs << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); | |
354 | + result = inv_mpu6050_write_reg(st, st->reg->accl_config, d); | |
355 | + if (result) | |
356 | + return result; | |
357 | + st->chip_config.accl_fs = fs; | |
358 | + | |
359 | + return 0; | |
360 | +} | |
361 | + | |
362 | +static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, | |
363 | + struct iio_chan_spec const *chan, | |
364 | + int val, | |
365 | + int val2, | |
366 | + long mask) { | |
367 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
368 | + int result; | |
369 | + | |
370 | + mutex_lock(&indio_dev->mlock); | |
371 | + /* we should only update scale when the chip is disabled, i.e., | |
372 | + not running */ | |
373 | + if (st->chip_config.enable) { | |
374 | + result = -EBUSY; | |
375 | + goto error_write_raw; | |
376 | + } | |
377 | + result = inv_mpu6050_set_power_itg(st, true); | |
378 | + if (result) | |
379 | + goto error_write_raw; | |
380 | + | |
381 | + switch (mask) { | |
382 | + case IIO_CHAN_INFO_SCALE: | |
383 | + switch (chan->type) { | |
384 | + case IIO_ANGL_VEL: | |
385 | + result = inv_mpu6050_write_fsr(st, val); | |
386 | + break; | |
387 | + case IIO_ACCEL: | |
388 | + result = inv_mpu6050_write_accel_fs(st, val); | |
389 | + break; | |
390 | + default: | |
391 | + result = -EINVAL; | |
392 | + break; | |
393 | + } | |
394 | + break; | |
395 | + default: | |
396 | + result = -EINVAL; | |
397 | + break; | |
398 | + } | |
399 | + | |
400 | +error_write_raw: | |
401 | + result |= inv_mpu6050_set_power_itg(st, false); | |
402 | + mutex_unlock(&indio_dev->mlock); | |
403 | + | |
404 | + return result; | |
405 | +} | |
406 | + | |
407 | +/** | |
408 | + * inv_mpu6050_set_lpf() - set low pass filer based on fifo rate. | |
409 | + * | |
410 | + * Based on the Nyquist principle, the sampling rate must | |
411 | + * exceed twice of the bandwidth of the signal, or there | |
412 | + * would be alising. This function basically search for the | |
413 | + * correct low pass parameters based on the fifo rate, e.g, | |
414 | + * sampling frequency. | |
415 | + */ | |
416 | +static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate) | |
417 | +{ | |
418 | + const int hz[] = {188, 98, 42, 20, 10, 5}; | |
419 | + const int d[] = {INV_MPU6050_FILTER_188HZ, INV_MPU6050_FILTER_98HZ, | |
420 | + INV_MPU6050_FILTER_42HZ, INV_MPU6050_FILTER_20HZ, | |
421 | + INV_MPU6050_FILTER_10HZ, INV_MPU6050_FILTER_5HZ}; | |
422 | + int i, h, result; | |
423 | + u8 data; | |
424 | + | |
425 | + h = (rate >> 1); | |
426 | + i = 0; | |
427 | + while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1)) | |
428 | + i++; | |
429 | + data = d[i]; | |
430 | + result = inv_mpu6050_write_reg(st, st->reg->lpf, data); | |
431 | + if (result) | |
432 | + return result; | |
433 | + st->chip_config.lpf = data; | |
434 | + | |
435 | + return 0; | |
436 | +} | |
437 | + | |
438 | +/** | |
439 | + * inv_mpu6050_fifo_rate_store() - Set fifo rate. | |
440 | + */ | |
441 | +static ssize_t inv_mpu6050_fifo_rate_store(struct device *dev, | |
442 | + struct device_attribute *attr, const char *buf, size_t count) | |
443 | +{ | |
444 | + s32 fifo_rate; | |
445 | + u8 d; | |
446 | + int result; | |
447 | + struct iio_dev *indio_dev = dev_to_iio_dev(dev); | |
448 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
449 | + | |
450 | + if (kstrtoint(buf, 10, &fifo_rate)) | |
451 | + return -EINVAL; | |
452 | + if (fifo_rate < INV_MPU6050_MIN_FIFO_RATE || | |
453 | + fifo_rate > INV_MPU6050_MAX_FIFO_RATE) | |
454 | + return -EINVAL; | |
455 | + if (fifo_rate == st->chip_config.fifo_rate) | |
456 | + return count; | |
457 | + | |
458 | + mutex_lock(&indio_dev->mlock); | |
459 | + if (st->chip_config.enable) { | |
460 | + result = -EBUSY; | |
461 | + goto fifo_rate_fail; | |
462 | + } | |
463 | + result = inv_mpu6050_set_power_itg(st, true); | |
464 | + if (result) | |
465 | + goto fifo_rate_fail; | |
466 | + | |
467 | + d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1; | |
468 | + result = inv_mpu6050_write_reg(st, st->reg->sample_rate_div, d); | |
469 | + if (result) | |
470 | + goto fifo_rate_fail; | |
471 | + st->chip_config.fifo_rate = fifo_rate; | |
472 | + | |
473 | + result = inv_mpu6050_set_lpf(st, fifo_rate); | |
474 | + if (result) | |
475 | + goto fifo_rate_fail; | |
476 | + | |
477 | +fifo_rate_fail: | |
478 | + result |= inv_mpu6050_set_power_itg(st, false); | |
479 | + mutex_unlock(&indio_dev->mlock); | |
480 | + if (result) | |
481 | + return result; | |
482 | + | |
483 | + return count; | |
484 | +} | |
485 | + | |
486 | +/** | |
487 | + * inv_fifo_rate_show() - Get the current sampling rate. | |
488 | + */ | |
489 | +static ssize_t inv_fifo_rate_show(struct device *dev, | |
490 | + struct device_attribute *attr, char *buf) | |
491 | +{ | |
492 | + struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev)); | |
493 | + | |
494 | + return sprintf(buf, "%d\n", st->chip_config.fifo_rate); | |
495 | +} | |
496 | + | |
497 | +/** | |
498 | + * inv_attr_show() - calling this function will show current | |
499 | + * parameters. | |
500 | + */ | |
501 | +static ssize_t inv_attr_show(struct device *dev, | |
502 | + struct device_attribute *attr, char *buf) | |
503 | +{ | |
504 | + struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev)); | |
505 | + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); | |
506 | + s8 *m; | |
507 | + | |
508 | + switch (this_attr->address) { | |
509 | + /* In MPU6050, the two matrix are the same because gyro and accel | |
510 | + are integrated in one chip */ | |
511 | + case ATTR_GYRO_MATRIX: | |
512 | + case ATTR_ACCL_MATRIX: | |
513 | + m = st->plat_data.orientation; | |
514 | + | |
515 | + return sprintf(buf, "%d, %d, %d; %d, %d, %d; %d, %d, %d\n", | |
516 | + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); | |
517 | + default: | |
518 | + return -EINVAL; | |
519 | + } | |
520 | +} | |
521 | + | |
522 | +/** | |
523 | + * inv_mpu6050_validate_trigger() - validate_trigger callback for invensense | |
524 | + * MPU6050 device. | |
525 | + * @indio_dev: The IIO device | |
526 | + * @trig: The new trigger | |
527 | + * | |
528 | + * Returns: 0 if the 'trig' matches the trigger registered by the MPU6050 | |
529 | + * device, -EINVAL otherwise. | |
530 | + */ | |
531 | +static int inv_mpu6050_validate_trigger(struct iio_dev *indio_dev, | |
532 | + struct iio_trigger *trig) | |
533 | +{ | |
534 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
535 | + | |
536 | + if (st->trig != trig) | |
537 | + return -EINVAL; | |
538 | + | |
539 | + return 0; | |
540 | +} | |
541 | + | |
542 | +#define INV_MPU6050_CHAN(_type, _channel2, _index) \ | |
543 | + { \ | |
544 | + .type = _type, \ | |
545 | + .modified = 1, \ | |
546 | + .channel2 = _channel2, \ | |
547 | + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT \ | |
548 | + | IIO_CHAN_INFO_RAW_SEPARATE_BIT, \ | |
549 | + .scan_index = _index, \ | |
550 | + .scan_type = { \ | |
551 | + .sign = 's', \ | |
552 | + .realbits = 16, \ | |
553 | + .storagebits = 16, \ | |
554 | + .shift = 0 , \ | |
555 | + .endianness = IIO_BE, \ | |
556 | + }, \ | |
557 | + } | |
558 | + | |
559 | +static const struct iio_chan_spec inv_mpu_channels[] = { | |
560 | + IIO_CHAN_SOFT_TIMESTAMP(INV_MPU6050_SCAN_TIMESTAMP), | |
561 | + /* | |
562 | + * Note that temperature should only be via polled reading only, | |
563 | + * not the final scan elements output. | |
564 | + */ | |
565 | + { | |
566 | + .type = IIO_TEMP, | |
567 | + .info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | |
568 | + | IIO_CHAN_INFO_OFFSET_SEPARATE_BIT | |
569 | + | IIO_CHAN_INFO_SCALE_SEPARATE_BIT, | |
570 | + .scan_index = -1, | |
571 | + }, | |
572 | + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X), | |
573 | + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y), | |
574 | + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z), | |
575 | + | |
576 | + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X), | |
577 | + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y), | |
578 | + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z), | |
579 | +}; | |
580 | + | |
581 | +/* constant IIO attribute */ | |
582 | +static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500"); | |
583 | +static IIO_DEV_ATTR_SAMP_FREQ(S_IRUGO | S_IWUSR, inv_fifo_rate_show, | |
584 | + inv_mpu6050_fifo_rate_store); | |
585 | +static IIO_DEVICE_ATTR(in_gyro_matrix, S_IRUGO, inv_attr_show, NULL, | |
586 | + ATTR_GYRO_MATRIX); | |
587 | +static IIO_DEVICE_ATTR(in_accel_matrix, S_IRUGO, inv_attr_show, NULL, | |
588 | + ATTR_ACCL_MATRIX); | |
589 | + | |
590 | +static struct attribute *inv_attributes[] = { | |
591 | + &iio_dev_attr_in_gyro_matrix.dev_attr.attr, | |
592 | + &iio_dev_attr_in_accel_matrix.dev_attr.attr, | |
593 | + &iio_dev_attr_sampling_frequency.dev_attr.attr, | |
594 | + &iio_const_attr_sampling_frequency_available.dev_attr.attr, | |
595 | + NULL, | |
596 | +}; | |
597 | + | |
598 | +static const struct attribute_group inv_attribute_group = { | |
599 | + .attrs = inv_attributes | |
600 | +}; | |
601 | + | |
602 | +static const struct iio_info mpu_info = { | |
603 | + .driver_module = THIS_MODULE, | |
604 | + .read_raw = &inv_mpu6050_read_raw, | |
605 | + .write_raw = &inv_mpu6050_write_raw, | |
606 | + .attrs = &inv_attribute_group, | |
607 | + .validate_trigger = inv_mpu6050_validate_trigger, | |
608 | +}; | |
609 | + | |
610 | +/** | |
611 | + * inv_check_and_setup_chip() - check and setup chip. | |
612 | + */ | |
613 | +static int inv_check_and_setup_chip(struct inv_mpu6050_state *st, | |
614 | + const struct i2c_device_id *id) | |
615 | +{ | |
616 | + int result; | |
617 | + | |
618 | + st->chip_type = INV_MPU6050; | |
619 | + st->hw = &hw_info[st->chip_type]; | |
620 | + st->reg = hw_info[st->chip_type].reg; | |
621 | + | |
622 | + /* reset to make sure previous state are not there */ | |
623 | + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, | |
624 | + INV_MPU6050_BIT_H_RESET); | |
625 | + if (result) | |
626 | + return result; | |
627 | + msleep(INV_MPU6050_POWER_UP_TIME); | |
628 | + /* toggle power state. After reset, the sleep bit could be on | |
629 | + or off depending on the OTP settings. Toggling power would | |
630 | + make it in a definite state as well as making the hardware | |
631 | + state align with the software state */ | |
632 | + result = inv_mpu6050_set_power_itg(st, false); | |
633 | + if (result) | |
634 | + return result; | |
635 | + result = inv_mpu6050_set_power_itg(st, true); | |
636 | + if (result) | |
637 | + return result; | |
638 | + | |
639 | + result = inv_mpu6050_switch_engine(st, false, | |
640 | + INV_MPU6050_BIT_PWR_ACCL_STBY); | |
641 | + if (result) | |
642 | + return result; | |
643 | + result = inv_mpu6050_switch_engine(st, false, | |
644 | + INV_MPU6050_BIT_PWR_GYRO_STBY); | |
645 | + if (result) | |
646 | + return result; | |
647 | + | |
648 | + return 0; | |
649 | +} | |
650 | + | |
651 | +/** | |
652 | + * inv_mpu_probe() - probe function. | |
653 | + * @client: i2c client. | |
654 | + * @id: i2c device id. | |
655 | + * | |
656 | + * Returns 0 on success, a negative error code otherwise. | |
657 | + */ | |
658 | +static int inv_mpu_probe(struct i2c_client *client, | |
659 | + const struct i2c_device_id *id) | |
660 | +{ | |
661 | + struct inv_mpu6050_state *st; | |
662 | + struct iio_dev *indio_dev; | |
663 | + int result; | |
664 | + | |
665 | + if (!i2c_check_functionality(client->adapter, | |
666 | + I2C_FUNC_SMBUS_READ_I2C_BLOCK | | |
667 | + I2C_FUNC_SMBUS_WRITE_I2C_BLOCK)) { | |
668 | + result = -ENOSYS; | |
669 | + goto out_no_free; | |
670 | + } | |
671 | + indio_dev = iio_device_alloc(sizeof(*st)); | |
672 | + if (indio_dev == NULL) { | |
673 | + result = -ENOMEM; | |
674 | + goto out_no_free; | |
675 | + } | |
676 | + st = iio_priv(indio_dev); | |
677 | + st->client = client; | |
678 | + st->plat_data = *(struct inv_mpu6050_platform_data | |
679 | + *)dev_get_platdata(&client->dev); | |
680 | + /* power is turned on inside check chip type*/ | |
681 | + result = inv_check_and_setup_chip(st, id); | |
682 | + if (result) | |
683 | + goto out_free; | |
684 | + | |
685 | + result = inv_mpu6050_init_config(indio_dev); | |
686 | + if (result) { | |
687 | + dev_err(&client->dev, | |
688 | + "Could not initialize device.\n"); | |
689 | + goto out_free; | |
690 | + } | |
691 | + | |
692 | + i2c_set_clientdata(client, indio_dev); | |
693 | + indio_dev->dev.parent = &client->dev; | |
694 | + indio_dev->name = id->name; | |
695 | + indio_dev->channels = inv_mpu_channels; | |
696 | + indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels); | |
697 | + | |
698 | + indio_dev->info = &mpu_info; | |
699 | + indio_dev->modes = INDIO_BUFFER_TRIGGERED; | |
700 | + | |
701 | + result = iio_triggered_buffer_setup(indio_dev, | |
702 | + inv_mpu6050_irq_handler, | |
703 | + inv_mpu6050_read_fifo, | |
704 | + NULL); | |
705 | + if (result) { | |
706 | + dev_err(&st->client->dev, "configure buffer fail %d\n", | |
707 | + result); | |
708 | + goto out_free; | |
709 | + } | |
710 | + result = inv_mpu6050_probe_trigger(indio_dev); | |
711 | + if (result) { | |
712 | + dev_err(&st->client->dev, "trigger probe fail %d\n", result); | |
713 | + goto out_unreg_ring; | |
714 | + } | |
715 | + | |
716 | + INIT_KFIFO(st->timestamps); | |
717 | + spin_lock_init(&st->time_stamp_lock); | |
718 | + result = iio_device_register(indio_dev); | |
719 | + if (result) { | |
720 | + dev_err(&st->client->dev, "IIO register fail %d\n", result); | |
721 | + goto out_remove_trigger; | |
722 | + } | |
723 | + | |
724 | + return 0; | |
725 | + | |
726 | +out_remove_trigger: | |
727 | + inv_mpu6050_remove_trigger(st); | |
728 | +out_unreg_ring: | |
729 | + iio_triggered_buffer_cleanup(indio_dev); | |
730 | +out_free: | |
731 | + iio_device_free(indio_dev); | |
732 | +out_no_free: | |
733 | + | |
734 | + return result; | |
735 | +} | |
736 | + | |
737 | +static int inv_mpu_remove(struct i2c_client *client) | |
738 | +{ | |
739 | + struct iio_dev *indio_dev = i2c_get_clientdata(client); | |
740 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
741 | + | |
742 | + iio_device_unregister(indio_dev); | |
743 | + inv_mpu6050_remove_trigger(st); | |
744 | + iio_triggered_buffer_cleanup(indio_dev); | |
745 | + iio_device_free(indio_dev); | |
746 | + | |
747 | + return 0; | |
748 | +} | |
749 | +#ifdef CONFIG_PM_SLEEP | |
750 | + | |
751 | +static int inv_mpu_resume(struct device *dev) | |
752 | +{ | |
753 | + return inv_mpu6050_set_power_itg( | |
754 | + iio_priv(i2c_get_clientdata(to_i2c_client(dev))), true); | |
755 | +} | |
756 | + | |
757 | +static int inv_mpu_suspend(struct device *dev) | |
758 | +{ | |
759 | + return inv_mpu6050_set_power_itg( | |
760 | + iio_priv(i2c_get_clientdata(to_i2c_client(dev))), false); | |
761 | +} | |
762 | +static SIMPLE_DEV_PM_OPS(inv_mpu_pmops, inv_mpu_suspend, inv_mpu_resume); | |
763 | + | |
764 | +#define INV_MPU6050_PMOPS (&inv_mpu_pmops) | |
765 | +#else | |
766 | +#define INV_MPU6050_PMOPS NULL | |
767 | +#endif /* CONFIG_PM_SLEEP */ | |
768 | + | |
769 | +/* | |
770 | + * device id table is used to identify what device can be | |
771 | + * supported by this driver | |
772 | + */ | |
773 | +static const struct i2c_device_id inv_mpu_id[] = { | |
774 | + {"mpu6050", INV_MPU6050}, | |
775 | + {} | |
776 | +}; | |
777 | + | |
778 | +MODULE_DEVICE_TABLE(i2c, inv_mpu_id); | |
779 | + | |
780 | +static struct i2c_driver inv_mpu_driver = { | |
781 | + .probe = inv_mpu_probe, | |
782 | + .remove = inv_mpu_remove, | |
783 | + .id_table = inv_mpu_id, | |
784 | + .driver = { | |
785 | + .owner = THIS_MODULE, | |
786 | + .name = "inv-mpu6050", | |
787 | + .pm = INV_MPU6050_PMOPS, | |
788 | + }, | |
789 | +}; | |
790 | + | |
791 | +module_i2c_driver(inv_mpu_driver); | |
792 | + | |
793 | +MODULE_AUTHOR("Invensense Corporation"); | |
794 | +MODULE_DESCRIPTION("Invensense device MPU6050 driver"); | |
795 | +MODULE_LICENSE("GPL"); |
drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
1 | +/* | |
2 | +* Copyright (C) 2012 Invensense, Inc. | |
3 | +* | |
4 | +* This software is licensed under the terms of the GNU General Public | |
5 | +* License version 2, as published by the Free Software Foundation, and | |
6 | +* may be copied, distributed, and modified under those terms. | |
7 | +* | |
8 | +* This program is distributed in the hope that it will be useful, | |
9 | +* but WITHOUT ANY WARRANTY; without even the implied warranty of | |
10 | +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
11 | +* GNU General Public License for more details. | |
12 | +*/ | |
13 | +#include <linux/i2c.h> | |
14 | +#include <linux/kfifo.h> | |
15 | +#include <linux/spinlock.h> | |
16 | +#include <linux/iio/iio.h> | |
17 | +#include <linux/iio/buffer.h> | |
18 | +#include <linux/iio/sysfs.h> | |
19 | +#include <linux/iio/kfifo_buf.h> | |
20 | +#include <linux/iio/trigger.h> | |
21 | +#include <linux/iio/triggered_buffer.h> | |
22 | +#include <linux/iio/trigger_consumer.h> | |
23 | +#include <linux/platform_data/invensense_mpu6050.h> | |
24 | + | |
25 | +/** | |
26 | + * struct inv_mpu6050_reg_map - Notable registers. | |
27 | + * @sample_rate_div: Divider applied to gyro output rate. | |
28 | + * @lpf: Configures internal low pass filter. | |
29 | + * @user_ctrl: Enables/resets the FIFO. | |
30 | + * @fifo_en: Determines which data will appear in FIFO. | |
31 | + * @gyro_config: gyro config register. | |
32 | + * @accl_config: accel config register | |
33 | + * @fifo_count_h: Upper byte of FIFO count. | |
34 | + * @fifo_r_w: FIFO register. | |
35 | + * @raw_gyro: Address of first gyro register. | |
36 | + * @raw_accl: Address of first accel register. | |
37 | + * @temperature: temperature register | |
38 | + * @int_enable: Interrupt enable register. | |
39 | + * @pwr_mgmt_1: Controls chip's power state and clock source. | |
40 | + * @pwr_mgmt_2: Controls power state of individual sensors. | |
41 | + */ | |
42 | +struct inv_mpu6050_reg_map { | |
43 | + u8 sample_rate_div; | |
44 | + u8 lpf; | |
45 | + u8 user_ctrl; | |
46 | + u8 fifo_en; | |
47 | + u8 gyro_config; | |
48 | + u8 accl_config; | |
49 | + u8 fifo_count_h; | |
50 | + u8 fifo_r_w; | |
51 | + u8 raw_gyro; | |
52 | + u8 raw_accl; | |
53 | + u8 temperature; | |
54 | + u8 int_enable; | |
55 | + u8 pwr_mgmt_1; | |
56 | + u8 pwr_mgmt_2; | |
57 | +}; | |
58 | + | |
59 | +/*device enum */ | |
60 | +enum inv_devices { | |
61 | + INV_MPU6050, | |
62 | + INV_NUM_PARTS | |
63 | +}; | |
64 | + | |
65 | +/** | |
66 | + * struct inv_mpu6050_chip_config - Cached chip configuration data. | |
67 | + * @fsr: Full scale range. | |
68 | + * @lpf: Digital low pass filter frequency. | |
69 | + * @accl_fs: accel full scale range. | |
70 | + * @enable: master enable state. | |
71 | + * @accl_fifo_enable: enable accel data output | |
72 | + * @gyro_fifo_enable: enable gyro data output | |
73 | + * @fifo_rate: FIFO update rate. | |
74 | + */ | |
75 | +struct inv_mpu6050_chip_config { | |
76 | + unsigned int fsr:2; | |
77 | + unsigned int lpf:3; | |
78 | + unsigned int accl_fs:2; | |
79 | + unsigned int enable:1; | |
80 | + unsigned int accl_fifo_enable:1; | |
81 | + unsigned int gyro_fifo_enable:1; | |
82 | + u16 fifo_rate; | |
83 | +}; | |
84 | + | |
85 | +/** | |
86 | + * struct inv_mpu6050_hw - Other important hardware information. | |
87 | + * @num_reg: Number of registers on device. | |
88 | + * @name: name of the chip. | |
89 | + * @reg: register map of the chip. | |
90 | + * @config: configuration of the chip. | |
91 | + */ | |
92 | +struct inv_mpu6050_hw { | |
93 | + u8 num_reg; | |
94 | + u8 *name; | |
95 | + const struct inv_mpu6050_reg_map *reg; | |
96 | + const struct inv_mpu6050_chip_config *config; | |
97 | +}; | |
98 | + | |
99 | +/* | |
100 | + * struct inv_mpu6050_state - Driver state variables. | |
101 | + * @TIMESTAMP_FIFO_SIZE: fifo size for timestamp. | |
102 | + * @trig: IIO trigger. | |
103 | + * @chip_config: Cached attribute information. | |
104 | + * @reg: Map of important registers. | |
105 | + * @hw: Other hardware-specific information. | |
106 | + * @chip_type: chip type. | |
107 | + * @time_stamp_lock: spin lock to time stamp. | |
108 | + * @client: i2c client handle. | |
109 | + * @plat_data: platform data. | |
110 | + * @timestamps: kfifo queue to store time stamp. | |
111 | + */ | |
112 | +struct inv_mpu6050_state { | |
113 | +#define TIMESTAMP_FIFO_SIZE 16 | |
114 | + struct iio_trigger *trig; | |
115 | + struct inv_mpu6050_chip_config chip_config; | |
116 | + const struct inv_mpu6050_reg_map *reg; | |
117 | + const struct inv_mpu6050_hw *hw; | |
118 | + enum inv_devices chip_type; | |
119 | + spinlock_t time_stamp_lock; | |
120 | + struct i2c_client *client; | |
121 | + struct inv_mpu6050_platform_data plat_data; | |
122 | + DECLARE_KFIFO(timestamps, long long, TIMESTAMP_FIFO_SIZE); | |
123 | +}; | |
124 | + | |
125 | +/*register and associated bit definition*/ | |
126 | +#define INV_MPU6050_REG_SAMPLE_RATE_DIV 0x19 | |
127 | +#define INV_MPU6050_REG_CONFIG 0x1A | |
128 | +#define INV_MPU6050_REG_GYRO_CONFIG 0x1B | |
129 | +#define INV_MPU6050_REG_ACCEL_CONFIG 0x1C | |
130 | + | |
131 | +#define INV_MPU6050_REG_FIFO_EN 0x23 | |
132 | +#define INV_MPU6050_BIT_ACCEL_OUT 0x08 | |
133 | +#define INV_MPU6050_BITS_GYRO_OUT 0x70 | |
134 | + | |
135 | +#define INV_MPU6050_REG_INT_ENABLE 0x38 | |
136 | +#define INV_MPU6050_BIT_DATA_RDY_EN 0x01 | |
137 | +#define INV_MPU6050_BIT_DMP_INT_EN 0x02 | |
138 | + | |
139 | +#define INV_MPU6050_REG_RAW_ACCEL 0x3B | |
140 | +#define INV_MPU6050_REG_TEMPERATURE 0x41 | |
141 | +#define INV_MPU6050_REG_RAW_GYRO 0x43 | |
142 | + | |
143 | +#define INV_MPU6050_REG_USER_CTRL 0x6A | |
144 | +#define INV_MPU6050_BIT_FIFO_RST 0x04 | |
145 | +#define INV_MPU6050_BIT_DMP_RST 0x08 | |
146 | +#define INV_MPU6050_BIT_I2C_MST_EN 0x20 | |
147 | +#define INV_MPU6050_BIT_FIFO_EN 0x40 | |
148 | +#define INV_MPU6050_BIT_DMP_EN 0x80 | |
149 | + | |
150 | +#define INV_MPU6050_REG_PWR_MGMT_1 0x6B | |
151 | +#define INV_MPU6050_BIT_H_RESET 0x80 | |
152 | +#define INV_MPU6050_BIT_SLEEP 0x40 | |
153 | +#define INV_MPU6050_BIT_CLK_MASK 0x7 | |
154 | + | |
155 | +#define INV_MPU6050_REG_PWR_MGMT_2 0x6C | |
156 | +#define INV_MPU6050_BIT_PWR_ACCL_STBY 0x38 | |
157 | +#define INV_MPU6050_BIT_PWR_GYRO_STBY 0x07 | |
158 | + | |
159 | +#define INV_MPU6050_REG_FIFO_COUNT_H 0x72 | |
160 | +#define INV_MPU6050_REG_FIFO_R_W 0x74 | |
161 | + | |
162 | +#define INV_MPU6050_BYTES_PER_3AXIS_SENSOR 6 | |
163 | +#define INV_MPU6050_FIFO_COUNT_BYTE 2 | |
164 | +#define INV_MPU6050_FIFO_THRESHOLD 500 | |
165 | +#define INV_MPU6050_POWER_UP_TIME 100 | |
166 | +#define INV_MPU6050_TEMP_UP_TIME 100 | |
167 | +#define INV_MPU6050_SENSOR_UP_TIME 30 | |
168 | +#define INV_MPU6050_REG_UP_TIME 5 | |
169 | + | |
170 | +#define INV_MPU6050_TEMP_OFFSET 12421 | |
171 | +#define INV_MPU6050_TEMP_SCALE 2941 | |
172 | +#define INV_MPU6050_MAX_GYRO_FS_PARAM 3 | |
173 | +#define INV_MPU6050_MAX_ACCL_FS_PARAM 3 | |
174 | +#define INV_MPU6050_THREE_AXIS 3 | |
175 | +#define INV_MPU6050_GYRO_CONFIG_FSR_SHIFT 3 | |
176 | +#define INV_MPU6050_ACCL_CONFIG_FSR_SHIFT 3 | |
177 | + | |
178 | +/* 6 + 6 round up and plus 8 */ | |
179 | +#define INV_MPU6050_OUTPUT_DATA_SIZE 24 | |
180 | + | |
181 | +/* init parameters */ | |
182 | +#define INV_MPU6050_INIT_FIFO_RATE 50 | |
183 | +#define INV_MPU6050_TIME_STAMP_TOR 5 | |
184 | +#define INV_MPU6050_MAX_FIFO_RATE 1000 | |
185 | +#define INV_MPU6050_MIN_FIFO_RATE 4 | |
186 | +#define INV_MPU6050_ONE_K_HZ 1000 | |
187 | + | |
188 | +/* scan element definition */ | |
189 | +enum inv_mpu6050_scan { | |
190 | + INV_MPU6050_SCAN_ACCL_X, | |
191 | + INV_MPU6050_SCAN_ACCL_Y, | |
192 | + INV_MPU6050_SCAN_ACCL_Z, | |
193 | + INV_MPU6050_SCAN_GYRO_X, | |
194 | + INV_MPU6050_SCAN_GYRO_Y, | |
195 | + INV_MPU6050_SCAN_GYRO_Z, | |
196 | + INV_MPU6050_SCAN_TIMESTAMP, | |
197 | +}; | |
198 | + | |
199 | +enum inv_mpu6050_filter_e { | |
200 | + INV_MPU6050_FILTER_256HZ_NOLPF2 = 0, | |
201 | + INV_MPU6050_FILTER_188HZ, | |
202 | + INV_MPU6050_FILTER_98HZ, | |
203 | + INV_MPU6050_FILTER_42HZ, | |
204 | + INV_MPU6050_FILTER_20HZ, | |
205 | + INV_MPU6050_FILTER_10HZ, | |
206 | + INV_MPU6050_FILTER_5HZ, | |
207 | + INV_MPU6050_FILTER_2100HZ_NOLPF, | |
208 | + NUM_MPU6050_FILTER | |
209 | +}; | |
210 | + | |
211 | +/* IIO attribute address */ | |
212 | +enum INV_MPU6050_IIO_ATTR_ADDR { | |
213 | + ATTR_GYRO_MATRIX, | |
214 | + ATTR_ACCL_MATRIX, | |
215 | +}; | |
216 | + | |
217 | +enum inv_mpu6050_accl_fs_e { | |
218 | + INV_MPU6050_FS_02G = 0, | |
219 | + INV_MPU6050_FS_04G, | |
220 | + INV_MPU6050_FS_08G, | |
221 | + INV_MPU6050_FS_16G, | |
222 | + NUM_ACCL_FSR | |
223 | +}; | |
224 | + | |
225 | +enum inv_mpu6050_fsr_e { | |
226 | + INV_MPU6050_FSR_250DPS = 0, | |
227 | + INV_MPU6050_FSR_500DPS, | |
228 | + INV_MPU6050_FSR_1000DPS, | |
229 | + INV_MPU6050_FSR_2000DPS, | |
230 | + NUM_MPU6050_FSR | |
231 | +}; | |
232 | + | |
233 | +enum inv_mpu6050_clock_sel_e { | |
234 | + INV_CLK_INTERNAL = 0, | |
235 | + INV_CLK_PLL, | |
236 | + NUM_CLK | |
237 | +}; | |
238 | + | |
239 | +irqreturn_t inv_mpu6050_irq_handler(int irq, void *p); | |
240 | +irqreturn_t inv_mpu6050_read_fifo(int irq, void *p); | |
241 | +int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev); | |
242 | +void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st); | |
243 | +int inv_reset_fifo(struct iio_dev *indio_dev); | |
244 | +int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask); | |
245 | +int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val); | |
246 | +int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on); |
drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
1 | +/* | |
2 | +* Copyright (C) 2012 Invensense, Inc. | |
3 | +* | |
4 | +* This software is licensed under the terms of the GNU General Public | |
5 | +* License version 2, as published by the Free Software Foundation, and | |
6 | +* may be copied, distributed, and modified under those terms. | |
7 | +* | |
8 | +* This program is distributed in the hope that it will be useful, | |
9 | +* but WITHOUT ANY WARRANTY; without even the implied warranty of | |
10 | +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
11 | +* GNU General Public License for more details. | |
12 | +*/ | |
13 | + | |
14 | +#include <linux/module.h> | |
15 | +#include <linux/init.h> | |
16 | +#include <linux/slab.h> | |
17 | +#include <linux/i2c.h> | |
18 | +#include <linux/err.h> | |
19 | +#include <linux/delay.h> | |
20 | +#include <linux/sysfs.h> | |
21 | +#include <linux/jiffies.h> | |
22 | +#include <linux/irq.h> | |
23 | +#include <linux/interrupt.h> | |
24 | +#include <linux/kfifo.h> | |
25 | +#include <linux/poll.h> | |
26 | +#include "inv_mpu_iio.h" | |
27 | + | |
28 | +int inv_reset_fifo(struct iio_dev *indio_dev) | |
29 | +{ | |
30 | + int result; | |
31 | + u8 d; | |
32 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
33 | + | |
34 | + /* disable interrupt */ | |
35 | + result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0); | |
36 | + if (result) { | |
37 | + dev_err(&st->client->dev, "int_enable failed %d\n", result); | |
38 | + return result; | |
39 | + } | |
40 | + /* disable the sensor output to FIFO */ | |
41 | + result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0); | |
42 | + if (result) | |
43 | + goto reset_fifo_fail; | |
44 | + /* disable fifo reading */ | |
45 | + result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0); | |
46 | + if (result) | |
47 | + goto reset_fifo_fail; | |
48 | + | |
49 | + /* reset FIFO*/ | |
50 | + result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, | |
51 | + INV_MPU6050_BIT_FIFO_RST); | |
52 | + if (result) | |
53 | + goto reset_fifo_fail; | |
54 | + /* enable interrupt */ | |
55 | + if (st->chip_config.accl_fifo_enable || | |
56 | + st->chip_config.gyro_fifo_enable) { | |
57 | + result = inv_mpu6050_write_reg(st, st->reg->int_enable, | |
58 | + INV_MPU6050_BIT_DATA_RDY_EN); | |
59 | + if (result) | |
60 | + return result; | |
61 | + } | |
62 | + /* enable FIFO reading and I2C master interface*/ | |
63 | + result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, | |
64 | + INV_MPU6050_BIT_FIFO_EN); | |
65 | + if (result) | |
66 | + goto reset_fifo_fail; | |
67 | + /* enable sensor output to FIFO */ | |
68 | + d = 0; | |
69 | + if (st->chip_config.gyro_fifo_enable) | |
70 | + d |= INV_MPU6050_BITS_GYRO_OUT; | |
71 | + if (st->chip_config.accl_fifo_enable) | |
72 | + d |= INV_MPU6050_BIT_ACCEL_OUT; | |
73 | + result = inv_mpu6050_write_reg(st, st->reg->fifo_en, d); | |
74 | + if (result) | |
75 | + goto reset_fifo_fail; | |
76 | + | |
77 | + return 0; | |
78 | + | |
79 | +reset_fifo_fail: | |
80 | + dev_err(&st->client->dev, "reset fifo failed %d\n", result); | |
81 | + result = inv_mpu6050_write_reg(st, st->reg->int_enable, | |
82 | + INV_MPU6050_BIT_DATA_RDY_EN); | |
83 | + | |
84 | + return result; | |
85 | +} | |
86 | + | |
87 | +static void inv_clear_kfifo(struct inv_mpu6050_state *st) | |
88 | +{ | |
89 | + unsigned long flags; | |
90 | + | |
91 | + /* take the spin lock sem to avoid interrupt kick in */ | |
92 | + spin_lock_irqsave(&st->time_stamp_lock, flags); | |
93 | + kfifo_reset(&st->timestamps); | |
94 | + spin_unlock_irqrestore(&st->time_stamp_lock, flags); | |
95 | +} | |
96 | + | |
97 | +/** | |
98 | + * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt. | |
99 | + */ | |
100 | +irqreturn_t inv_mpu6050_irq_handler(int irq, void *p) | |
101 | +{ | |
102 | + struct iio_poll_func *pf = p; | |
103 | + struct iio_dev *indio_dev = pf->indio_dev; | |
104 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
105 | + s64 timestamp; | |
106 | + | |
107 | + timestamp = iio_get_time_ns(); | |
108 | + spin_lock(&st->time_stamp_lock); | |
109 | + kfifo_in(&st->timestamps, ×tamp, 1); | |
110 | + spin_unlock(&st->time_stamp_lock); | |
111 | + | |
112 | + return IRQ_WAKE_THREAD; | |
113 | +} | |
114 | + | |
115 | +/** | |
116 | + * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO. | |
117 | + */ | |
118 | +irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) | |
119 | +{ | |
120 | + struct iio_poll_func *pf = p; | |
121 | + struct iio_dev *indio_dev = pf->indio_dev; | |
122 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
123 | + size_t bytes_per_datum; | |
124 | + int result; | |
125 | + u8 data[INV_MPU6050_OUTPUT_DATA_SIZE]; | |
126 | + u16 fifo_count; | |
127 | + s64 timestamp; | |
128 | + u64 *tmp; | |
129 | + | |
130 | + mutex_lock(&indio_dev->mlock); | |
131 | + if (!(st->chip_config.accl_fifo_enable | | |
132 | + st->chip_config.gyro_fifo_enable)) | |
133 | + goto end_session; | |
134 | + bytes_per_datum = 0; | |
135 | + if (st->chip_config.accl_fifo_enable) | |
136 | + bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; | |
137 | + | |
138 | + if (st->chip_config.gyro_fifo_enable) | |
139 | + bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; | |
140 | + | |
141 | + /* | |
142 | + * read fifo_count register to know how many bytes inside FIFO | |
143 | + * right now | |
144 | + */ | |
145 | + result = i2c_smbus_read_i2c_block_data(st->client, | |
146 | + st->reg->fifo_count_h, | |
147 | + INV_MPU6050_FIFO_COUNT_BYTE, data); | |
148 | + if (result != INV_MPU6050_FIFO_COUNT_BYTE) | |
149 | + goto end_session; | |
150 | + fifo_count = be16_to_cpup((__be16 *)(&data[0])); | |
151 | + if (fifo_count < bytes_per_datum) | |
152 | + goto end_session; | |
153 | + /* fifo count can't be odd number, if it is odd, reset fifo*/ | |
154 | + if (fifo_count & 1) | |
155 | + goto flush_fifo; | |
156 | + if (fifo_count > INV_MPU6050_FIFO_THRESHOLD) | |
157 | + goto flush_fifo; | |
158 | + /* Timestamp mismatch. */ | |
159 | + if (kfifo_len(&st->timestamps) > | |
160 | + fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR) | |
161 | + goto flush_fifo; | |
162 | + while (fifo_count >= bytes_per_datum) { | |
163 | + result = i2c_smbus_read_i2c_block_data(st->client, | |
164 | + st->reg->fifo_r_w, | |
165 | + bytes_per_datum, data); | |
166 | + if (result != bytes_per_datum) | |
167 | + goto flush_fifo; | |
168 | + | |
169 | + result = kfifo_out(&st->timestamps, ×tamp, 1); | |
170 | + /* when there is no timestamp, put timestamp as 0 */ | |
171 | + if (0 == result) | |
172 | + timestamp = 0; | |
173 | + | |
174 | + tmp = (u64 *)data; | |
175 | + tmp[DIV_ROUND_UP(bytes_per_datum, 8)] = timestamp; | |
176 | + result = iio_push_to_buffers(indio_dev, data); | |
177 | + if (result) | |
178 | + goto flush_fifo; | |
179 | + fifo_count -= bytes_per_datum; | |
180 | + } | |
181 | + | |
182 | +end_session: | |
183 | + mutex_unlock(&indio_dev->mlock); | |
184 | + iio_trigger_notify_done(indio_dev->trig); | |
185 | + | |
186 | + return IRQ_HANDLED; | |
187 | + | |
188 | +flush_fifo: | |
189 | + /* Flush HW and SW FIFOs. */ | |
190 | + inv_reset_fifo(indio_dev); | |
191 | + inv_clear_kfifo(st); | |
192 | + mutex_unlock(&indio_dev->mlock); | |
193 | + iio_trigger_notify_done(indio_dev->trig); | |
194 | + | |
195 | + return IRQ_HANDLED; | |
196 | +} |
drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
1 | +/* | |
2 | +* Copyright (C) 2012 Invensense, Inc. | |
3 | +* | |
4 | +* This software is licensed under the terms of the GNU General Public | |
5 | +* License version 2, as published by the Free Software Foundation, and | |
6 | +* may be copied, distributed, and modified under those terms. | |
7 | +* | |
8 | +* This program is distributed in the hope that it will be useful, | |
9 | +* but WITHOUT ANY WARRANTY; without even the implied warranty of | |
10 | +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
11 | +* GNU General Public License for more details. | |
12 | +*/ | |
13 | + | |
14 | +#include "inv_mpu_iio.h" | |
15 | + | |
16 | +static void inv_scan_query(struct iio_dev *indio_dev) | |
17 | +{ | |
18 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
19 | + | |
20 | + st->chip_config.gyro_fifo_enable = | |
21 | + test_bit(INV_MPU6050_SCAN_GYRO_X, | |
22 | + indio_dev->active_scan_mask) || | |
23 | + test_bit(INV_MPU6050_SCAN_GYRO_Y, | |
24 | + indio_dev->active_scan_mask) || | |
25 | + test_bit(INV_MPU6050_SCAN_GYRO_Z, | |
26 | + indio_dev->active_scan_mask); | |
27 | + | |
28 | + st->chip_config.accl_fifo_enable = | |
29 | + test_bit(INV_MPU6050_SCAN_ACCL_X, | |
30 | + indio_dev->active_scan_mask) || | |
31 | + test_bit(INV_MPU6050_SCAN_ACCL_Y, | |
32 | + indio_dev->active_scan_mask) || | |
33 | + test_bit(INV_MPU6050_SCAN_ACCL_Z, | |
34 | + indio_dev->active_scan_mask); | |
35 | +} | |
36 | + | |
37 | +/** | |
38 | + * inv_mpu6050_set_enable() - enable chip functions. | |
39 | + * @indio_dev: Device driver instance. | |
40 | + * @enable: enable/disable | |
41 | + */ | |
42 | +static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) | |
43 | +{ | |
44 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
45 | + int result; | |
46 | + | |
47 | + if (enable) { | |
48 | + result = inv_mpu6050_set_power_itg(st, true); | |
49 | + if (result) | |
50 | + return result; | |
51 | + inv_scan_query(indio_dev); | |
52 | + if (st->chip_config.gyro_fifo_enable) { | |
53 | + result = inv_mpu6050_switch_engine(st, true, | |
54 | + INV_MPU6050_BIT_PWR_GYRO_STBY); | |
55 | + if (result) | |
56 | + return result; | |
57 | + } | |
58 | + if (st->chip_config.accl_fifo_enable) { | |
59 | + result = inv_mpu6050_switch_engine(st, true, | |
60 | + INV_MPU6050_BIT_PWR_ACCL_STBY); | |
61 | + if (result) | |
62 | + return result; | |
63 | + } | |
64 | + result = inv_reset_fifo(indio_dev); | |
65 | + if (result) | |
66 | + return result; | |
67 | + } else { | |
68 | + result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0); | |
69 | + if (result) | |
70 | + return result; | |
71 | + | |
72 | + result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0); | |
73 | + if (result) | |
74 | + return result; | |
75 | + | |
76 | + result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0); | |
77 | + if (result) | |
78 | + return result; | |
79 | + | |
80 | + result = inv_mpu6050_switch_engine(st, false, | |
81 | + INV_MPU6050_BIT_PWR_GYRO_STBY); | |
82 | + if (result) | |
83 | + return result; | |
84 | + | |
85 | + result = inv_mpu6050_switch_engine(st, false, | |
86 | + INV_MPU6050_BIT_PWR_ACCL_STBY); | |
87 | + if (result) | |
88 | + return result; | |
89 | + result = inv_mpu6050_set_power_itg(st, false); | |
90 | + if (result) | |
91 | + return result; | |
92 | + } | |
93 | + st->chip_config.enable = enable; | |
94 | + | |
95 | + return 0; | |
96 | +} | |
97 | + | |
98 | +/** | |
99 | + * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state | |
100 | + * @trig: Trigger instance | |
101 | + * @state: Desired trigger state | |
102 | + */ | |
103 | +static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig, | |
104 | + bool state) | |
105 | +{ | |
106 | + return inv_mpu6050_set_enable(trig->private_data, state); | |
107 | +} | |
108 | + | |
109 | +static const struct iio_trigger_ops inv_mpu_trigger_ops = { | |
110 | + .owner = THIS_MODULE, | |
111 | + .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, | |
112 | +}; | |
113 | + | |
114 | +int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev) | |
115 | +{ | |
116 | + int ret; | |
117 | + struct inv_mpu6050_state *st = iio_priv(indio_dev); | |
118 | + | |
119 | + st->trig = iio_trigger_alloc("%s-dev%d", | |
120 | + indio_dev->name, | |
121 | + indio_dev->id); | |
122 | + if (st->trig == NULL) { | |
123 | + ret = -ENOMEM; | |
124 | + goto error_ret; | |
125 | + } | |
126 | + ret = request_irq(st->client->irq, &iio_trigger_generic_data_rdy_poll, | |
127 | + IRQF_TRIGGER_RISING, | |
128 | + "inv_mpu", | |
129 | + st->trig); | |
130 | + if (ret) | |
131 | + goto error_free_trig; | |
132 | + st->trig->dev.parent = &st->client->dev; | |
133 | + st->trig->private_data = indio_dev; | |
134 | + st->trig->ops = &inv_mpu_trigger_ops; | |
135 | + ret = iio_trigger_register(st->trig); | |
136 | + if (ret) | |
137 | + goto error_free_irq; | |
138 | + indio_dev->trig = st->trig; | |
139 | + | |
140 | + return 0; | |
141 | + | |
142 | +error_free_irq: | |
143 | + free_irq(st->client->irq, st->trig); | |
144 | +error_free_trig: | |
145 | + iio_trigger_free(st->trig); | |
146 | +error_ret: | |
147 | + return ret; | |
148 | +} | |
149 | + | |
150 | +void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st) | |
151 | +{ | |
152 | + iio_trigger_unregister(st->trig); | |
153 | + free_irq(st->client->irq, st->trig); | |
154 | + iio_trigger_free(st->trig); | |
155 | +} |
include/linux/platform_data/invensense_mpu6050.h
1 | +/* | |
2 | +* Copyright (C) 2012 Invensense, Inc. | |
3 | +* | |
4 | +* This software is licensed under the terms of the GNU General Public | |
5 | +* License version 2, as published by the Free Software Foundation, and | |
6 | +* may be copied, distributed, and modified under those terms. | |
7 | +* | |
8 | +* This program is distributed in the hope that it will be useful, | |
9 | +* but WITHOUT ANY WARRANTY; without even the implied warranty of | |
10 | +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
11 | +* GNU General Public License for more details. | |
12 | +*/ | |
13 | + | |
14 | +#ifndef __INV_MPU6050_PLATFORM_H_ | |
15 | +#define __INV_MPU6050_PLATFORM_H_ | |
16 | + | |
17 | +/** | |
18 | + * struct inv_mpu6050_platform_data - Platform data for the mpu driver | |
19 | + * @orientation: Orientation matrix of the chip | |
20 | + * | |
21 | + * Contains platform specific information on how to configure the MPU6050 to | |
22 | + * work on this platform. The orientation matricies are 3x3 rotation matricies | |
23 | + * that are applied to the data to rotate from the mounting orientation to the | |
24 | + * platform orientation. The values must be one of 0, 1, or -1 and each row and | |
25 | + * column should have exactly 1 non-zero value. | |
26 | + */ | |
27 | +struct inv_mpu6050_platform_data { | |
28 | + __s8 orientation[9]; | |
29 | +}; | |
30 | + | |
31 | +#endif |