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