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