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