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
inv_scan_query(struct iio_dev * indio_dev)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 */
inv_mpu6050_set_enable(struct iio_dev * indio_dev,bool enable)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 st->skip_samples = 0;
53 if (st->chip_config.gyro_fifo_enable) {
54 result = inv_mpu6050_switch_engine(st, true,
55 INV_MPU6050_BIT_PWR_GYRO_STBY);
56 if (result)
57 goto error_power_off;
58 /* gyro first sample is out of specs, skip it */
59 st->skip_samples = 1;
60 }
61 if (st->chip_config.accl_fifo_enable) {
62 result = inv_mpu6050_switch_engine(st, true,
63 INV_MPU6050_BIT_PWR_ACCL_STBY);
64 if (result)
65 goto error_gyro_off;
66 }
67 result = inv_reset_fifo(indio_dev);
68 if (result)
69 goto error_accl_off;
70 } else {
71 result = regmap_write(st->map, st->reg->fifo_en, 0);
72 if (result)
73 goto error_accl_off;
74
75 result = regmap_write(st->map, st->reg->int_enable, 0);
76 if (result)
77 goto error_accl_off;
78
79 result = regmap_write(st->map, st->reg->user_ctrl,
80 st->chip_config.user_ctrl);
81 if (result)
82 goto error_accl_off;
83
84 result = inv_mpu6050_switch_engine(st, false,
85 INV_MPU6050_BIT_PWR_ACCL_STBY);
86 if (result)
87 goto error_accl_off;
88
89 result = inv_mpu6050_switch_engine(st, false,
90 INV_MPU6050_BIT_PWR_GYRO_STBY);
91 if (result)
92 goto error_gyro_off;
93
94 result = inv_mpu6050_set_power_itg(st, false);
95 if (result)
96 goto error_power_off;
97 }
98
99 return 0;
100
101 error_accl_off:
102 if (st->chip_config.accl_fifo_enable)
103 inv_mpu6050_switch_engine(st, false,
104 INV_MPU6050_BIT_PWR_ACCL_STBY);
105 error_gyro_off:
106 if (st->chip_config.gyro_fifo_enable)
107 inv_mpu6050_switch_engine(st, false,
108 INV_MPU6050_BIT_PWR_GYRO_STBY);
109 error_power_off:
110 inv_mpu6050_set_power_itg(st, false);
111 return result;
112 }
113
114 /**
115 * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state
116 * @trig: Trigger instance
117 * @state: Desired trigger state
118 */
inv_mpu_data_rdy_trigger_set_state(struct iio_trigger * trig,bool state)119 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig,
120 bool state)
121 {
122 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
123 struct inv_mpu6050_state *st = iio_priv(indio_dev);
124 int result;
125
126 mutex_lock(&st->lock);
127 result = inv_mpu6050_set_enable(indio_dev, state);
128 mutex_unlock(&st->lock);
129
130 return result;
131 }
132
133 static const struct iio_trigger_ops inv_mpu_trigger_ops = {
134 .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state,
135 };
136
inv_mpu6050_probe_trigger(struct iio_dev * indio_dev,int irq_type)137 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type)
138 {
139 int ret;
140 struct inv_mpu6050_state *st = iio_priv(indio_dev);
141
142 st->trig = devm_iio_trigger_alloc(&indio_dev->dev,
143 "%s-dev%d",
144 indio_dev->name,
145 indio_dev->id);
146 if (!st->trig)
147 return -ENOMEM;
148
149 ret = devm_request_irq(&indio_dev->dev, st->irq,
150 &iio_trigger_generic_data_rdy_poll,
151 irq_type,
152 "inv_mpu",
153 st->trig);
154 if (ret)
155 return ret;
156
157 st->trig->dev.parent = regmap_get_device(st->map);
158 st->trig->ops = &inv_mpu_trigger_ops;
159 iio_trigger_set_drvdata(st->trig, indio_dev);
160
161 ret = devm_iio_trigger_register(&indio_dev->dev, st->trig);
162 if (ret)
163 return ret;
164
165 indio_dev->trig = iio_trigger_get(st->trig);
166
167 return 0;
168 }
169