1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Copyright (C) 2019 TDK-InvenSense, Inc. |
4 | */ |
5 | |
6 | #include <linux/kernel.h> |
7 | #include <linux/device.h> |
8 | #include <linux/string.h> |
9 | |
10 | #include "inv_mpu_aux.h" |
11 | #include "inv_mpu_iio.h" |
12 | #include "inv_mpu_magn.h" |
13 | |
14 | /* |
15 | * MPU9xxx magnetometer are AKM chips on I2C aux bus |
16 | * MPU9150 is AK8975 |
17 | * MPU9250 is AK8963 |
18 | */ |
19 | #define INV_MPU_MAGN_I2C_ADDR 0x0C |
20 | |
21 | #define INV_MPU_MAGN_REG_WIA 0x00 |
22 | #define INV_MPU_MAGN_BITS_WIA 0x48 |
23 | |
24 | #define INV_MPU_MAGN_REG_ST1 0x02 |
25 | #define INV_MPU_MAGN_BIT_DRDY 0x01 |
26 | #define INV_MPU_MAGN_BIT_DOR 0x02 |
27 | |
28 | #define INV_MPU_MAGN_REG_DATA 0x03 |
29 | |
30 | #define INV_MPU_MAGN_REG_ST2 0x09 |
31 | #define INV_MPU_MAGN_BIT_HOFL 0x08 |
32 | #define INV_MPU_MAGN_BIT_BITM 0x10 |
33 | |
34 | #define INV_MPU_MAGN_REG_CNTL1 0x0A |
35 | #define INV_MPU_MAGN_BITS_MODE_PWDN 0x00 |
36 | #define INV_MPU_MAGN_BITS_MODE_SINGLE 0x01 |
37 | #define INV_MPU_MAGN_BITS_MODE_FUSE 0x0F |
38 | #define INV_MPU9250_MAGN_BIT_OUTPUT_BIT 0x10 |
39 | |
40 | #define INV_MPU9250_MAGN_REG_CNTL2 0x0B |
41 | #define INV_MPU9250_MAGN_BIT_SRST 0x01 |
42 | |
43 | #define INV_MPU_MAGN_REG_ASAX 0x10 |
44 | #define INV_MPU_MAGN_REG_ASAY 0x11 |
45 | #define INV_MPU_MAGN_REG_ASAZ 0x12 |
46 | |
47 | static bool inv_magn_supported(const struct inv_mpu6050_state *st) |
48 | { |
49 | switch (st->chip_type) { |
50 | case INV_MPU9150: |
51 | case INV_MPU9250: |
52 | case INV_MPU9255: |
53 | return true; |
54 | default: |
55 | return false; |
56 | } |
57 | } |
58 | |
59 | /* init magnetometer chip */ |
60 | static int inv_magn_init(struct inv_mpu6050_state *st) |
61 | { |
62 | uint8_t val; |
63 | uint8_t asa[3]; |
64 | int32_t sensitivity; |
65 | int ret; |
66 | |
67 | /* check whoami */ |
68 | ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_WIA, |
69 | val: &val, size: sizeof(val)); |
70 | if (ret) |
71 | return ret; |
72 | if (val != INV_MPU_MAGN_BITS_WIA) |
73 | return -ENODEV; |
74 | |
75 | /* software reset for MPU925x only */ |
76 | switch (st->chip_type) { |
77 | case INV_MPU9250: |
78 | case INV_MPU9255: |
79 | ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR, |
80 | INV_MPU9250_MAGN_REG_CNTL2, |
81 | INV_MPU9250_MAGN_BIT_SRST); |
82 | if (ret) |
83 | return ret; |
84 | break; |
85 | default: |
86 | break; |
87 | } |
88 | |
89 | /* read fuse ROM data */ |
90 | ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR, |
91 | INV_MPU_MAGN_REG_CNTL1, |
92 | INV_MPU_MAGN_BITS_MODE_FUSE); |
93 | if (ret) |
94 | return ret; |
95 | |
96 | ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_ASAX, |
97 | val: asa, size: sizeof(asa)); |
98 | if (ret) |
99 | return ret; |
100 | |
101 | /* switch back to power-down */ |
102 | ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR, |
103 | INV_MPU_MAGN_REG_CNTL1, |
104 | INV_MPU_MAGN_BITS_MODE_PWDN); |
105 | if (ret) |
106 | return ret; |
107 | |
108 | /* |
109 | * Sensor sentivity |
110 | * 1 uT = 0.01 G and value is in micron (1e6) |
111 | * sensitvity = x uT * 0.01 * 1e6 |
112 | */ |
113 | switch (st->chip_type) { |
114 | case INV_MPU9150: |
115 | /* sensor sensitivity is 0.3 uT */ |
116 | sensitivity = 3000; |
117 | break; |
118 | case INV_MPU9250: |
119 | case INV_MPU9255: |
120 | /* sensor sensitivity in 16 bits mode: 0.15 uT */ |
121 | sensitivity = 1500; |
122 | break; |
123 | default: |
124 | return -EINVAL; |
125 | } |
126 | |
127 | /* |
128 | * Sensitivity adjustement and scale to Gauss |
129 | * |
130 | * Hadj = H * (((ASA - 128) * 0.5 / 128) + 1) |
131 | * Factor simplification: |
132 | * Hadj = H * ((ASA + 128) / 256) |
133 | * |
134 | * raw_to_gauss = Hadj * sensitivity |
135 | */ |
136 | st->magn_raw_to_gauss[0] = (((int32_t)asa[0] + 128) * sensitivity) / 256; |
137 | st->magn_raw_to_gauss[1] = (((int32_t)asa[1] + 128) * sensitivity) / 256; |
138 | st->magn_raw_to_gauss[2] = (((int32_t)asa[2] + 128) * sensitivity) / 256; |
139 | |
140 | return 0; |
141 | } |
142 | |
143 | /** |
144 | * inv_mpu_magn_probe() - probe and setup magnetometer chip |
145 | * @st: driver internal state |
146 | * |
147 | * Returns 0 on success, a negative error code otherwise |
148 | * |
149 | * It is probing the chip and setting up all needed i2c transfers. |
150 | * Noop if there is no magnetometer in the chip. |
151 | */ |
152 | int inv_mpu_magn_probe(struct inv_mpu6050_state *st) |
153 | { |
154 | uint8_t val; |
155 | int ret; |
156 | |
157 | /* quit if chip is not supported */ |
158 | if (!inv_magn_supported(st)) |
159 | return 0; |
160 | |
161 | /* configure i2c master aux port */ |
162 | ret = inv_mpu_aux_init(st); |
163 | if (ret) |
164 | return ret; |
165 | |
166 | /* check and init mag chip */ |
167 | ret = inv_magn_init(st); |
168 | if (ret) |
169 | return ret; |
170 | |
171 | /* |
172 | * configure mpu i2c master accesses |
173 | * i2c SLV0: read sensor data, 7 bytes data(6)-ST2 |
174 | * Byte swap data to store them in big-endian in impair address groups |
175 | */ |
176 | ret = regmap_write(map: st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0), |
177 | INV_MPU6050_BIT_I2C_SLV_RNW | INV_MPU_MAGN_I2C_ADDR); |
178 | if (ret) |
179 | return ret; |
180 | |
181 | ret = regmap_write(map: st->map, INV_MPU6050_REG_I2C_SLV_REG(0), |
182 | INV_MPU_MAGN_REG_DATA); |
183 | if (ret) |
184 | return ret; |
185 | |
186 | ret = regmap_write(map: st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), |
187 | INV_MPU6050_BIT_SLV_EN | |
188 | INV_MPU6050_BIT_SLV_BYTE_SW | |
189 | INV_MPU6050_BIT_SLV_GRP | |
190 | INV_MPU9X50_BYTES_MAGN); |
191 | if (ret) |
192 | return ret; |
193 | |
194 | /* i2c SLV1: launch single measurement */ |
195 | ret = regmap_write(map: st->map, INV_MPU6050_REG_I2C_SLV_ADDR(1), |
196 | INV_MPU_MAGN_I2C_ADDR); |
197 | if (ret) |
198 | return ret; |
199 | |
200 | ret = regmap_write(map: st->map, INV_MPU6050_REG_I2C_SLV_REG(1), |
201 | INV_MPU_MAGN_REG_CNTL1); |
202 | if (ret) |
203 | return ret; |
204 | |
205 | /* add 16 bits mode for MPU925x */ |
206 | val = INV_MPU_MAGN_BITS_MODE_SINGLE; |
207 | switch (st->chip_type) { |
208 | case INV_MPU9250: |
209 | case INV_MPU9255: |
210 | val |= INV_MPU9250_MAGN_BIT_OUTPUT_BIT; |
211 | break; |
212 | default: |
213 | break; |
214 | } |
215 | ret = regmap_write(map: st->map, INV_MPU6050_REG_I2C_SLV_DO(1), val); |
216 | if (ret) |
217 | return ret; |
218 | |
219 | return regmap_write(map: st->map, INV_MPU6050_REG_I2C_SLV_CTRL(1), |
220 | INV_MPU6050_BIT_SLV_EN | 1); |
221 | } |
222 | |
223 | /** |
224 | * inv_mpu_magn_set_rate() - set magnetometer sampling rate |
225 | * @st: driver internal state |
226 | * @fifo_rate: mpu set fifo rate |
227 | * |
228 | * Returns 0 on success, a negative error code otherwise |
229 | * |
230 | * Limit sampling frequency to the maximum value supported by the |
231 | * magnetometer chip. Resulting in duplicated data for higher frequencies. |
232 | * Noop if there is no magnetometer in the chip. |
233 | */ |
234 | int inv_mpu_magn_set_rate(const struct inv_mpu6050_state *st, int fifo_rate) |
235 | { |
236 | uint8_t d; |
237 | |
238 | /* quit if chip is not supported */ |
239 | if (!inv_magn_supported(st)) |
240 | return 0; |
241 | |
242 | /* |
243 | * update i2c master delay to limit mag sampling to max frequency |
244 | * compute fifo_rate divider d: rate = fifo_rate / (d + 1) |
245 | */ |
246 | if (fifo_rate > INV_MPU_MAGN_FREQ_HZ_MAX) |
247 | d = fifo_rate / INV_MPU_MAGN_FREQ_HZ_MAX - 1; |
248 | else |
249 | d = 0; |
250 | |
251 | return regmap_write(map: st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, val: d); |
252 | } |
253 | |
254 | /** |
255 | * inv_mpu_magn_set_orient() - fill magnetometer mounting matrix |
256 | * @st: driver internal state |
257 | * |
258 | * Returns 0 on success, a negative error code otherwise |
259 | * |
260 | * Fill magnetometer mounting matrix using the provided chip matrix. |
261 | */ |
262 | int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st) |
263 | { |
264 | struct device *dev = regmap_get_device(map: st->map); |
265 | const char *orient; |
266 | char *str; |
267 | int i; |
268 | |
269 | /* fill magnetometer orientation */ |
270 | switch (st->chip_type) { |
271 | case INV_MPU9150: |
272 | case INV_MPU9250: |
273 | case INV_MPU9255: |
274 | /* x <- y */ |
275 | st->magn_orient.rotation[0] = st->orientation.rotation[3]; |
276 | st->magn_orient.rotation[1] = st->orientation.rotation[4]; |
277 | st->magn_orient.rotation[2] = st->orientation.rotation[5]; |
278 | /* y <- x */ |
279 | st->magn_orient.rotation[3] = st->orientation.rotation[0]; |
280 | st->magn_orient.rotation[4] = st->orientation.rotation[1]; |
281 | st->magn_orient.rotation[5] = st->orientation.rotation[2]; |
282 | /* z <- -z */ |
283 | for (i = 6; i < 9; ++i) { |
284 | orient = st->orientation.rotation[i]; |
285 | |
286 | /* |
287 | * The value is negated according to one of the following |
288 | * rules: |
289 | * |
290 | * 1) Drop leading minus. |
291 | * 2) Leave 0 as is. |
292 | * 3) Add leading minus. |
293 | */ |
294 | if (orient[0] == '-') |
295 | str = devm_kstrdup(dev, s: orient + 1, GFP_KERNEL); |
296 | else if (!strcmp(orient, "0" )) |
297 | str = devm_kstrdup(dev, s: orient, GFP_KERNEL); |
298 | else |
299 | str = devm_kasprintf(dev, GFP_KERNEL, fmt: "-%s" , orient); |
300 | if (!str) |
301 | return -ENOMEM; |
302 | |
303 | st->magn_orient.rotation[i] = str; |
304 | } |
305 | break; |
306 | default: |
307 | st->magn_orient = st->orientation; |
308 | break; |
309 | } |
310 | |
311 | return 0; |
312 | } |
313 | |
314 | /** |
315 | * inv_mpu_magn_read() - read magnetometer data |
316 | * @st: driver internal state |
317 | * @axis: IIO modifier axis value |
318 | * @val: store corresponding axis value |
319 | * |
320 | * Returns 0 on success, a negative error code otherwise |
321 | */ |
322 | int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val) |
323 | { |
324 | unsigned int status; |
325 | __be16 data; |
326 | uint8_t addr; |
327 | int ret; |
328 | |
329 | /* quit if chip is not supported */ |
330 | if (!inv_magn_supported(st)) |
331 | return -ENODEV; |
332 | |
333 | /* Mag data: XH,XL,YH,YL,ZH,ZL */ |
334 | switch (axis) { |
335 | case IIO_MOD_X: |
336 | addr = 0; |
337 | break; |
338 | case IIO_MOD_Y: |
339 | addr = 2; |
340 | break; |
341 | case IIO_MOD_Z: |
342 | addr = 4; |
343 | break; |
344 | default: |
345 | return -EINVAL; |
346 | } |
347 | addr += INV_MPU6050_REG_EXT_SENS_DATA; |
348 | |
349 | /* check i2c status and read raw data */ |
350 | ret = regmap_read(map: st->map, INV_MPU6050_REG_I2C_MST_STATUS, val: &status); |
351 | if (ret) |
352 | return ret; |
353 | |
354 | if (status & INV_MPU6050_BIT_I2C_SLV0_NACK || |
355 | status & INV_MPU6050_BIT_I2C_SLV1_NACK) |
356 | return -EIO; |
357 | |
358 | ret = regmap_bulk_read(map: st->map, reg: addr, val: &data, val_count: sizeof(data)); |
359 | if (ret) |
360 | return ret; |
361 | |
362 | *val = (int16_t)be16_to_cpu(data); |
363 | |
364 | return IIO_VAL_INT; |
365 | } |
366 | |