drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c

   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 }