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