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