drivers/iio/imu/inv_mpu6050/inv_mpu_core.c

   1 /*
   2 * Copyright (C) 2012 Invensense, Inc.
   3 *
   4 * This software is licensed under the terms of the GNU General Public
   5 * License version 2, as published by the Free Software Foundation, and
   6 * may be copied, distributed, and modified under those terms.
   7 *
   8 * This program is distributed in the hope that it will be useful,
   9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11 * GNU General Public License for more details.
  12 */
  13
  14 #include <linux/module.h>
  15 #include <linux/slab.h>
  16 #include <linux/i2c.h>
  17 #include <linux/err.h>
  18 #include <linux/delay.h>
  19 #include <linux/sysfs.h>
  20 #include <linux/jiffies.h>
  21 #include <linux/irq.h>
  22 #include <linux/interrupt.h>
  23 #include <linux/kfifo.h>
  24 #include <linux/spinlock.h>
  25 #include <linux/iio/iio.h>
  26 #include <linux/acpi.h>
  27 #include "inv_mpu_iio.h"
  28
  29 /*
  30  * this is the gyro scale translated from dynamic range plus/minus
  31  * {250, 500, 1000, 2000} to rad/s
  32  */
  33 static const int gyro_scale_6050[] = {133090, 266181, 532362, 1064724};
  34
  35 /*
  36  * this is the accel scale translated from dynamic range plus/minus
  37  * {2, 4, 8, 16} to m/s^2
  38  */
  39 static const int accel_scale[] = {598, 1196, 2392, 4785};
  40
  41 static const struct inv_mpu6050_reg_map reg_set_6500 = {
  42         .sample_rate_div        = INV_MPU6050_REG_SAMPLE_RATE_DIV,
  43         .lpf                    = INV_MPU6050_REG_CONFIG,
  44         .accel_lpf              = INV_MPU6500_REG_ACCEL_CONFIG_2,
  45         .user_ctrl              = INV_MPU6050_REG_USER_CTRL,
  46         .fifo_en                = INV_MPU6050_REG_FIFO_EN,
  47         .gyro_config            = INV_MPU6050_REG_GYRO_CONFIG,
  48         .accl_config            = INV_MPU6050_REG_ACCEL_CONFIG,
  49         .fifo_count_h           = INV_MPU6050_REG_FIFO_COUNT_H,
  50         .fifo_r_w               = INV_MPU6050_REG_FIFO_R_W,
  51         .raw_gyro               = INV_MPU6050_REG_RAW_GYRO,
  52         .raw_accl               = INV_MPU6050_REG_RAW_ACCEL,
  53         .temperature            = INV_MPU6050_REG_TEMPERATURE,
  54         .int_enable             = INV_MPU6050_REG_INT_ENABLE,
  55         .pwr_mgmt_1             = INV_MPU6050_REG_PWR_MGMT_1,
  56         .pwr_mgmt_2             = INV_MPU6050_REG_PWR_MGMT_2,
  57         .int_pin_cfg            = INV_MPU6050_REG_INT_PIN_CFG,
  58         .accl_offset            = INV_MPU6500_REG_ACCEL_OFFSET,
  59         .gyro_offset            = INV_MPU6050_REG_GYRO_OFFSET,
  60 };
  61
  62 static const struct inv_mpu6050_reg_map reg_set_6050 = {
  63         .sample_rate_div        = INV_MPU6050_REG_SAMPLE_RATE_DIV,
  64         .lpf                    = INV_MPU6050_REG_CONFIG,
  65         .user_ctrl              = INV_MPU6050_REG_USER_CTRL,
  66         .fifo_en                = INV_MPU6050_REG_FIFO_EN,
  67         .gyro_config            = INV_MPU6050_REG_GYRO_CONFIG,
  68         .accl_config            = INV_MPU6050_REG_ACCEL_CONFIG,
  69         .fifo_count_h           = INV_MPU6050_REG_FIFO_COUNT_H,
  70         .fifo_r_w               = INV_MPU6050_REG_FIFO_R_W,
  71         .raw_gyro               = INV_MPU6050_REG_RAW_GYRO,
  72         .raw_accl               = INV_MPU6050_REG_RAW_ACCEL,
  73         .temperature            = INV_MPU6050_REG_TEMPERATURE,
  74         .int_enable             = INV_MPU6050_REG_INT_ENABLE,
  75         .pwr_mgmt_1             = INV_MPU6050_REG_PWR_MGMT_1,
  76         .pwr_mgmt_2             = INV_MPU6050_REG_PWR_MGMT_2,
  77         .int_pin_cfg            = INV_MPU6050_REG_INT_PIN_CFG,
  78         .accl_offset            = INV_MPU6050_REG_ACCEL_OFFSET,
  79         .gyro_offset            = INV_MPU6050_REG_GYRO_OFFSET,
  80 };
  81
  82 static const struct inv_mpu6050_chip_config chip_config_6050 = {
  83         .fsr = INV_MPU6050_FSR_2000DPS,
  84         .lpf = INV_MPU6050_FILTER_20HZ,
  85         .fifo_rate = INV_MPU6050_INIT_FIFO_RATE,
  86         .gyro_fifo_enable = false,
  87         .accl_fifo_enable = false,
  88         .accl_fs = INV_MPU6050_FS_02G,
  89 };
  90
  91 /* Indexed by enum inv_devices */
  92 static const struct inv_mpu6050_hw hw_info[] = {
  93         {
  94                 .whoami = INV_MPU6050_WHOAMI_VALUE,
  95                 .name = "MPU6050",
  96                 .reg = &reg_set_6050,
  97                 .config = &chip_config_6050,
  98         },
  99         {
 100                 .whoami = INV_MPU6500_WHOAMI_VALUE,
 101                 .name = "MPU6500",
 102                 .reg = &reg_set_6500,
 103                 .config = &chip_config_6050,
 104         },
 105         {
 106                 .whoami = INV_MPU6000_WHOAMI_VALUE,
 107                 .name = "MPU6000",
 108                 .reg = &reg_set_6050,
 109                 .config = &chip_config_6050,
 110         },
 111         {
 112                 .whoami = INV_MPU9150_WHOAMI_VALUE,
 113                 .name = "MPU9150",
 114                 .reg = &reg_set_6050,
 115                 .config = &chip_config_6050,
 116         },
 117         {
 118                 .whoami = INV_MPU9250_WHOAMI_VALUE,
 119                 .name = "MPU9250",
 120                 .reg = &reg_set_6500,
 121                 .config = &chip_config_6050,
 122         },
 123         {
 124                 .whoami = INV_ICM20608_WHOAMI_VALUE,
 125                 .name = "ICM20608",
 126                 .reg = &reg_set_6500,
 127                 .config = &chip_config_6050,
 128         },
 129 };
 130
 131 int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask)
 132 {
 133         unsigned int d, mgmt_1;
 134         int result;
 135         /*
 136          * switch clock needs to be careful. Only when gyro is on, can
 137          * clock source be switched to gyro. Otherwise, it must be set to
 138          * internal clock
 139          */
 140         if (mask == INV_MPU6050_BIT_PWR_GYRO_STBY) {
 141                 result = regmap_read(st->map, st->reg->pwr_mgmt_1, &mgmt_1);
 142                 if (result)
 143                         return result;
 144
 145                 mgmt_1 &= ~INV_MPU6050_BIT_CLK_MASK;
 146         }
 147
 148         if ((mask == INV_MPU6050_BIT_PWR_GYRO_STBY) && (!en)) {
 149                 /*
 150                  * turning off gyro requires switch to internal clock first.
 151                  * Then turn off gyro engine
 152                  */
 153                 mgmt_1 |= INV_CLK_INTERNAL;
 154                 result = regmap_write(st->map, st->reg->pwr_mgmt_1, mgmt_1);
 155                 if (result)
 156                         return result;
 157         }
 158
 159         result = regmap_read(st->map, st->reg->pwr_mgmt_2, &d);
 160         if (result)
 161                 return result;
 162         if (en)
 163                 d &= ~mask;
 164         else
 165                 d |= mask;
 166         result = regmap_write(st->map, st->reg->pwr_mgmt_2, d);
 167         if (result)
 168                 return result;
 169
 170         if (en) {
 171                 /* Wait for output stabilize */
 172                 msleep(INV_MPU6050_TEMP_UP_TIME);
 173                 if (mask == INV_MPU6050_BIT_PWR_GYRO_STBY) {
 174                         /* switch internal clock to PLL */
 175                         mgmt_1 |= INV_CLK_PLL;
 176                         result = regmap_write(st->map,
 177                                               st->reg->pwr_mgmt_1, mgmt_1);
 178                         if (result)
 179                                 return result;
 180                 }
 181         }
 182
 183         return 0;
 184 }
 185
 186 int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on)
 187 {
 188         int result = 0;
 189
 190         if (power_on) {
 191                 /* Already under indio-dev->mlock mutex */
 192                 if (!st->powerup_count)
 193                         result = regmap_write(st->map, st->reg->pwr_mgmt_1, 0);
 194                 if (!result)
 195                         st->powerup_count++;
 196         } else {
 197                 st->powerup_count--;
 198                 if (!st->powerup_count)
 199                         result = regmap_write(st->map, st->reg->pwr_mgmt_1,
 200                                               INV_MPU6050_BIT_SLEEP);
 201         }
 202
 203         if (result)
 204                 return result;
 205
 206         if (power_on)
 207                 usleep_range(INV_MPU6050_REG_UP_TIME_MIN,
 208                              INV_MPU6050_REG_UP_TIME_MAX);
 209
 210         return 0;
 211 }
 212 EXPORT_SYMBOL_GPL(inv_mpu6050_set_power_itg);
 213
 214 /**
 215  *  inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent
 216  *
 217  *  MPU60xx/MPU9150 use only 1 register for accelerometer + gyroscope
 218  *  MPU6500 and above have a dedicated register for accelerometer
 219  */
 220 static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
 221                                     enum inv_mpu6050_filter_e val)
 222 {
 223         int result;
 224
 225         result = regmap_write(st->map, st->reg->lpf, val);
 226         if (result)
 227                 return result;
 228
 229         switch (st->chip_type) {
 230         case INV_MPU6050:
 231         case INV_MPU6000:
 232         case INV_MPU9150:
 233                 /* old chips, nothing to do */
 234                 result = 0;
 235                 break;
 236         default:
 237                 /* set accel lpf */
 238                 result = regmap_write(st->map, st->reg->accel_lpf, val);
 239                 break;
 240         }
 241
 242         return result;
 243 }
 244
 245 /**
 246  *  inv_mpu6050_init_config() - Initialize hardware, disable FIFO.
 247  *
 248  *  Initial configuration:
 249  *  FSR: ± 2000DPS
 250  *  DLPF: 20Hz
 251  *  FIFO rate: 50Hz
 252  *  Clock source: Gyro PLL
 253  */
 254 static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
 255 {
 256         int result;
 257         u8 d;
 258         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 259
 260         result = inv_mpu6050_set_power_itg(st, true);
 261         if (result)
 262                 return result;
 263         d = (INV_MPU6050_FSR_2000DPS << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT);
 264         result = regmap_write(st->map, st->reg->gyro_config, d);
 265         if (result)
 266                 return result;
 267
 268         result = inv_mpu6050_set_lpf_regs(st, INV_MPU6050_FILTER_20HZ);
 269         if (result)
 270                 return result;
 271
 272         d = INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE - 1;
 273         result = regmap_write(st->map, st->reg->sample_rate_div, d);
 274         if (result)
 275                 return result;
 276
 277         d = (INV_MPU6050_FS_02G << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT);
 278         result = regmap_write(st->map, st->reg->accl_config, d);
 279         if (result)
 280                 return result;
 281
 282         memcpy(&st->chip_config, hw_info[st->chip_type].config,
 283                sizeof(struct inv_mpu6050_chip_config));
 284         result = inv_mpu6050_set_power_itg(st, false);
 285
 286         return result;
 287 }
 288
 289 static int inv_mpu6050_sensor_set(struct inv_mpu6050_state  *st, int reg,
 290                                 int axis, int val)
 291 {
 292         int ind, result;
 293         __be16 d = cpu_to_be16(val);
 294
 295         ind = (axis - IIO_MOD_X) * 2;
 296         result = regmap_bulk_write(st->map, reg + ind, (u8 *)&d, 2);
 297         if (result)
 298                 return -EINVAL;
 299
 300         return 0;
 301 }
 302
 303 static int inv_mpu6050_sensor_show(struct inv_mpu6050_state  *st, int reg,
 304                                    int axis, int *val)
 305 {
 306         int ind, result;
 307         __be16 d;
 308
 309         ind = (axis - IIO_MOD_X) * 2;
 310         result = regmap_bulk_read(st->map, reg + ind, (u8 *)&d, 2);
 311         if (result)
 312                 return -EINVAL;
 313         *val = (short)be16_to_cpup(&d);
 314
 315         return IIO_VAL_INT;
 316 }
 317
 318 static int
 319 inv_mpu6050_read_raw(struct iio_dev *indio_dev,
 320                      struct iio_chan_spec const *chan,
 321                      int *val, int *val2, long mask)
 322 {
 323         struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 324         int ret = 0;
 325
 326         switch (mask) {
 327         case IIO_CHAN_INFO_RAW:
 328         {
 329                 int result;
 330
 331                 ret = IIO_VAL_INT;
 332                 result = 0;
 333                 mutex_lock(&indio_dev->mlock);
 334                 if (!st->chip_config.enable) {
 335                         result = inv_mpu6050_set_power_itg(st, true);
 336                         if (result)
 337                                 goto error_read_raw;
 338                 }
 339                 /* when enable is on, power is already on */
 340                 switch (chan->type) {
 341                 case IIO_ANGL_VEL:
 342                         if (!st->chip_config.gyro_fifo_enable ||
 343                             !st->chip_config.enable) {
 344                                 result = inv_mpu6050_switch_engine(st, true,
 345                                                 INV_MPU6050_BIT_PWR_GYRO_STBY);
 346                                 if (result)
 347                                         goto error_read_raw;
 348                         }
 349                         ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro,
 350                                                       chan->channel2, val);
 351                         if (!st->chip_config.gyro_fifo_enable ||
 352                             !st->chip_config.enable) {
 353                                 result = inv_mpu6050_switch_engine(st, false,
 354                                                 INV_MPU6050_BIT_PWR_GYRO_STBY);
 355                                 if (result)
 356                                         goto error_read_raw;
 357                         }
 358                         break;
 359                 case IIO_ACCEL:
 360                         if (!st->chip_config.accl_fifo_enable ||
 361                             !st->chip_config.enable) {
 362                                 result = inv_mpu6050_switch_engine(st, true,
 363                                                 INV_MPU6050_BIT_PWR_ACCL_STBY);
 364                                 if (result)
 365                                         goto error_read_raw;
 366                         }
 367                         ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl,
 368                                                       chan->channel2, val);
 369                         if (!st->chip_config.accl_fifo_enable ||
 370                             !st->chip_config.enable) {
 371                                 result = inv_mpu6050_switch_engine(st, false,
 372                                                 INV_MPU6050_BIT_PWR_ACCL_STBY);
 373                                 if (result)
 374                                         goto error_read_raw;
 375                         }
 376                         break;
 377                 case IIO_TEMP:
 378                         /* wait for stablization */
 379                         msleep(INV_MPU6050_SENSOR_UP_TIME);
 380                         ret = inv_mpu6050_sensor_show(st, st->reg->temperature,
 381                                                 IIO_MOD_X, val);
 382                         break;
 383                 default:
 384                         ret = -EINVAL;
 385                         break;
 386                 }
 387 error_read_raw:
 388                 if (!st->chip_config.enable)
 389                         result |= inv_mpu6050_set_power_itg(st, false);
 390                 mutex_unlock(&indio_dev->mlock);
 391                 if (result)
 392                         return result;
 393
 394                 return ret;
 395         }
 396         case IIO_CHAN_INFO_SCALE:
 397                 switch (chan->type) {
 398                 case IIO_ANGL_VEL:
 399                         *val  = 0;
 400                         *val2 = gyro_scale_6050[st->chip_config.fsr];
 401
 402                         return IIO_VAL_INT_PLUS_NANO;
 403                 case IIO_ACCEL:
 404                         *val = 0;
 405                         *val2 = accel_scale[st->chip_config.accl_fs];
 406
 407                         return IIO_VAL_INT_PLUS_MICRO;
 408                 case IIO_TEMP:
 409                         *val = 0;
 410                         *val2 = INV_MPU6050_TEMP_SCALE;
 411
 412                         return IIO_VAL_INT_PLUS_MICRO;
 413                 default:
 414                         return -EINVAL;
 415                 }
 416         case IIO_CHAN_INFO_OFFSET:
 417                 switch (chan->type) {
 418                 case IIO_TEMP:
 419                         *val = INV_MPU6050_TEMP_OFFSET;
 420
 421                         return IIO_VAL_INT;
 422                 default:
 423                         return -EINVAL;
 424                 }
 425         case IIO_CHAN_INFO_CALIBBIAS:
 426                 switch (chan->type) {
 427                 case IIO_ANGL_VEL:
 428                         ret = inv_mpu6050_sensor_show(st, st->reg->gyro_offset,
 429                                                 chan->channel2, val);
 430                         return IIO_VAL_INT;
 431                 case IIO_ACCEL:
 432                         ret = inv_mpu6050_sensor_show(st, st->reg->accl_offset,
 433                                                 chan->channel2, val);
 434                         return IIO_VAL_INT;
 435
 436                 default:
 437                         return -EINVAL;
 438                 }
 439         default:
 440                 return -EINVAL;
 441         }
 442 }
 443
 444 static int inv_mpu6050_write_gyro_scale(struct inv_mpu6050_state *st, int val)
 445 {
 446         int result, i;
 447         u8 d;
 448
 449         for (i = 0; i < ARRAY_SIZE(gyro_scale_6050); ++i) {
 450                 if (gyro_scale_6050[i] == val) {
 451                         d = (i << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT);
 452                         result = regmap_write(st->map, st->reg->gyro_config, d);
 453                         if (result)
 454                                 return result;
 455
 456                         st->chip_config.fsr = i;
 457                         return 0;
 458                 }
 459         }
 460
 461         return -EINVAL;
 462 }
 463
 464 static int inv_write_raw_get_fmt(struct iio_dev *indio_dev,
 465                                  struct iio_chan_spec const *chan, long mask)
 466 {
 467         switch (mask) {
 468         case IIO_CHAN_INFO_SCALE:
 469                 switch (chan->type) {
 470                 case IIO_ANGL_VEL:
 471                         return IIO_VAL_INT_PLUS_NANO;
 472                 default:
 473                         return IIO_VAL_INT_PLUS_MICRO;
 474                 }
 475         default:
 476                 return IIO_VAL_INT_PLUS_MICRO;
 477         }
 478
 479         return -EINVAL;
 480 }
 481
 482 static int inv_mpu6050_write_accel_scale(struct inv_mpu6050_state *st, int val)
 483 {
 484         int result, i;
 485         u8 d;
 486
 487         for (i = 0; i < ARRAY_SIZE(accel_scale); ++i) {
 488                 if (accel_scale[i] == val) {
 489                         d = (i << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT);
 490                         result = regmap_write(st->map, st->reg->accl_config, d);
 491                         if (result)
 492                                 return result;
 493
 494                         st->chip_config.accl_fs = i;
 495                         return 0;
 496                 }
 497         }
 498
 499         return -EINVAL;
 500 }
 501
 502 static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
 503                                  struct iio_chan_spec const *chan,
 504                                  int val, int val2, long mask)
 505 {
 506         struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 507         int result;
 508
 509         mutex_lock(&indio_dev->mlock);
 510         /*
 511          * we should only update scale when the chip is disabled, i.e.
 512          * not running
 513          */
 514         if (st->chip_config.enable) {
 515                 result = -EBUSY;
 516                 goto error_write_raw;
 517         }
 518         result = inv_mpu6050_set_power_itg(st, true);
 519         if (result)
 520                 goto error_write_raw;
 521
 522         switch (mask) {
 523         case IIO_CHAN_INFO_SCALE:
 524                 switch (chan->type) {
 525                 case IIO_ANGL_VEL:
 526                         result = inv_mpu6050_write_gyro_scale(st, val2);
 527                         break;
 528                 case IIO_ACCEL:
 529                         result = inv_mpu6050_write_accel_scale(st, val2);
 530                         break;
 531                 default:
 532                         result = -EINVAL;
 533                         break;
 534                 }
 535                 break;
 536         case IIO_CHAN_INFO_CALIBBIAS:
 537                 switch (chan->type) {
 538                 case IIO_ANGL_VEL:
 539                         result = inv_mpu6050_sensor_set(st,
 540                                                         st->reg->gyro_offset,
 541                                                         chan->channel2, val);
 542                         break;
 543                 case IIO_ACCEL:
 544                         result = inv_mpu6050_sensor_set(st,
 545                                                         st->reg->accl_offset,
 546                                                         chan->channel2, val);
 547                         break;
 548                 default:
 549                         result = -EINVAL;
 550                 }
 551         default:
 552                 result = -EINVAL;
 553                 break;
 554         }
 555
 556 error_write_raw:
 557         result |= inv_mpu6050_set_power_itg(st, false);
 558         mutex_unlock(&indio_dev->mlock);
 559
 560         return result;
 561 }
 562
 563 /**
 564  *  inv_mpu6050_set_lpf() - set low pass filer based on fifo rate.
 565  *
 566  *                  Based on the Nyquist principle, the sampling rate must
 567  *                  exceed twice of the bandwidth of the signal, or there
 568  *                  would be alising. This function basically search for the
 569  *                  correct low pass parameters based on the fifo rate, e.g,
 570  *                  sampling frequency.
 571  *
 572  *  lpf is set automatically when setting sampling rate to avoid any aliases.
 573  */
 574 static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate)
 575 {
 576         const int hz[] = {188, 98, 42, 20, 10, 5};
 577         const int d[] = {INV_MPU6050_FILTER_188HZ, INV_MPU6050_FILTER_98HZ,
 578                         INV_MPU6050_FILTER_42HZ, INV_MPU6050_FILTER_20HZ,
 579                         INV_MPU6050_FILTER_10HZ, INV_MPU6050_FILTER_5HZ};
 580         int i, h, result;
 581         u8 data;
 582
 583         h = (rate >> 1);
 584         i = 0;
 585         while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1))
 586                 i++;
 587         data = d[i];
 588         result = inv_mpu6050_set_lpf_regs(st, data);
 589         if (result)
 590                 return result;
 591         st->chip_config.lpf = data;
 592
 593         return 0;
 594 }
 595
 596 /**
 597  * inv_mpu6050_fifo_rate_store() - Set fifo rate.
 598  */
 599 static ssize_t
 600 inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
 601                             const char *buf, size_t count)
 602 {
 603         s32 fifo_rate;
 604         u8 d;
 605         int result;
 606         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 607         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 608
 609         if (kstrtoint(buf, 10, &fifo_rate))
 610                 return -EINVAL;
 611         if (fifo_rate < INV_MPU6050_MIN_FIFO_RATE ||
 612             fifo_rate > INV_MPU6050_MAX_FIFO_RATE)
 613                 return -EINVAL;
 614         if (fifo_rate == st->chip_config.fifo_rate)
 615                 return count;
 616
 617         mutex_lock(&indio_dev->mlock);
 618         if (st->chip_config.enable) {
 619                 result = -EBUSY;
 620                 goto fifo_rate_fail;
 621         }
 622         result = inv_mpu6050_set_power_itg(st, true);
 623         if (result)
 624                 goto fifo_rate_fail;
 625
 626         d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
 627         result = regmap_write(st->map, st->reg->sample_rate_div, d);
 628         if (result)
 629                 goto fifo_rate_fail;
 630         st->chip_config.fifo_rate = fifo_rate;
 631
 632         result = inv_mpu6050_set_lpf(st, fifo_rate);
 633         if (result)
 634                 goto fifo_rate_fail;
 635
 636 fifo_rate_fail:
 637         result |= inv_mpu6050_set_power_itg(st, false);
 638         mutex_unlock(&indio_dev->mlock);
 639         if (result)
 640                 return result;
 641
 642         return count;
 643 }
 644
 645 /**
 646  * inv_fifo_rate_show() - Get the current sampling rate.
 647  */
 648 static ssize_t
 649 inv_fifo_rate_show(struct device *dev, struct device_attribute *attr,
 650                    char *buf)
 651 {
 652         struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
 653
 654         return sprintf(buf, "%d\n", st->chip_config.fifo_rate);
 655 }
 656
 657 /**
 658  * inv_attr_show() - calling this function will show current
 659  *                    parameters.
 660  *
 661  * Deprecated in favor of IIO mounting matrix API.
 662  *
 663  * See inv_get_mount_matrix()
 664  */
 665 static ssize_t inv_attr_show(struct device *dev, struct device_attribute *attr,
 666                              char *buf)
 667 {
 668         struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
 669         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 670         s8 *m;
 671
 672         switch (this_attr->address) {
 673         /*
 674          * In MPU6050, the two matrix are the same because gyro and accel
 675          * are integrated in one chip
 676          */
 677         case ATTR_GYRO_MATRIX:
 678         case ATTR_ACCL_MATRIX:
 679                 m = st->plat_data.orientation;
 680
 681                 return sprintf(buf, "%d, %d, %d; %d, %d, %d; %d, %d, %d\n",
 682                         m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
 683         default:
 684                 return -EINVAL;
 685         }
 686 }
 687
 688 /**
 689  * inv_mpu6050_validate_trigger() - validate_trigger callback for invensense
 690  *                                  MPU6050 device.
 691  * @indio_dev: The IIO device
 692  * @trig: The new trigger
 693  *
 694  * Returns: 0 if the 'trig' matches the trigger registered by the MPU6050
 695  * device, -EINVAL otherwise.
 696  */
 697 static int inv_mpu6050_validate_trigger(struct iio_dev *indio_dev,
 698                                         struct iio_trigger *trig)
 699 {
 700         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 701
 702         if (st->trig != trig)
 703                 return -EINVAL;
 704
 705         return 0;
 706 }
 707
 708 static const struct iio_mount_matrix *
 709 inv_get_mount_matrix(const struct iio_dev *indio_dev,
 710                      const struct iio_chan_spec *chan)
 711 {
 712         return &((struct inv_mpu6050_state *)iio_priv(indio_dev))->orientation;
 713 }
 714
 715 static const struct iio_chan_spec_ext_info inv_ext_info[] = {
 716         IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, inv_get_mount_matrix),
 717         { },
 718 };
 719
 720 #define INV_MPU6050_CHAN(_type, _channel2, _index)                    \
 721         {                                                             \
 722                 .type = _type,                                        \
 723                 .modified = 1,                                        \
 724                 .channel2 = _channel2,                                \
 725                 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 726                 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |        \
 727                                       BIT(IIO_CHAN_INFO_CALIBBIAS),   \
 728                 .scan_index = _index,                                 \
 729                 .scan_type = {                                        \
 730                                 .sign = 's',                          \
 731                                 .realbits = 16,                       \
 732                                 .storagebits = 16,                    \
 733                                 .shift = 0,                           \
 734                                 .endianness = IIO_BE,                 \
 735                              },                                       \
 736                 .ext_info = inv_ext_info,                             \
 737         }
 738
 739 static const struct iio_chan_spec inv_mpu_channels[] = {
 740         IIO_CHAN_SOFT_TIMESTAMP(INV_MPU6050_SCAN_TIMESTAMP),
 741         /*
 742          * Note that temperature should only be via polled reading only,
 743          * not the final scan elements output.
 744          */
 745         {
 746                 .type = IIO_TEMP,
 747                 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW)
 748                                 | BIT(IIO_CHAN_INFO_OFFSET)
 749                                 | BIT(IIO_CHAN_INFO_SCALE),
 750                 .scan_index = -1,
 751         },
 752         INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X),
 753         INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y),
 754         INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z),
 755
 756         INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X),
 757         INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y),
 758         INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z),
 759 };
 760
 761 /* constant IIO attribute */
 762 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500");
 763 static IIO_CONST_ATTR(in_anglvel_scale_available,
 764                                           "0.000133090 0.000266181 0.000532362 0.001064724");
 765 static IIO_CONST_ATTR(in_accel_scale_available,
 766                                           "0.000598 0.001196 0.002392 0.004785");
 767 static IIO_DEV_ATTR_SAMP_FREQ(S_IRUGO | S_IWUSR, inv_fifo_rate_show,
 768         inv_mpu6050_fifo_rate_store);
 769
 770 /* Deprecated: kept for userspace backward compatibility. */
 771 static IIO_DEVICE_ATTR(in_gyro_matrix, S_IRUGO, inv_attr_show, NULL,
 772         ATTR_GYRO_MATRIX);
 773 static IIO_DEVICE_ATTR(in_accel_matrix, S_IRUGO, inv_attr_show, NULL,
 774         ATTR_ACCL_MATRIX);
 775
 776 static struct attribute *inv_attributes[] = {
 777         &iio_dev_attr_in_gyro_matrix.dev_attr.attr,  /* deprecated */
 778         &iio_dev_attr_in_accel_matrix.dev_attr.attr, /* deprecated */
 779         &iio_dev_attr_sampling_frequency.dev_attr.attr,
 780         &iio_const_attr_sampling_frequency_available.dev_attr.attr,
 781         &iio_const_attr_in_accel_scale_available.dev_attr.attr,
 782         &iio_const_attr_in_anglvel_scale_available.dev_attr.attr,
 783         NULL,
 784 };
 785
 786 static const struct attribute_group inv_attribute_group = {
 787         .attrs = inv_attributes
 788 };
 789
 790 static const struct iio_info mpu_info = {
 791         .driver_module = THIS_MODULE,
 792         .read_raw = &inv_mpu6050_read_raw,
 793         .write_raw = &inv_mpu6050_write_raw,
 794         .write_raw_get_fmt = &inv_write_raw_get_fmt,
 795         .attrs = &inv_attribute_group,
 796         .validate_trigger = inv_mpu6050_validate_trigger,
 797 };
 798
 799 /**
 800  *  inv_check_and_setup_chip() - check and setup chip.
 801  */
 802 static int inv_check_and_setup_chip(struct inv_mpu6050_state *st)
 803 {
 804         int result;
 805         unsigned int regval;
 806
 807         st->hw  = &hw_info[st->chip_type];
 808         st->reg = hw_info[st->chip_type].reg;
 809
 810         /* reset to make sure previous state are not there */
 811         result = regmap_write(st->map, st->reg->pwr_mgmt_1,
 812                               INV_MPU6050_BIT_H_RESET);
 813         if (result)
 814                 return result;
 815         msleep(INV_MPU6050_POWER_UP_TIME);
 816
 817         /* check chip self-identification */
 818         result = regmap_read(st->map, INV_MPU6050_REG_WHOAMI, &regval);
 819         if (result)
 820                 return result;
 821         if (regval != st->hw->whoami) {
 822                 dev_warn(regmap_get_device(st->map),
 823                                 "whoami mismatch got %#02x expected %#02hhx for %s\n",
 824                                 regval, st->hw->whoami, st->hw->name);
 825         }
 826
 827         /*
 828          * toggle power state. After reset, the sleep bit could be on
 829          * or off depending on the OTP settings. Toggling power would
 830          * make it in a definite state as well as making the hardware
 831          * state align with the software state
 832          */
 833         result = inv_mpu6050_set_power_itg(st, false);
 834         if (result)
 835                 return result;
 836         result = inv_mpu6050_set_power_itg(st, true);
 837         if (result)
 838                 return result;
 839
 840         result = inv_mpu6050_switch_engine(st, false,
 841                                            INV_MPU6050_BIT_PWR_ACCL_STBY);
 842         if (result)
 843                 return result;
 844         result = inv_mpu6050_switch_engine(st, false,
 845                                            INV_MPU6050_BIT_PWR_GYRO_STBY);
 846         if (result)
 847                 return result;
 848
 849         return 0;
 850 }
 851
 852 int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 853                 int (*inv_mpu_bus_setup)(struct iio_dev *), int chip_type)
 854 {
 855         struct inv_mpu6050_state *st;
 856         struct iio_dev *indio_dev;
 857         struct inv_mpu6050_platform_data *pdata;
 858         struct device *dev = regmap_get_device(regmap);
 859         int result;
 860
 861         indio_dev = devm_iio_device_alloc(dev, sizeof(*st));
 862         if (!indio_dev)
 863                 return -ENOMEM;
 864
 865         BUILD_BUG_ON(ARRAY_SIZE(hw_info) != INV_NUM_PARTS);
 866         if (chip_type < 0 || chip_type >= INV_NUM_PARTS) {
 867                 dev_err(dev, "Bad invensense chip_type=%d name=%s\n",
 868                                 chip_type, name);
 869                 return -ENODEV;
 870         }
 871         st = iio_priv(indio_dev);
 872         st->chip_type = chip_type;
 873         st->powerup_count = 0;
 874         st->irq = irq;
 875         st->map = regmap;
 876
 877         pdata = dev_get_platdata(dev);
 878         if (!pdata) {
 879                 result = of_iio_read_mount_matrix(dev, "mount-matrix",
 880                                                   &st->orientation);
 881                 if (result) {
 882                         dev_err(dev, "Failed to retrieve mounting matrix %d\n",
 883                                 result);
 884                         return result;
 885                 }
 886         } else {
 887                 st->plat_data = *pdata;
 888         }
 889
 890         /* power is turned on inside check chip type*/
 891         result = inv_check_and_setup_chip(st);
 892         if (result)
 893                 return result;
 894
 895         if (inv_mpu_bus_setup)
 896                 inv_mpu_bus_setup(indio_dev);
 897
 898         result = inv_mpu6050_init_config(indio_dev);
 899         if (result) {
 900                 dev_err(dev, "Could not initialize device.\n");
 901                 return result;
 902         }
 903
 904         dev_set_drvdata(dev, indio_dev);
 905         indio_dev->dev.parent = dev;
 906         /* name will be NULL when enumerated via ACPI */
 907         if (name)
 908                 indio_dev->name = name;
 909         else
 910                 indio_dev->name = dev_name(dev);
 911         indio_dev->channels = inv_mpu_channels;
 912         indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels);
 913
 914         indio_dev->info = &mpu_info;
 915         indio_dev->modes = INDIO_BUFFER_TRIGGERED;
 916
 917         result = iio_triggered_buffer_setup(indio_dev,
 918                                             inv_mpu6050_irq_handler,
 919                                             inv_mpu6050_read_fifo,
 920                                             NULL);
 921         if (result) {
 922                 dev_err(dev, "configure buffer fail %d\n", result);
 923                 return result;
 924         }
 925         result = inv_mpu6050_probe_trigger(indio_dev);
 926         if (result) {
 927                 dev_err(dev, "trigger probe fail %d\n", result);
 928                 goto out_unreg_ring;
 929         }
 930
 931         INIT_KFIFO(st->timestamps);
 932         spin_lock_init(&st->time_stamp_lock);
 933         result = iio_device_register(indio_dev);
 934         if (result) {
 935                 dev_err(dev, "IIO register fail %d\n", result);
 936                 goto out_remove_trigger;
 937         }
 938
 939         return 0;
 940
 941 out_remove_trigger:
 942         inv_mpu6050_remove_trigger(st);
 943 out_unreg_ring:
 944         iio_triggered_buffer_cleanup(indio_dev);
 945         return result;
 946 }
 947 EXPORT_SYMBOL_GPL(inv_mpu_core_probe);
 948
 949 int inv_mpu_core_remove(struct device  *dev)
 950 {
 951         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 952
 953         iio_device_unregister(indio_dev);
 954         inv_mpu6050_remove_trigger(iio_priv(indio_dev));
 955         iio_triggered_buffer_cleanup(indio_dev);
 956
 957         return 0;
 958 }
 959 EXPORT_SYMBOL_GPL(inv_mpu_core_remove);
 960
 961 #ifdef CONFIG_PM_SLEEP
 962
 963 static int inv_mpu_resume(struct device *dev)
 964 {
 965         return inv_mpu6050_set_power_itg(iio_priv(dev_get_drvdata(dev)), true);
 966 }
 967
 968 static int inv_mpu_suspend(struct device *dev)
 969 {
 970         return inv_mpu6050_set_power_itg(iio_priv(dev_get_drvdata(dev)), false);
 971 }
 972 #endif /* CONFIG_PM_SLEEP */
 973
 974 SIMPLE_DEV_PM_OPS(inv_mpu_pmops, inv_mpu_suspend, inv_mpu_resume);
 975 EXPORT_SYMBOL_GPL(inv_mpu_pmops);
 976
 977 MODULE_AUTHOR("Invensense Corporation");
 978 MODULE_DESCRIPTION("Invensense device MPU6050 driver");
 979 MODULE_LICENSE("GPL");