LXR linux/drivers/iio/imu/inv_mpu6050/inv_mpu

   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
  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 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 */
 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 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 */
 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 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 */
 262int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st)
 263{
 264        const char *orient;
 265        char *str;
 266        int i;
 267
 268        /* fill magnetometer orientation */
 269        switch (st->chip_type) {
 270        case INV_MPU9150:
 271        case INV_MPU9250:
 272        case INV_MPU9255:
 273                /* x <- y */
 274                st->magn_orient.rotation[0] = st->orientation.rotation[3];
 275                st->magn_orient.rotation[1] = st->orientation.rotation[4];
 276                st->magn_orient.rotation[2] = st->orientation.rotation[5];
 277                /* y <- x */
 278                st->magn_orient.rotation[3] = st->orientation.rotation[0];
 279                st->magn_orient.rotation[4] = st->orientation.rotation[1];
 280                st->magn_orient.rotation[5] = st->orientation.rotation[2];
 281                /* z <- -z */
 282                for (i = 0; i < 3; ++i) {
 283                        orient = st->orientation.rotation[6 + i];
 284                        /* use length + 2 for adding minus sign if needed */
 285                        str = devm_kzalloc(regmap_get_device(st->map),
 286                                           strlen(orient) + 2, GFP_KERNEL);
 287                        if (str == NULL)
 288                                return -ENOMEM;
 289                        if (strcmp(orient, "0") == 0) {
 290                                strcpy(str, orient);
 291                        } else if (orient[0] == '-') {
 292                                strcpy(str, &orient[1]);
 293                        } else {
 294                                str[0] = '-';
 295                                strcpy(&str[1], orient);
 296                        }
 297                        st->magn_orient.rotation[6 + i] = str;
 298                }
 299                break;
 300        default:
 301                st->magn_orient = st->orientation;
 302                break;
 303        }
 304
 305        return 0;
 306}
 307
 308/**
 309 * inv_mpu_magn_read() - read magnetometer data
 310 * @st: driver internal state
 311 * @axis: IIO modifier axis value
 312 * @val: store corresponding axis value
 313 *
 314 * Returns 0 on success, a negative error code otherwise
 315 */
 316int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val)
 317{
 318        unsigned int status;
 319        __be16 data;
 320        uint8_t addr;
 321        int ret;
 322
 323        /* quit if chip is not supported */
 324        if (!inv_magn_supported(st))
 325                return -ENODEV;
 326
 327        /* Mag data: XH,XL,YH,YL,ZH,ZL */
 328        switch (axis) {
 329        case IIO_MOD_X:
 330                addr = 0;
 331                break;
 332        case IIO_MOD_Y:
 333                addr = 2;
 334                break;
 335        case IIO_MOD_Z:
 336                addr = 4;
 337                break;
 338        default:
 339                return -EINVAL;
 340        }
 341        addr += INV_MPU6050_REG_EXT_SENS_DATA;
 342
 343        /* check i2c status and read raw data */
 344        ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status);
 345        if (ret)
 346                return ret;
 347
 348        if (status & INV_MPU6050_BIT_I2C_SLV0_NACK ||
 349                        status & INV_MPU6050_BIT_I2C_SLV1_NACK)
 350                return -EIO;
 351
 352        ret = regmap_bulk_read(st->map, addr, &data, sizeof(data));
 353        if (ret)
 354                return ret;
 355
 356        *val = (int16_t)be16_to_cpu(data);
 357
 358        return IIO_VAL_INT;
 359}
 360