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 /* Magnetometer maximum frequency */ 48 #define INV_MPU_MAGN_FREQ_HZ_MAX 50 49 50 static bool inv_magn_supported(const struct inv_mpu6050_state *st) 51 { 52 switch (st->chip_type) { 53 case INV_MPU9150: 54 case INV_MPU9250: 55 case INV_MPU9255: 56 return true; 57 default: 58 return false; 59 } 60 } 61 62 /* init magnetometer chip */ 63 static int inv_magn_init(struct inv_mpu6050_state *st) 64 { 65 uint8_t val; 66 uint8_t asa[3]; 67 int32_t sensitivity; 68 int ret; 69 70 /* check whoami */ 71 ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_WIA, 72 &val, sizeof(val)); 73 if (ret) 74 return ret; 75 if (val != INV_MPU_MAGN_BITS_WIA) 76 return -ENODEV; 77 78 /* software reset for MPU925x only */ 79 switch (st->chip_type) { 80 case INV_MPU9250: 81 case INV_MPU9255: 82 ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR, 83 INV_MPU9250_MAGN_REG_CNTL2, 84 INV_MPU9250_MAGN_BIT_SRST); 85 if (ret) 86 return ret; 87 break; 88 default: 89 break; 90 } 91 92 /* read fuse ROM data */ 93 ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR, 94 INV_MPU_MAGN_REG_CNTL1, 95 INV_MPU_MAGN_BITS_MODE_FUSE); 96 if (ret) 97 return ret; 98 99 ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_ASAX, 100 asa, sizeof(asa)); 101 if (ret) 102 return ret; 103 104 /* switch back to power-down */ 105 ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR, 106 INV_MPU_MAGN_REG_CNTL1, 107 INV_MPU_MAGN_BITS_MODE_PWDN); 108 if (ret) 109 return ret; 110 111 /* 112 * Sensor sentivity 113 * 1 uT = 0.01 G and value is in micron (1e6) 114 * sensitvity = x uT * 0.01 * 1e6 115 */ 116 switch (st->chip_type) { 117 case INV_MPU9150: 118 /* sensor sensitivity is 0.3 uT */ 119 sensitivity = 3000; 120 break; 121 case INV_MPU9250: 122 case INV_MPU9255: 123 /* sensor sensitivity in 16 bits mode: 0.15 uT */ 124 sensitivity = 1500; 125 break; 126 default: 127 return -EINVAL; 128 } 129 130 /* 131 * Sensitivity adjustement and scale to Gauss 132 * 133 * Hadj = H * (((ASA - 128) * 0.5 / 128) + 1) 134 * Factor simplification: 135 * Hadj = H * ((ASA + 128) / 256) 136 * 137 * raw_to_gauss = Hadj * sensitivity 138 */ 139 st->magn_raw_to_gauss[0] = (((int32_t)asa[0] + 128) * sensitivity) / 256; 140 st->magn_raw_to_gauss[1] = (((int32_t)asa[1] + 128) * sensitivity) / 256; 141 st->magn_raw_to_gauss[2] = (((int32_t)asa[2] + 128) * sensitivity) / 256; 142 143 return 0; 144 } 145 146 /** 147 * inv_mpu_magn_probe() - probe and setup magnetometer chip 148 * @st: driver internal state 149 * 150 * Returns 0 on success, a negative error code otherwise 151 * 152 * It is probing the chip and setting up all needed i2c transfers. 153 * Noop if there is no magnetometer in the chip. 154 */ 155 int inv_mpu_magn_probe(struct inv_mpu6050_state *st) 156 { 157 uint8_t val; 158 int ret; 159 160 /* quit if chip is not supported */ 161 if (!inv_magn_supported(st)) 162 return 0; 163 164 /* configure i2c master aux port */ 165 ret = inv_mpu_aux_init(st); 166 if (ret) 167 return ret; 168 169 /* check and init mag chip */ 170 ret = inv_magn_init(st); 171 if (ret) 172 return ret; 173 174 /* 175 * configure mpu i2c master accesses 176 * i2c SLV0: read sensor data, 7 bytes data(6)-ST2 177 * Byte swap data to store them in big-endian in impair address groups 178 */ 179 ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0), 180 INV_MPU6050_BIT_I2C_SLV_RNW | INV_MPU_MAGN_I2C_ADDR); 181 if (ret) 182 return ret; 183 184 ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0), 185 INV_MPU_MAGN_REG_DATA); 186 if (ret) 187 return ret; 188 189 ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 190 INV_MPU6050_BIT_SLV_EN | 191 INV_MPU6050_BIT_SLV_BYTE_SW | 192 INV_MPU6050_BIT_SLV_GRP | 193 INV_MPU9X50_BYTES_MAGN); 194 if (ret) 195 return ret; 196 197 /* i2c SLV1: launch single measurement */ 198 ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(1), 199 INV_MPU_MAGN_I2C_ADDR); 200 if (ret) 201 return ret; 202 203 ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(1), 204 INV_MPU_MAGN_REG_CNTL1); 205 if (ret) 206 return ret; 207 208 /* add 16 bits mode for MPU925x */ 209 val = INV_MPU_MAGN_BITS_MODE_SINGLE; 210 switch (st->chip_type) { 211 case INV_MPU9250: 212 case INV_MPU9255: 213 val |= INV_MPU9250_MAGN_BIT_OUTPUT_BIT; 214 break; 215 default: 216 break; 217 } 218 ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_DO(1), val); 219 if (ret) 220 return ret; 221 222 return regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(1), 223 INV_MPU6050_BIT_SLV_EN | 1); 224 } 225 226 /** 227 * inv_mpu_magn_set_rate() - set magnetometer sampling rate 228 * @st: driver internal state 229 * @fifo_rate: mpu set fifo rate 230 * 231 * Returns 0 on success, a negative error code otherwise 232 * 233 * Limit sampling frequency to the maximum value supported by the 234 * magnetometer chip. Resulting in duplicated data for higher frequencies. 235 * Noop if there is no magnetometer in the chip. 236 */ 237 int inv_mpu_magn_set_rate(const struct inv_mpu6050_state *st, int fifo_rate) 238 { 239 uint8_t d; 240 241 /* quit if chip is not supported */ 242 if (!inv_magn_supported(st)) 243 return 0; 244 245 /* 246 * update i2c master delay to limit mag sampling to max frequency 247 * compute fifo_rate divider d: rate = fifo_rate / (d + 1) 248 */ 249 if (fifo_rate > INV_MPU_MAGN_FREQ_HZ_MAX) 250 d = fifo_rate / INV_MPU_MAGN_FREQ_HZ_MAX - 1; 251 else 252 d = 0; 253 254 return regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, d); 255 } 256 257 /** 258 * inv_mpu_magn_set_orient() - fill magnetometer mounting matrix 259 * @st: driver internal state 260 * 261 * Returns 0 on success, a negative error code otherwise 262 * 263 * Fill magnetometer mounting matrix using the provided chip matrix. 264 */ 265 int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st) 266 { 267 const char *orient; 268 char *str; 269 int i; 270 271 /* fill magnetometer orientation */ 272 switch (st->chip_type) { 273 case INV_MPU9150: 274 case INV_MPU9250: 275 case INV_MPU9255: 276 /* x <- y */ 277 st->magn_orient.rotation[0] = st->orientation.rotation[3]; 278 st->magn_orient.rotation[1] = st->orientation.rotation[4]; 279 st->magn_orient.rotation[2] = st->orientation.rotation[5]; 280 /* y <- x */ 281 st->magn_orient.rotation[3] = st->orientation.rotation[0]; 282 st->magn_orient.rotation[4] = st->orientation.rotation[1]; 283 st->magn_orient.rotation[5] = st->orientation.rotation[2]; 284 /* z <- -z */ 285 for (i = 0; i < 3; ++i) { 286 orient = st->orientation.rotation[6 + i]; 287 /* use length + 2 for adding minus sign if needed */ 288 str = devm_kzalloc(regmap_get_device(st->map), 289 strlen(orient) + 2, GFP_KERNEL); 290 if (str == NULL) 291 return -ENOMEM; 292 if (strcmp(orient, "0") == 0) { 293 strcpy(str, orient); 294 } else if (orient[0] == '-') { 295 strcpy(str, &orient[1]); 296 } else { 297 str[0] = '-'; 298 strcpy(&str[1], orient); 299 } 300 st->magn_orient.rotation[6 + i] = str; 301 } 302 break; 303 default: 304 st->magn_orient = st->orientation; 305 break; 306 } 307 308 return 0; 309 } 310 311 /** 312 * inv_mpu_magn_read() - read magnetometer data 313 * @st: driver internal state 314 * @axis: IIO modifier axis value 315 * @val: store corresponding axis value 316 * 317 * Returns 0 on success, a negative error code otherwise 318 */ 319 int inv_mpu_magn_read(const struct inv_mpu6050_state *st, int axis, int *val) 320 { 321 unsigned int user_ctrl, status; 322 __be16 data[3]; 323 uint8_t addr; 324 uint8_t d; 325 unsigned int period_ms; 326 int ret; 327 328 /* quit if chip is not supported */ 329 if (!inv_magn_supported(st)) 330 return -ENODEV; 331 332 /* Mag data: X - Y - Z */ 333 switch (axis) { 334 case IIO_MOD_X: 335 addr = 0; 336 break; 337 case IIO_MOD_Y: 338 addr = 1; 339 break; 340 case IIO_MOD_Z: 341 addr = 2; 342 break; 343 default: 344 return -EINVAL; 345 } 346 347 /* set sample rate to max mag freq */ 348 d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU_MAGN_FREQ_HZ_MAX); 349 ret = regmap_write(st->map, st->reg->sample_rate_div, d); 350 if (ret) 351 return ret; 352 353 /* start i2c master, wait for xfer, stop */ 354 user_ctrl = st->chip_config.user_ctrl | INV_MPU6050_BIT_I2C_MST_EN; 355 ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl); 356 if (ret) 357 return ret; 358 359 /* need to wait 2 periods + half-period margin */ 360 period_ms = 1000 / INV_MPU_MAGN_FREQ_HZ_MAX; 361 msleep(period_ms * 2 + period_ms / 2); 362 user_ctrl = st->chip_config.user_ctrl; 363 ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl); 364 if (ret) 365 return ret; 366 367 /* restore sample rate */ 368 d = st->chip_config.divider; 369 ret = regmap_write(st->map, st->reg->sample_rate_div, d); 370 if (ret) 371 return ret; 372 373 /* check i2c status and read raw data */ 374 ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status); 375 if (ret) 376 return ret; 377 378 if (status & INV_MPU6050_BIT_I2C_SLV0_NACK || 379 status & INV_MPU6050_BIT_I2C_SLV1_NACK) 380 return -EIO; 381 382 ret = regmap_bulk_read(st->map, INV_MPU6050_REG_EXT_SENS_DATA, 383 data, sizeof(data)); 384 if (ret) 385 return ret; 386 387 *val = (int16_t)be16_to_cpu(data[addr]); 388 389 return IIO_VAL_INT; 390 } 391