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
inv_magn_supported(const struct inv_mpu6050_state * st)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 */
inv_magn_init(struct inv_mpu6050_state * st)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, 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 asa, 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 */
inv_mpu_magn_probe(struct inv_mpu6050_state * st)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(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(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(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(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(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(st->map, INV_MPU6050_REG_I2C_SLV_DO(1), val);
216 if (ret)
217 return ret;
218
219 return regmap_write(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 */
inv_mpu_magn_set_rate(const struct inv_mpu6050_state * st,int fifo_rate)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(st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, 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 */
inv_mpu_magn_set_orient(struct inv_mpu6050_state * st)262 int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st)
263 {
264 struct device *dev = regmap_get_device(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, orient + 1, GFP_KERNEL);
296 else if (!strcmp(orient, "0"))
297 str = devm_kstrdup(dev, orient, GFP_KERNEL);
298 else
299 str = devm_kasprintf(dev, GFP_KERNEL, "-%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 */
inv_mpu_magn_read(struct inv_mpu6050_state * st,int axis,int * val)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(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &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(st->map, addr, &data, 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