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 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 */
  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  */
 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  */
 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  */
 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  */
 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 }