2 * Copyright (C) 2012 Invensense, Inc.
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.
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.
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"
31 * this is the gyro scale translated from dynamic range plus/minus
32 * {250, 500, 1000, 2000} to rad/s
34 static const int gyro_scale_6050
[] = {133090, 266181, 532362, 1064724};
37 * this is the accel scale translated from dynamic range plus/minus
38 * {2, 4, 8, 16} to m/s^2
40 static const int accel_scale
[] = {598, 1196, 2392, 4785};
42 static 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
,
62 static 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
,
82 static 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
,
91 static const struct inv_mpu6050_hw hw_info
[] = {
96 .config
= &chip_config_6050
,
101 .reg
= ®_set_6050
,
102 .config
= &chip_config_6050
,
106 int inv_mpu6050_switch_engine(struct inv_mpu6050_state
*st
, bool en
, u32 mask
)
108 unsigned int d
, mgmt_1
;
111 * switch clock needs to be careful. Only when gyro is on, can
112 * clock source be switched to gyro. Otherwise, it must be set to
115 if (mask
== INV_MPU6050_BIT_PWR_GYRO_STBY
) {
116 result
= regmap_read(st
->map
, st
->reg
->pwr_mgmt_1
, &mgmt_1
);
120 mgmt_1
&= ~INV_MPU6050_BIT_CLK_MASK
;
123 if ((mask
== INV_MPU6050_BIT_PWR_GYRO_STBY
) && (!en
)) {
125 * turning off gyro requires switch to internal clock first.
126 * Then turn off gyro engine
128 mgmt_1
|= INV_CLK_INTERNAL
;
129 result
= regmap_write(st
->map
, st
->reg
->pwr_mgmt_1
, mgmt_1
);
134 result
= regmap_read(st
->map
, st
->reg
->pwr_mgmt_2
, &d
);
141 result
= regmap_write(st
->map
, st
->reg
->pwr_mgmt_2
, d
);
146 /* Wait for output stabilize */
147 msleep(INV_MPU6050_TEMP_UP_TIME
);
148 if (mask
== INV_MPU6050_BIT_PWR_GYRO_STBY
) {
149 /* switch internal clock to PLL */
150 mgmt_1
|= INV_CLK_PLL
;
151 result
= regmap_write(st
->map
,
152 st
->reg
->pwr_mgmt_1
, mgmt_1
);
161 int inv_mpu6050_set_power_itg(struct inv_mpu6050_state
*st
, bool power_on
)
166 /* Already under indio-dev->mlock mutex */
167 if (!st
->powerup_count
)
168 result
= regmap_write(st
->map
, st
->reg
->pwr_mgmt_1
, 0);
173 if (!st
->powerup_count
)
174 result
= regmap_write(st
->map
, st
->reg
->pwr_mgmt_1
,
175 INV_MPU6050_BIT_SLEEP
);
182 usleep_range(INV_MPU6050_REG_UP_TIME_MIN
,
183 INV_MPU6050_REG_UP_TIME_MAX
);
187 EXPORT_SYMBOL_GPL(inv_mpu6050_set_power_itg
);
190 * inv_mpu6050_init_config() - Initialize hardware, disable FIFO.
192 * Initial configuration:
196 * Clock source: Gyro PLL
198 static int inv_mpu6050_init_config(struct iio_dev
*indio_dev
)
202 struct inv_mpu6050_state
*st
= iio_priv(indio_dev
);
204 result
= inv_mpu6050_set_power_itg(st
, true);
207 d
= (INV_MPU6050_FSR_2000DPS
<< INV_MPU6050_GYRO_CONFIG_FSR_SHIFT
);
208 result
= regmap_write(st
->map
, st
->reg
->gyro_config
, d
);
212 d
= INV_MPU6050_FILTER_20HZ
;
213 result
= regmap_write(st
->map
, st
->reg
->lpf
, d
);
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
);
222 d
= (INV_MPU6050_FS_02G
<< INV_MPU6050_ACCL_CONFIG_FSR_SHIFT
);
223 result
= regmap_write(st
->map
, st
->reg
->accl_config
, d
);
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);
234 static int inv_mpu6050_sensor_set(struct inv_mpu6050_state
*st
, int reg
,
238 __be16 d
= cpu_to_be16(val
);
240 ind
= (axis
- IIO_MOD_X
) * 2;
241 result
= regmap_bulk_write(st
->map
, reg
+ ind
, (u8
*)&d
, 2);
248 static int inv_mpu6050_sensor_show(struct inv_mpu6050_state
*st
, int reg
,
254 ind
= (axis
- IIO_MOD_X
) * 2;
255 result
= regmap_bulk_read(st
->map
, reg
+ ind
, (u8
*)&d
, 2);
258 *val
= (short)be16_to_cpup(&d
);
264 inv_mpu6050_read_raw(struct iio_dev
*indio_dev
,
265 struct iio_chan_spec
const *chan
,
266 int *val
, int *val2
, long mask
)
268 struct inv_mpu6050_state
*st
= iio_priv(indio_dev
);
272 case IIO_CHAN_INFO_RAW
:
278 mutex_lock(&indio_dev
->mlock
);
279 if (!st
->chip_config
.enable
) {
280 result
= inv_mpu6050_set_power_itg(st
, true);
284 /* when enable is on, power is already on */
285 switch (chan
->type
) {
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
);
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
);
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
);
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
);
323 /* wait for stablization */
324 msleep(INV_MPU6050_SENSOR_UP_TIME
);
325 ret
= inv_mpu6050_sensor_show(st
, st
->reg
->temperature
,
333 if (!st
->chip_config
.enable
)
334 result
|= inv_mpu6050_set_power_itg(st
, false);
335 mutex_unlock(&indio_dev
->mlock
);
341 case IIO_CHAN_INFO_SCALE
:
342 switch (chan
->type
) {
345 *val2
= gyro_scale_6050
[st
->chip_config
.fsr
];
347 return IIO_VAL_INT_PLUS_NANO
;
350 *val2
= accel_scale
[st
->chip_config
.accl_fs
];
352 return IIO_VAL_INT_PLUS_MICRO
;
355 *val2
= INV_MPU6050_TEMP_SCALE
;
357 return IIO_VAL_INT_PLUS_MICRO
;
361 case IIO_CHAN_INFO_OFFSET
:
362 switch (chan
->type
) {
364 *val
= INV_MPU6050_TEMP_OFFSET
;
370 case IIO_CHAN_INFO_CALIBBIAS
:
371 switch (chan
->type
) {
373 ret
= inv_mpu6050_sensor_show(st
, st
->reg
->gyro_offset
,
374 chan
->channel2
, val
);
377 ret
= inv_mpu6050_sensor_show(st
, st
->reg
->accl_offset
,
378 chan
->channel2
, val
);
389 static int inv_mpu6050_write_gyro_scale(struct inv_mpu6050_state
*st
, int val
)
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
);
401 st
->chip_config
.fsr
= i
;
409 static int inv_write_raw_get_fmt(struct iio_dev
*indio_dev
,
410 struct iio_chan_spec
const *chan
, long mask
)
413 case IIO_CHAN_INFO_SCALE
:
414 switch (chan
->type
) {
416 return IIO_VAL_INT_PLUS_NANO
;
418 return IIO_VAL_INT_PLUS_MICRO
;
421 return IIO_VAL_INT_PLUS_MICRO
;
427 static int inv_mpu6050_write_accel_scale(struct inv_mpu6050_state
*st
, int val
)
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
);
439 st
->chip_config
.accl_fs
= i
;
447 static int inv_mpu6050_write_raw(struct iio_dev
*indio_dev
,
448 struct iio_chan_spec
const *chan
,
449 int val
, int val2
, long mask
)
451 struct inv_mpu6050_state
*st
= iio_priv(indio_dev
);
454 mutex_lock(&indio_dev
->mlock
);
456 * we should only update scale when the chip is disabled, i.e.
459 if (st
->chip_config
.enable
) {
461 goto error_write_raw
;
463 result
= inv_mpu6050_set_power_itg(st
, true);
465 goto error_write_raw
;
468 case IIO_CHAN_INFO_SCALE
:
469 switch (chan
->type
) {
471 result
= inv_mpu6050_write_gyro_scale(st
, val2
);
474 result
= inv_mpu6050_write_accel_scale(st
, val2
);
481 case IIO_CHAN_INFO_CALIBBIAS
:
482 switch (chan
->type
) {
484 result
= inv_mpu6050_sensor_set(st
,
485 st
->reg
->gyro_offset
,
486 chan
->channel2
, val
);
489 result
= inv_mpu6050_sensor_set(st
,
490 st
->reg
->accl_offset
,
491 chan
->channel2
, val
);
502 result
|= inv_mpu6050_set_power_itg(st
, false);
503 mutex_unlock(&indio_dev
->mlock
);
509 * inv_mpu6050_set_lpf() - set low pass filer based on fifo rate.
511 * Based on the Nyquist principle, the sampling rate must
512 * exceed twice of the bandwidth of the signal, or there
513 * would be alising. This function basically search for the
514 * correct low pass parameters based on the fifo rate, e.g,
515 * sampling frequency.
517 static int inv_mpu6050_set_lpf(struct inv_mpu6050_state
*st
, int rate
)
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
};
528 while ((h
< hz
[i
]) && (i
< ARRAY_SIZE(d
) - 1))
531 result
= regmap_write(st
->map
, st
->reg
->lpf
, data
);
534 st
->chip_config
.lpf
= data
;
540 * inv_mpu6050_fifo_rate_store() - Set fifo rate.
543 inv_mpu6050_fifo_rate_store(struct device
*dev
, struct device_attribute
*attr
,
544 const char *buf
, size_t count
)
549 struct iio_dev
*indio_dev
= dev_to_iio_dev(dev
);
550 struct inv_mpu6050_state
*st
= iio_priv(indio_dev
);
552 if (kstrtoint(buf
, 10, &fifo_rate
))
554 if (fifo_rate
< INV_MPU6050_MIN_FIFO_RATE
||
555 fifo_rate
> INV_MPU6050_MAX_FIFO_RATE
)
557 if (fifo_rate
== st
->chip_config
.fifo_rate
)
560 mutex_lock(&indio_dev
->mlock
);
561 if (st
->chip_config
.enable
) {
565 result
= inv_mpu6050_set_power_itg(st
, true);
569 d
= INV_MPU6050_ONE_K_HZ
/ fifo_rate
- 1;
570 result
= regmap_write(st
->map
, st
->reg
->sample_rate_div
, d
);
573 st
->chip_config
.fifo_rate
= fifo_rate
;
575 result
= inv_mpu6050_set_lpf(st
, fifo_rate
);
580 result
|= inv_mpu6050_set_power_itg(st
, false);
581 mutex_unlock(&indio_dev
->mlock
);
589 * inv_fifo_rate_show() - Get the current sampling rate.
592 inv_fifo_rate_show(struct device
*dev
, struct device_attribute
*attr
,
595 struct inv_mpu6050_state
*st
= iio_priv(dev_to_iio_dev(dev
));
597 return sprintf(buf
, "%d\n", st
->chip_config
.fifo_rate
);
601 * inv_attr_show() - calling this function will show current
604 static ssize_t
inv_attr_show(struct device
*dev
, struct device_attribute
*attr
,
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
);
611 switch (this_attr
->address
) {
613 * In MPU6050, the two matrix are the same because gyro and accel
614 * are integrated in one chip
616 case ATTR_GYRO_MATRIX
:
617 case ATTR_ACCL_MATRIX
:
618 m
= st
->plat_data
.orientation
;
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]);
628 * inv_mpu6050_validate_trigger() - validate_trigger callback for invensense
630 * @indio_dev: The IIO device
631 * @trig: The new trigger
633 * Returns: 0 if the 'trig' matches the trigger registered by the MPU6050
634 * device, -EINVAL otherwise.
636 static int inv_mpu6050_validate_trigger(struct iio_dev
*indio_dev
,
637 struct iio_trigger
*trig
)
639 struct inv_mpu6050_state
*st
= iio_priv(indio_dev
);
641 if (st
->trig
!= trig
)
647 #define INV_MPU6050_CHAN(_type, _channel2, _index) \
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, \
661 .endianness = IIO_BE, \
665 static const struct iio_chan_spec inv_mpu_channels
[] = {
666 IIO_CHAN_SOFT_TIMESTAMP(INV_MPU6050_SCAN_TIMESTAMP
),
668 * Note that temperature should only be via polled reading only,
669 * not the final scan elements output.
673 .info_mask_separate
= BIT(IIO_CHAN_INFO_RAW
)
674 | BIT(IIO_CHAN_INFO_OFFSET
)
675 | BIT(IIO_CHAN_INFO_SCALE
),
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
),
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
),
687 /* constant IIO attribute */
688 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500");
689 static IIO_CONST_ATTR(in_anglvel_scale_available
,
690 "0.000133090 0.000266181 0.000532362 0.001064724");
691 static IIO_CONST_ATTR(in_accel_scale_available
,
692 "0.000598 0.001196 0.002392 0.004785");
693 static IIO_DEV_ATTR_SAMP_FREQ(S_IRUGO
| S_IWUSR
, inv_fifo_rate_show
,
694 inv_mpu6050_fifo_rate_store
);
695 static IIO_DEVICE_ATTR(in_gyro_matrix
, S_IRUGO
, inv_attr_show
, NULL
,
697 static IIO_DEVICE_ATTR(in_accel_matrix
, S_IRUGO
, inv_attr_show
, NULL
,
700 static 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
,
710 static const struct attribute_group inv_attribute_group
= {
711 .attrs
= inv_attributes
714 static 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
,
724 * inv_check_and_setup_chip() - check and setup chip.
726 static int inv_check_and_setup_chip(struct inv_mpu6050_state
*st
)
730 st
->hw
= &hw_info
[st
->chip_type
];
731 st
->reg
= hw_info
[st
->chip_type
].reg
;
733 /* reset to make sure previous state are not there */
734 result
= regmap_write(st
->map
, st
->reg
->pwr_mgmt_1
,
735 INV_MPU6050_BIT_H_RESET
);
738 msleep(INV_MPU6050_POWER_UP_TIME
);
740 * toggle power state. After reset, the sleep bit could be on
741 * or off depending on the OTP settings. Toggling power would
742 * make it in a definite state as well as making the hardware
743 * state align with the software state
745 result
= inv_mpu6050_set_power_itg(st
, false);
748 result
= inv_mpu6050_set_power_itg(st
, true);
752 result
= inv_mpu6050_switch_engine(st
, false,
753 INV_MPU6050_BIT_PWR_ACCL_STBY
);
756 result
= inv_mpu6050_switch_engine(st
, false,
757 INV_MPU6050_BIT_PWR_GYRO_STBY
);
764 int inv_mpu_core_probe(struct regmap
*regmap
, int irq
, const char *name
,
765 int (*inv_mpu_bus_setup
)(struct iio_dev
*), int chip_type
)
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
);
773 indio_dev
= devm_iio_device_alloc(dev
, sizeof(*st
));
777 st
= iio_priv(indio_dev
);
778 st
->chip_type
= chip_type
;
779 st
->powerup_count
= 0;
782 pdata
= dev_get_platdata(dev
);
784 st
->plat_data
= *pdata
;
785 /* power is turned on inside check chip type*/
786 result
= inv_check_and_setup_chip(st
);
790 if (inv_mpu_bus_setup
)
791 inv_mpu_bus_setup(indio_dev
);
793 result
= inv_mpu6050_init_config(indio_dev
);
795 dev_err(dev
, "Could not initialize device.\n");
799 dev_set_drvdata(dev
, indio_dev
);
800 indio_dev
->dev
.parent
= dev
;
801 /* name will be NULL when enumerated via ACPI */
803 indio_dev
->name
= name
;
805 indio_dev
->name
= dev_name(dev
);
806 indio_dev
->channels
= inv_mpu_channels
;
807 indio_dev
->num_channels
= ARRAY_SIZE(inv_mpu_channels
);
809 indio_dev
->info
= &mpu_info
;
810 indio_dev
->modes
= INDIO_BUFFER_TRIGGERED
;
812 result
= iio_triggered_buffer_setup(indio_dev
,
813 inv_mpu6050_irq_handler
,
814 inv_mpu6050_read_fifo
,
817 dev_err(dev
, "configure buffer fail %d\n", result
);
820 result
= inv_mpu6050_probe_trigger(indio_dev
);
822 dev_err(dev
, "trigger probe fail %d\n", result
);
826 INIT_KFIFO(st
->timestamps
);
827 spin_lock_init(&st
->time_stamp_lock
);
828 result
= iio_device_register(indio_dev
);
830 dev_err(dev
, "IIO register fail %d\n", result
);
831 goto out_remove_trigger
;
837 inv_mpu6050_remove_trigger(st
);
839 iio_triggered_buffer_cleanup(indio_dev
);
842 EXPORT_SYMBOL_GPL(inv_mpu_core_probe
);
844 int inv_mpu_core_remove(struct device
*dev
)
846 struct iio_dev
*indio_dev
= dev_get_drvdata(dev
);
848 iio_device_unregister(indio_dev
);
849 inv_mpu6050_remove_trigger(iio_priv(indio_dev
));
850 iio_triggered_buffer_cleanup(indio_dev
);
854 EXPORT_SYMBOL_GPL(inv_mpu_core_remove
);
856 #ifdef CONFIG_PM_SLEEP
858 static int inv_mpu_resume(struct device
*dev
)
860 return inv_mpu6050_set_power_itg(iio_priv(dev_get_drvdata(dev
)), true);
863 static int inv_mpu_suspend(struct device
*dev
)
865 return inv_mpu6050_set_power_itg(iio_priv(dev_get_drvdata(dev
)), false);
867 #endif /* CONFIG_PM_SLEEP */
869 SIMPLE_DEV_PM_OPS(inv_mpu_pmops
, inv_mpu_suspend
, inv_mpu_resume
);
870 EXPORT_SYMBOL_GPL(inv_mpu_pmops
);
872 MODULE_AUTHOR("Invensense Corporation");
873 MODULE_DESCRIPTION("Invensense device MPU6050 driver");
874 MODULE_LICENSE("GPL");