1
2
3
4
5
6
7
8
9
10
11
12
13
14#include <linux/module.h>
15#include <linux/slab.h>
16#include <linux/i2c.h>
17#include <linux/err.h>
18#include <linux/delay.h>
19#include <linux/sysfs.h>
20#include <linux/jiffies.h>
21#include <linux/irq.h>
22#include <linux/interrupt.h>
23#include <linux/kfifo.h>
24#include <linux/spinlock.h>
25#include <linux/iio/iio.h>
26#include <linux/i2c-mux.h>
27#include <linux/acpi.h>
28#include "inv_mpu_iio.h"
29
30
31
32
33
34static const int gyro_scale_6050[] = {133090, 266181, 532362, 1064724};
35
36
37
38
39
40static const int accel_scale[] = {598, 1196, 2392, 4785};
41
42static const struct inv_mpu6050_reg_map reg_set_6500 = {
43 .sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV,
44 .lpf = INV_MPU6050_REG_CONFIG,
45 .user_ctrl = INV_MPU6050_REG_USER_CTRL,
46 .fifo_en = INV_MPU6050_REG_FIFO_EN,
47 .gyro_config = INV_MPU6050_REG_GYRO_CONFIG,
48 .accl_config = INV_MPU6050_REG_ACCEL_CONFIG,
49 .fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H,
50 .fifo_r_w = INV_MPU6050_REG_FIFO_R_W,
51 .raw_gyro = INV_MPU6050_REG_RAW_GYRO,
52 .raw_accl = INV_MPU6050_REG_RAW_ACCEL,
53 .temperature = INV_MPU6050_REG_TEMPERATURE,
54 .int_enable = INV_MPU6050_REG_INT_ENABLE,
55 .pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1,
56 .pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2,
57 .int_pin_cfg = INV_MPU6050_REG_INT_PIN_CFG,
58 .accl_offset = INV_MPU6500_REG_ACCEL_OFFSET,
59 .gyro_offset = INV_MPU6050_REG_GYRO_OFFSET,
60};
61
62static const struct inv_mpu6050_reg_map reg_set_6050 = {
63 .sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV,
64 .lpf = INV_MPU6050_REG_CONFIG,
65 .user_ctrl = INV_MPU6050_REG_USER_CTRL,
66 .fifo_en = INV_MPU6050_REG_FIFO_EN,
67 .gyro_config = INV_MPU6050_REG_GYRO_CONFIG,
68 .accl_config = INV_MPU6050_REG_ACCEL_CONFIG,
69 .fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H,
70 .fifo_r_w = INV_MPU6050_REG_FIFO_R_W,
71 .raw_gyro = INV_MPU6050_REG_RAW_GYRO,
72 .raw_accl = INV_MPU6050_REG_RAW_ACCEL,
73 .temperature = INV_MPU6050_REG_TEMPERATURE,
74 .int_enable = INV_MPU6050_REG_INT_ENABLE,
75 .pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1,
76 .pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2,
77 .int_pin_cfg = INV_MPU6050_REG_INT_PIN_CFG,
78 .accl_offset = INV_MPU6050_REG_ACCEL_OFFSET,
79 .gyro_offset = INV_MPU6050_REG_GYRO_OFFSET,
80};
81
82static const struct inv_mpu6050_chip_config chip_config_6050 = {
83 .fsr = INV_MPU6050_FSR_2000DPS,
84 .lpf = INV_MPU6050_FILTER_20HZ,
85 .fifo_rate = INV_MPU6050_INIT_FIFO_RATE,
86 .gyro_fifo_enable = false,
87 .accl_fifo_enable = false,
88 .accl_fs = INV_MPU6050_FS_02G,
89};
90
91static const struct inv_mpu6050_hw hw_info[] = {
92 {
93 .num_reg = 117,
94 .name = "MPU6500",
95 .reg = ®_set_6500,
96 .config = &chip_config_6050,
97 },
98 {
99 .num_reg = 117,
100 .name = "MPU6050",
101 .reg = ®_set_6050,
102 .config = &chip_config_6050,
103 },
104};
105
106int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask)
107{
108 unsigned int d, mgmt_1;
109 int result;
110
111
112
113
114
115 if (mask == INV_MPU6050_BIT_PWR_GYRO_STBY) {
116 result = regmap_read(st->map, st->reg->pwr_mgmt_1, &mgmt_1);
117 if (result)
118 return result;
119
120 mgmt_1 &= ~INV_MPU6050_BIT_CLK_MASK;
121 }
122
123 if ((mask == INV_MPU6050_BIT_PWR_GYRO_STBY) && (!en)) {
124
125
126
127
128 mgmt_1 |= INV_CLK_INTERNAL;
129 result = regmap_write(st->map, st->reg->pwr_mgmt_1, mgmt_1);
130 if (result)
131 return result;
132 }
133
134 result = regmap_read(st->map, st->reg->pwr_mgmt_2, &d);
135 if (result)
136 return result;
137 if (en)
138 d &= ~mask;
139 else
140 d |= mask;
141 result = regmap_write(st->map, st->reg->pwr_mgmt_2, d);
142 if (result)
143 return result;
144
145 if (en) {
146
147 msleep(INV_MPU6050_TEMP_UP_TIME);
148 if (mask == INV_MPU6050_BIT_PWR_GYRO_STBY) {
149
150 mgmt_1 |= INV_CLK_PLL;
151 result = regmap_write(st->map,
152 st->reg->pwr_mgmt_1, mgmt_1);
153 if (result)
154 return result;
155 }
156 }
157
158 return 0;
159}
160
161int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on)
162{
163 int result = 0;
164
165 if (power_on) {
166
167 if (!st->powerup_count)
168 result = regmap_write(st->map, st->reg->pwr_mgmt_1, 0);
169 if (!result)
170 st->powerup_count++;
171 } else {
172 st->powerup_count--;
173 if (!st->powerup_count)
174 result = regmap_write(st->map, st->reg->pwr_mgmt_1,
175 INV_MPU6050_BIT_SLEEP);
176 }
177
178 if (result)
179 return result;
180
181 if (power_on)
182 usleep_range(INV_MPU6050_REG_UP_TIME_MIN,
183 INV_MPU6050_REG_UP_TIME_MAX);
184
185 return 0;
186}
187EXPORT_SYMBOL_GPL(inv_mpu6050_set_power_itg);
188
189
190
191
192
193
194
195
196
197
198static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
199{
200 int result;
201 u8 d;
202 struct inv_mpu6050_state *st = iio_priv(indio_dev);
203
204 result = inv_mpu6050_set_power_itg(st, true);
205 if (result)
206 return result;
207 d = (INV_MPU6050_FSR_2000DPS << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT);
208 result = regmap_write(st->map, st->reg->gyro_config, d);
209 if (result)
210 return result;
211
212 d = INV_MPU6050_FILTER_20HZ;
213 result = regmap_write(st->map, st->reg->lpf, d);
214 if (result)
215 return result;
216
217 d = INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE - 1;
218 result = regmap_write(st->map, st->reg->sample_rate_div, d);
219 if (result)
220 return result;
221
222 d = (INV_MPU6050_FS_02G << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT);
223 result = regmap_write(st->map, st->reg->accl_config, d);
224 if (result)
225 return result;
226
227 memcpy(&st->chip_config, hw_info[st->chip_type].config,
228 sizeof(struct inv_mpu6050_chip_config));
229 result = inv_mpu6050_set_power_itg(st, false);
230
231 return result;
232}
233
234static int inv_mpu6050_sensor_set(struct inv_mpu6050_state *st, int reg,
235 int axis, int val)
236{
237 int ind, result;
238 __be16 d = cpu_to_be16(val);
239
240 ind = (axis - IIO_MOD_X) * 2;
241 result = regmap_bulk_write(st->map, reg + ind, (u8 *)&d, 2);
242 if (result)
243 return -EINVAL;
244
245 return 0;
246}
247
248static int inv_mpu6050_sensor_show(struct inv_mpu6050_state *st, int reg,
249 int axis, int *val)
250{
251 int ind, result;
252 __be16 d;
253
254 ind = (axis - IIO_MOD_X) * 2;
255 result = regmap_bulk_read(st->map, reg + ind, (u8 *)&d, 2);
256 if (result)
257 return -EINVAL;
258 *val = (short)be16_to_cpup(&d);
259
260 return IIO_VAL_INT;
261}
262
263static int
264inv_mpu6050_read_raw(struct iio_dev *indio_dev,
265 struct iio_chan_spec const *chan,
266 int *val, int *val2, long mask)
267{
268 struct inv_mpu6050_state *st = iio_priv(indio_dev);
269 int ret = 0;
270
271 switch (mask) {
272 case IIO_CHAN_INFO_RAW:
273 {
274 int result;
275
276 ret = IIO_VAL_INT;
277 result = 0;
278 mutex_lock(&indio_dev->mlock);
279 if (!st->chip_config.enable) {
280 result = inv_mpu6050_set_power_itg(st, true);
281 if (result)
282 goto error_read_raw;
283 }
284
285 switch (chan->type) {
286 case IIO_ANGL_VEL:
287 if (!st->chip_config.gyro_fifo_enable ||
288 !st->chip_config.enable) {
289 result = inv_mpu6050_switch_engine(st, true,
290 INV_MPU6050_BIT_PWR_GYRO_STBY);
291 if (result)
292 goto error_read_raw;
293 }
294 ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro,
295 chan->channel2, val);
296 if (!st->chip_config.gyro_fifo_enable ||
297 !st->chip_config.enable) {
298 result = inv_mpu6050_switch_engine(st, false,
299 INV_MPU6050_BIT_PWR_GYRO_STBY);
300 if (result)
301 goto error_read_raw;
302 }
303 break;
304 case IIO_ACCEL:
305 if (!st->chip_config.accl_fifo_enable ||
306 !st->chip_config.enable) {
307 result = inv_mpu6050_switch_engine(st, true,
308 INV_MPU6050_BIT_PWR_ACCL_STBY);
309 if (result)
310 goto error_read_raw;
311 }
312 ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl,
313 chan->channel2, val);
314 if (!st->chip_config.accl_fifo_enable ||
315 !st->chip_config.enable) {
316 result = inv_mpu6050_switch_engine(st, false,
317 INV_MPU6050_BIT_PWR_ACCL_STBY);
318 if (result)
319 goto error_read_raw;
320 }
321 break;
322 case IIO_TEMP:
323
324 msleep(INV_MPU6050_SENSOR_UP_TIME);
325 ret = inv_mpu6050_sensor_show(st, st->reg->temperature,
326 IIO_MOD_X, val);
327 break;
328 default:
329 ret = -EINVAL;
330 break;
331 }
332error_read_raw:
333 if (!st->chip_config.enable)
334 result |= inv_mpu6050_set_power_itg(st, false);
335 mutex_unlock(&indio_dev->mlock);
336 if (result)
337 return result;
338
339 return ret;
340 }
341 case IIO_CHAN_INFO_SCALE:
342 switch (chan->type) {
343 case IIO_ANGL_VEL:
344 *val = 0;
345 *val2 = gyro_scale_6050[st->chip_config.fsr];
346
347 return IIO_VAL_INT_PLUS_NANO;
348 case IIO_ACCEL:
349 *val = 0;
350 *val2 = accel_scale[st->chip_config.accl_fs];
351
352 return IIO_VAL_INT_PLUS_MICRO;
353 case IIO_TEMP:
354 *val = 0;
355 *val2 = INV_MPU6050_TEMP_SCALE;
356
357 return IIO_VAL_INT_PLUS_MICRO;
358 default:
359 return -EINVAL;
360 }
361 case IIO_CHAN_INFO_OFFSET:
362 switch (chan->type) {
363 case IIO_TEMP:
364 *val = INV_MPU6050_TEMP_OFFSET;
365
366 return IIO_VAL_INT;
367 default:
368 return -EINVAL;
369 }
370 case IIO_CHAN_INFO_CALIBBIAS:
371 switch (chan->type) {
372 case IIO_ANGL_VEL:
373 ret = inv_mpu6050_sensor_show(st, st->reg->gyro_offset,
374 chan->channel2, val);
375 return IIO_VAL_INT;
376 case IIO_ACCEL:
377 ret = inv_mpu6050_sensor_show(st, st->reg->accl_offset,
378 chan->channel2, val);
379 return IIO_VAL_INT;
380
381 default:
382 return -EINVAL;
383 }
384 default:
385 return -EINVAL;
386 }
387}
388
389static int inv_mpu6050_write_gyro_scale(struct inv_mpu6050_state *st, int val)
390{
391 int result, i;
392 u8 d;
393
394 for (i = 0; i < ARRAY_SIZE(gyro_scale_6050); ++i) {
395 if (gyro_scale_6050[i] == val) {
396 d = (i << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT);
397 result = regmap_write(st->map, st->reg->gyro_config, d);
398 if (result)
399 return result;
400
401 st->chip_config.fsr = i;
402 return 0;
403 }
404 }
405
406 return -EINVAL;
407}
408
409static int inv_write_raw_get_fmt(struct iio_dev *indio_dev,
410 struct iio_chan_spec const *chan, long mask)
411{
412 switch (mask) {
413 case IIO_CHAN_INFO_SCALE:
414 switch (chan->type) {
415 case IIO_ANGL_VEL:
416 return IIO_VAL_INT_PLUS_NANO;
417 default:
418 return IIO_VAL_INT_PLUS_MICRO;
419 }
420 default:
421 return IIO_VAL_INT_PLUS_MICRO;
422 }
423
424 return -EINVAL;
425}
426
427static int inv_mpu6050_write_accel_scale(struct inv_mpu6050_state *st, int val)
428{
429 int result, i;
430 u8 d;
431
432 for (i = 0; i < ARRAY_SIZE(accel_scale); ++i) {
433 if (accel_scale[i] == val) {
434 d = (i << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT);
435 result = regmap_write(st->map, st->reg->accl_config, d);
436 if (result)
437 return result;
438
439 st->chip_config.accl_fs = i;
440 return 0;
441 }
442 }
443
444 return -EINVAL;
445}
446
447static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
448 struct iio_chan_spec const *chan,
449 int val, int val2, long mask)
450{
451 struct inv_mpu6050_state *st = iio_priv(indio_dev);
452 int result;
453
454 mutex_lock(&indio_dev->mlock);
455
456
457
458
459 if (st->chip_config.enable) {
460 result = -EBUSY;
461 goto error_write_raw;
462 }
463 result = inv_mpu6050_set_power_itg(st, true);
464 if (result)
465 goto error_write_raw;
466
467 switch (mask) {
468 case IIO_CHAN_INFO_SCALE:
469 switch (chan->type) {
470 case IIO_ANGL_VEL:
471 result = inv_mpu6050_write_gyro_scale(st, val2);
472 break;
473 case IIO_ACCEL:
474 result = inv_mpu6050_write_accel_scale(st, val2);
475 break;
476 default:
477 result = -EINVAL;
478 break;
479 }
480 break;
481 case IIO_CHAN_INFO_CALIBBIAS:
482 switch (chan->type) {
483 case IIO_ANGL_VEL:
484 result = inv_mpu6050_sensor_set(st,
485 st->reg->gyro_offset,
486 chan->channel2, val);
487 break;
488 case IIO_ACCEL:
489 result = inv_mpu6050_sensor_set(st,
490 st->reg->accl_offset,
491 chan->channel2, val);
492 break;
493 default:
494 result = -EINVAL;
495 }
496 default:
497 result = -EINVAL;
498 break;
499 }
500
501error_write_raw:
502 result |= inv_mpu6050_set_power_itg(st, false);
503 mutex_unlock(&indio_dev->mlock);
504
505 return result;
506}
507
508
509
510
511
512
513
514
515
516
517static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate)
518{
519 const int hz[] = {188, 98, 42, 20, 10, 5};
520 const int d[] = {INV_MPU6050_FILTER_188HZ, INV_MPU6050_FILTER_98HZ,
521 INV_MPU6050_FILTER_42HZ, INV_MPU6050_FILTER_20HZ,
522 INV_MPU6050_FILTER_10HZ, INV_MPU6050_FILTER_5HZ};
523 int i, h, result;
524 u8 data;
525
526 h = (rate >> 1);
527 i = 0;
528 while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1))
529 i++;
530 data = d[i];
531 result = regmap_write(st->map, st->reg->lpf, data);
532 if (result)
533 return result;
534 st->chip_config.lpf = data;
535
536 return 0;
537}
538
539
540
541
542static ssize_t
543inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
544 const char *buf, size_t count)
545{
546 s32 fifo_rate;
547 u8 d;
548 int result;
549 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
550 struct inv_mpu6050_state *st = iio_priv(indio_dev);
551
552 if (kstrtoint(buf, 10, &fifo_rate))
553 return -EINVAL;
554 if (fifo_rate < INV_MPU6050_MIN_FIFO_RATE ||
555 fifo_rate > INV_MPU6050_MAX_FIFO_RATE)
556 return -EINVAL;
557 if (fifo_rate == st->chip_config.fifo_rate)
558 return count;
559
560 mutex_lock(&indio_dev->mlock);
561 if (st->chip_config.enable) {
562 result = -EBUSY;
563 goto fifo_rate_fail;
564 }
565 result = inv_mpu6050_set_power_itg(st, true);
566 if (result)
567 goto fifo_rate_fail;
568
569 d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
570 result = regmap_write(st->map, st->reg->sample_rate_div, d);
571 if (result)
572 goto fifo_rate_fail;
573 st->chip_config.fifo_rate = fifo_rate;
574
575 result = inv_mpu6050_set_lpf(st, fifo_rate);
576 if (result)
577 goto fifo_rate_fail;
578
579fifo_rate_fail:
580 result |= inv_mpu6050_set_power_itg(st, false);
581 mutex_unlock(&indio_dev->mlock);
582 if (result)
583 return result;
584
585 return count;
586}
587
588
589
590
591static ssize_t
592inv_fifo_rate_show(struct device *dev, struct device_attribute *attr,
593 char *buf)
594{
595 struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
596
597 return sprintf(buf, "%d\n", st->chip_config.fifo_rate);
598}
599
600
601
602
603
604static ssize_t inv_attr_show(struct device *dev, struct device_attribute *attr,
605 char *buf)
606{
607 struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
608 struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
609 s8 *m;
610
611 switch (this_attr->address) {
612
613
614
615
616 case ATTR_GYRO_MATRIX:
617 case ATTR_ACCL_MATRIX:
618 m = st->plat_data.orientation;
619
620 return sprintf(buf, "%d, %d, %d; %d, %d, %d; %d, %d, %d\n",
621 m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
622 default:
623 return -EINVAL;
624 }
625}
626
627
628
629
630
631
632
633
634
635
636static int inv_mpu6050_validate_trigger(struct iio_dev *indio_dev,
637 struct iio_trigger *trig)
638{
639 struct inv_mpu6050_state *st = iio_priv(indio_dev);
640
641 if (st->trig != trig)
642 return -EINVAL;
643
644 return 0;
645}
646
647#define INV_MPU6050_CHAN(_type, _channel2, _index) \
648 { \
649 .type = _type, \
650 .modified = 1, \
651 .channel2 = _channel2, \
652 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
653 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
654 BIT(IIO_CHAN_INFO_CALIBBIAS), \
655 .scan_index = _index, \
656 .scan_type = { \
657 .sign = 's', \
658 .realbits = 16, \
659 .storagebits = 16, \
660 .shift = 0, \
661 .endianness = IIO_BE, \
662 }, \
663 }
664
665static const struct iio_chan_spec inv_mpu_channels[] = {
666 IIO_CHAN_SOFT_TIMESTAMP(INV_MPU6050_SCAN_TIMESTAMP),
667
668
669
670
671 {
672 .type = IIO_TEMP,
673 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW)
674 | BIT(IIO_CHAN_INFO_OFFSET)
675 | BIT(IIO_CHAN_INFO_SCALE),
676 .scan_index = -1,
677 },
678 INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X),
679 INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y),
680 INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z),
681
682 INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X),
683 INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y),
684 INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z),
685};
686
687
688static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500");
689static IIO_CONST_ATTR(in_anglvel_scale_available,
690 "0.000133090 0.000266181 0.000532362 0.001064724");
691static IIO_CONST_ATTR(in_accel_scale_available,
692 "0.000598 0.001196 0.002392 0.004785");
693static IIO_DEV_ATTR_SAMP_FREQ(S_IRUGO | S_IWUSR, inv_fifo_rate_show,
694 inv_mpu6050_fifo_rate_store);
695static IIO_DEVICE_ATTR(in_gyro_matrix, S_IRUGO, inv_attr_show, NULL,
696 ATTR_GYRO_MATRIX);
697static IIO_DEVICE_ATTR(in_accel_matrix, S_IRUGO, inv_attr_show, NULL,
698 ATTR_ACCL_MATRIX);
699
700static struct attribute *inv_attributes[] = {
701 &iio_dev_attr_in_gyro_matrix.dev_attr.attr,
702 &iio_dev_attr_in_accel_matrix.dev_attr.attr,
703 &iio_dev_attr_sampling_frequency.dev_attr.attr,
704 &iio_const_attr_sampling_frequency_available.dev_attr.attr,
705 &iio_const_attr_in_accel_scale_available.dev_attr.attr,
706 &iio_const_attr_in_anglvel_scale_available.dev_attr.attr,
707 NULL,
708};
709
710static const struct attribute_group inv_attribute_group = {
711 .attrs = inv_attributes
712};
713
714static const struct iio_info mpu_info = {
715 .driver_module = THIS_MODULE,
716 .read_raw = &inv_mpu6050_read_raw,
717 .write_raw = &inv_mpu6050_write_raw,
718 .write_raw_get_fmt = &inv_write_raw_get_fmt,
719 .attrs = &inv_attribute_group,
720 .validate_trigger = inv_mpu6050_validate_trigger,
721};
722
723
724
725
726static int inv_check_and_setup_chip(struct inv_mpu6050_state *st)
727{
728 int result;
729
730 st->hw = &hw_info[st->chip_type];
731 st->reg = hw_info[st->chip_type].reg;
732
733
734 result = regmap_write(st->map, st->reg->pwr_mgmt_1,
735 INV_MPU6050_BIT_H_RESET);
736 if (result)
737 return result;
738 msleep(INV_MPU6050_POWER_UP_TIME);
739
740
741
742
743
744
745 result = inv_mpu6050_set_power_itg(st, false);
746 if (result)
747 return result;
748 result = inv_mpu6050_set_power_itg(st, true);
749 if (result)
750 return result;
751
752 result = inv_mpu6050_switch_engine(st, false,
753 INV_MPU6050_BIT_PWR_ACCL_STBY);
754 if (result)
755 return result;
756 result = inv_mpu6050_switch_engine(st, false,
757 INV_MPU6050_BIT_PWR_GYRO_STBY);
758 if (result)
759 return result;
760
761 return 0;
762}
763
764int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
765 int (*inv_mpu_bus_setup)(struct iio_dev *), int chip_type)
766{
767 struct inv_mpu6050_state *st;
768 struct iio_dev *indio_dev;
769 struct inv_mpu6050_platform_data *pdata;
770 struct device *dev = regmap_get_device(regmap);
771 int result;
772
773 indio_dev = devm_iio_device_alloc(dev, sizeof(*st));
774 if (!indio_dev)
775 return -ENOMEM;
776
777 st = iio_priv(indio_dev);
778 st->chip_type = chip_type;
779 st->powerup_count = 0;
780 st->irq = irq;
781 st->map = regmap;
782 pdata = dev_get_platdata(dev);
783 if (pdata)
784 st->plat_data = *pdata;
785
786 result = inv_check_and_setup_chip(st);
787 if (result)
788 return result;
789
790 if (inv_mpu_bus_setup)
791 inv_mpu_bus_setup(indio_dev);
792
793 result = inv_mpu6050_init_config(indio_dev);
794 if (result) {
795 dev_err(dev, "Could not initialize device.\n");
796 return result;
797 }
798
799 dev_set_drvdata(dev, indio_dev);
800 indio_dev->dev.parent = dev;
801
802 if (name)
803 indio_dev->name = name;
804 else
805 indio_dev->name = dev_name(dev);
806 indio_dev->channels = inv_mpu_channels;
807 indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels);
808
809 indio_dev->info = &mpu_info;
810 indio_dev->modes = INDIO_BUFFER_TRIGGERED;
811
812 result = iio_triggered_buffer_setup(indio_dev,
813 inv_mpu6050_irq_handler,
814 inv_mpu6050_read_fifo,
815 NULL);
816 if (result) {
817 dev_err(dev, "configure buffer fail %d\n", result);
818 return result;
819 }
820 result = inv_mpu6050_probe_trigger(indio_dev);
821 if (result) {
822 dev_err(dev, "trigger probe fail %d\n", result);
823 goto out_unreg_ring;
824 }
825
826 INIT_KFIFO(st->timestamps);
827 spin_lock_init(&st->time_stamp_lock);
828 result = iio_device_register(indio_dev);
829 if (result) {
830 dev_err(dev, "IIO register fail %d\n", result);
831 goto out_remove_trigger;
832 }
833
834 return 0;
835
836out_remove_trigger:
837 inv_mpu6050_remove_trigger(st);
838out_unreg_ring:
839 iio_triggered_buffer_cleanup(indio_dev);
840 return result;
841}
842EXPORT_SYMBOL_GPL(inv_mpu_core_probe);
843
844int inv_mpu_core_remove(struct device *dev)
845{
846 struct iio_dev *indio_dev = dev_get_drvdata(dev);
847
848 iio_device_unregister(indio_dev);
849 inv_mpu6050_remove_trigger(iio_priv(indio_dev));
850 iio_triggered_buffer_cleanup(indio_dev);
851
852 return 0;
853}
854EXPORT_SYMBOL_GPL(inv_mpu_core_remove);
855
856#ifdef CONFIG_PM_SLEEP
857
858static int inv_mpu_resume(struct device *dev)
859{
860 return inv_mpu6050_set_power_itg(iio_priv(dev_get_drvdata(dev)), true);
861}
862
863static int inv_mpu_suspend(struct device *dev)
864{
865 return inv_mpu6050_set_power_itg(iio_priv(dev_get_drvdata(dev)), false);
866}
867#endif
868
869SIMPLE_DEV_PM_OPS(inv_mpu_pmops, inv_mpu_suspend, inv_mpu_resume);
870EXPORT_SYMBOL_GPL(inv_mpu_pmops);
871
872MODULE_AUTHOR("Invensense Corporation");
873MODULE_DESCRIPTION("Invensense device MPU6050 driver");
874MODULE_LICENSE("GPL");
875
