1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Copyright (C) 2012 Invensense, Inc.
4 */
5
6 #include <linux/pm_runtime.h>
7 #include "inv_mpu_iio.h"
8
inv_scan_query_mpu6050(struct iio_dev * indio_dev)9 static unsigned int inv_scan_query_mpu6050(struct iio_dev *indio_dev)
10 {
11 struct inv_mpu6050_state *st = iio_priv(indio_dev);
12 unsigned int mask;
13
14 /*
15 * If the MPU6050 is just used as a trigger, then the scan mask
16 * is not allocated so we simply enable the temperature channel
17 * as a dummy and bail out.
18 */
19 if (!indio_dev->active_scan_mask) {
20 st->chip_config.temp_fifo_enable = true;
21 return INV_MPU6050_SENSOR_TEMP;
22 }
23
24 st->chip_config.gyro_fifo_enable =
25 test_bit(INV_MPU6050_SCAN_GYRO_X,
26 indio_dev->active_scan_mask) ||
27 test_bit(INV_MPU6050_SCAN_GYRO_Y,
28 indio_dev->active_scan_mask) ||
29 test_bit(INV_MPU6050_SCAN_GYRO_Z,
30 indio_dev->active_scan_mask);
31
32 st->chip_config.accl_fifo_enable =
33 test_bit(INV_MPU6050_SCAN_ACCL_X,
34 indio_dev->active_scan_mask) ||
35 test_bit(INV_MPU6050_SCAN_ACCL_Y,
36 indio_dev->active_scan_mask) ||
37 test_bit(INV_MPU6050_SCAN_ACCL_Z,
38 indio_dev->active_scan_mask);
39
40 st->chip_config.temp_fifo_enable =
41 test_bit(INV_MPU6050_SCAN_TEMP, indio_dev->active_scan_mask);
42
43 mask = 0;
44 if (st->chip_config.gyro_fifo_enable)
45 mask |= INV_MPU6050_SENSOR_GYRO;
46 if (st->chip_config.accl_fifo_enable)
47 mask |= INV_MPU6050_SENSOR_ACCL;
48 if (st->chip_config.temp_fifo_enable)
49 mask |= INV_MPU6050_SENSOR_TEMP;
50
51 return mask;
52 }
53
inv_scan_query_mpu9x50(struct iio_dev * indio_dev)54 static unsigned int inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
55 {
56 struct inv_mpu6050_state *st = iio_priv(indio_dev);
57 unsigned int mask;
58
59 mask = inv_scan_query_mpu6050(indio_dev);
60
61 /* no magnetometer if i2c auxiliary bus is used */
62 if (st->magn_disabled)
63 return mask;
64
65 st->chip_config.magn_fifo_enable =
66 test_bit(INV_MPU9X50_SCAN_MAGN_X,
67 indio_dev->active_scan_mask) ||
68 test_bit(INV_MPU9X50_SCAN_MAGN_Y,
69 indio_dev->active_scan_mask) ||
70 test_bit(INV_MPU9X50_SCAN_MAGN_Z,
71 indio_dev->active_scan_mask);
72 if (st->chip_config.magn_fifo_enable)
73 mask |= INV_MPU6050_SENSOR_MAGN;
74
75 return mask;
76 }
77
inv_scan_query(struct iio_dev * indio_dev)78 static unsigned int inv_scan_query(struct iio_dev *indio_dev)
79 {
80 struct inv_mpu6050_state *st = iio_priv(indio_dev);
81
82 switch (st->chip_type) {
83 case INV_MPU9150:
84 case INV_MPU9250:
85 case INV_MPU9255:
86 return inv_scan_query_mpu9x50(indio_dev);
87 default:
88 return inv_scan_query_mpu6050(indio_dev);
89 }
90 }
91
inv_compute_skip_samples(const struct inv_mpu6050_state * st)92 static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
93 {
94 unsigned int gyro_skip = 0;
95 unsigned int magn_skip = 0;
96 unsigned int skip_samples;
97
98 /* gyro first sample is out of specs, skip it */
99 if (st->chip_config.gyro_fifo_enable)
100 gyro_skip = 1;
101
102 /* mag first sample is always not ready, skip it */
103 if (st->chip_config.magn_fifo_enable)
104 magn_skip = 1;
105
106 /* compute first samples to skip */
107 skip_samples = gyro_skip;
108 if (magn_skip > skip_samples)
109 skip_samples = magn_skip;
110
111 return skip_samples;
112 }
113
inv_mpu6050_prepare_fifo(struct inv_mpu6050_state * st,bool enable)114 int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable)
115 {
116 uint8_t d;
117 int ret;
118
119 if (enable) {
120 st->it_timestamp = 0;
121 /* reset FIFO */
122 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
123 ret = regmap_write(st->map, st->reg->user_ctrl, d);
124 if (ret)
125 return ret;
126 /* enable sensor output to FIFO */
127 d = 0;
128 if (st->chip_config.gyro_fifo_enable)
129 d |= INV_MPU6050_BITS_GYRO_OUT;
130 if (st->chip_config.accl_fifo_enable)
131 d |= INV_MPU6050_BIT_ACCEL_OUT;
132 if (st->chip_config.temp_fifo_enable)
133 d |= INV_MPU6050_BIT_TEMP_OUT;
134 if (st->chip_config.magn_fifo_enable)
135 d |= INV_MPU6050_BIT_SLAVE_0;
136 ret = regmap_write(st->map, st->reg->fifo_en, d);
137 if (ret)
138 return ret;
139 /* enable FIFO reading */
140 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
141 ret = regmap_write(st->map, st->reg->user_ctrl, d);
142 if (ret)
143 return ret;
144 /* enable interrupt */
145 ret = regmap_write(st->map, st->reg->int_enable,
146 INV_MPU6050_BIT_DATA_RDY_EN);
147 } else {
148 ret = regmap_write(st->map, st->reg->int_enable, 0);
149 if (ret)
150 return ret;
151 ret = regmap_write(st->map, st->reg->fifo_en, 0);
152 if (ret)
153 return ret;
154 /* restore user_ctrl for disabling FIFO reading */
155 ret = regmap_write(st->map, st->reg->user_ctrl,
156 st->chip_config.user_ctrl);
157 }
158
159 return ret;
160 }
161
162 /**
163 * inv_mpu6050_set_enable() - enable chip functions.
164 * @indio_dev: Device driver instance.
165 * @enable: enable/disable
166 */
inv_mpu6050_set_enable(struct iio_dev * indio_dev,bool enable)167 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
168 {
169 struct inv_mpu6050_state *st = iio_priv(indio_dev);
170 struct device *pdev = regmap_get_device(st->map);
171 unsigned int scan;
172 int result;
173
174 if (enable) {
175 scan = inv_scan_query(indio_dev);
176 result = pm_runtime_get_sync(pdev);
177 if (result < 0) {
178 pm_runtime_put_noidle(pdev);
179 return result;
180 }
181 /*
182 * In case autosuspend didn't trigger, turn off first not
183 * required sensors.
184 */
185 result = inv_mpu6050_switch_engine(st, false, ~scan);
186 if (result)
187 goto error_power_off;
188 result = inv_mpu6050_switch_engine(st, true, scan);
189 if (result)
190 goto error_power_off;
191 st->skip_samples = inv_compute_skip_samples(st);
192 result = inv_mpu6050_prepare_fifo(st, true);
193 if (result)
194 goto error_power_off;
195 } else {
196 result = inv_mpu6050_prepare_fifo(st, false);
197 if (result)
198 goto error_power_off;
199 pm_runtime_mark_last_busy(pdev);
200 pm_runtime_put_autosuspend(pdev);
201 }
202
203 return 0;
204
205 error_power_off:
206 pm_runtime_put_autosuspend(pdev);
207 return result;
208 }
209
210 /**
211 * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state
212 * @trig: Trigger instance
213 * @state: Desired trigger state
214 */
inv_mpu_data_rdy_trigger_set_state(struct iio_trigger * trig,bool state)215 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig,
216 bool state)
217 {
218 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
219 struct inv_mpu6050_state *st = iio_priv(indio_dev);
220 int result;
221
222 mutex_lock(&st->lock);
223 result = inv_mpu6050_set_enable(indio_dev, state);
224 mutex_unlock(&st->lock);
225
226 return result;
227 }
228
229 static const struct iio_trigger_ops inv_mpu_trigger_ops = {
230 .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state,
231 };
232
inv_mpu6050_probe_trigger(struct iio_dev * indio_dev,int irq_type)233 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type)
234 {
235 int ret;
236 struct inv_mpu6050_state *st = iio_priv(indio_dev);
237
238 st->trig = devm_iio_trigger_alloc(&indio_dev->dev,
239 "%s-dev%d",
240 indio_dev->name,
241 indio_dev->id);
242 if (!st->trig)
243 return -ENOMEM;
244
245 ret = devm_request_irq(&indio_dev->dev, st->irq,
246 &iio_trigger_generic_data_rdy_poll,
247 irq_type,
248 "inv_mpu",
249 st->trig);
250 if (ret)
251 return ret;
252
253 st->trig->dev.parent = regmap_get_device(st->map);
254 st->trig->ops = &inv_mpu_trigger_ops;
255 iio_trigger_set_drvdata(st->trig, indio_dev);
256
257 ret = devm_iio_trigger_register(&indio_dev->dev, st->trig);
258 if (ret)
259 return ret;
260
261 indio_dev->trig = iio_trigger_get(st->trig);
262
263 return 0;
264 }
265