366 lines · c
1// SPDX-License-Identifier: GPL-2.02/*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 bus16 * MPU9150 is AK897517 * MPU9250 is AK896318 */19#define INV_MPU_MAGN_I2C_ADDR 0x0C20 21#define INV_MPU_MAGN_REG_WIA 0x0022#define INV_MPU_MAGN_BITS_WIA 0x4823 24#define INV_MPU_MAGN_REG_ST1 0x0225#define INV_MPU_MAGN_BIT_DRDY 0x0126#define INV_MPU_MAGN_BIT_DOR 0x0227 28#define INV_MPU_MAGN_REG_DATA 0x0329 30#define INV_MPU_MAGN_REG_ST2 0x0931#define INV_MPU_MAGN_BIT_HOFL 0x0832#define INV_MPU_MAGN_BIT_BITM 0x1033 34#define INV_MPU_MAGN_REG_CNTL1 0x0A35#define INV_MPU_MAGN_BITS_MODE_PWDN 0x0036#define INV_MPU_MAGN_BITS_MODE_SINGLE 0x0137#define INV_MPU_MAGN_BITS_MODE_FUSE 0x0F38#define INV_MPU9250_MAGN_BIT_OUTPUT_BIT 0x1039 40#define INV_MPU9250_MAGN_REG_CNTL2 0x0B41#define INV_MPU9250_MAGN_BIT_SRST 0x0142 43#define INV_MPU_MAGN_REG_ASAX 0x1044#define INV_MPU_MAGN_REG_ASAY 0x1145#define INV_MPU_MAGN_REG_ASAZ 0x1246 47static 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 */60static 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 sentivity110 * 1 uT = 0.01 G and value is in micron (1e6)111 * sensitvity = x uT * 0.01 * 1e6112 */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 Gauss129 *130 * Hadj = H * (((ASA - 128) * 0.5 / 128) + 1)131 * Factor simplification:132 * Hadj = H * ((ASA + 128) / 256)133 *134 * raw_to_gauss = Hadj * sensitivity135 */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 chip145 * @st: driver internal state146 *147 * Returns 0 on success, a negative error code otherwise148 *149 * It is probing the chip and setting up all needed i2c transfers.150 * Noop if there is no magnetometer in the chip.151 */152int 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 accesses173 * i2c SLV0: read sensor data, 7 bytes data(6)-ST2174 * Byte swap data to store them in big-endian in impair address groups175 */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 rate225 * @st: driver internal state226 * @fifo_rate: mpu set fifo rate227 *228 * Returns 0 on success, a negative error code otherwise229 *230 * Limit sampling frequency to the maximum value supported by the231 * magnetometer chip. Resulting in duplicated data for higher frequencies.232 * Noop if there is no magnetometer in the chip.233 */234int 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 frequency244 * 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 else249 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 matrix256 * @st: driver internal state257 *258 * Returns 0 on success, a negative error code otherwise259 *260 * Fill magnetometer mounting matrix using the provided chip matrix.261 */262int 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 following288 * 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 else299 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 data316 * @st: driver internal state317 * @axis: IIO modifier axis value318 * @val: store corresponding axis value319 *320 * Returns 0 on success, a negative error code otherwise321 */322int 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