drivers/staging/iio/imu/inv_mpu/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
  15 /**
  16  *  @addtogroup  DRIVERS
  17  *  @brief       Hardware drivers.
  18  *
  19  *  @{
  20  *      @file    inv_mpu_core.c
  21  *      @brief   A sysfs device driver for Invensense devices
  22  *      @details This driver currently works for the
  23  *               MPU3050/MPU6050/MPU9150/MPU6500/MPU9250 devices.
  24  */
  25 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  26
  27 #include <linux/module.h>
  28 #include <linux/init.h>
  29 #include <linux/slab.h>
  30 #include <linux/i2c.h>
  31 #include <linux/err.h>
  32 #include <linux/delay.h>
  33 #include <linux/sysfs.h>
  34 #include <linux/jiffies.h>
  35 #include <linux/irq.h>
  36 #include <linux/interrupt.h>
  37 #include <linux/kfifo.h>
  38 #include <linux/poll.h>
  39 #include <linux/miscdevice.h>
  40 #include <linux/spinlock.h>
  41
  42 #include "inv_mpu_iio.h"
  43 #ifdef INV_KERNEL_3_10
  44 #include <linux/iio/sysfs.h>
  45 #else
  46 #include "sysfs.h"
  47 #endif
  48 #include "inv_counters.h"
  49
  50 s64 get_time_ns(void)
  51 {
  52         struct timespec ts;
  53         ktime_get_ts(&ts);
  54         return timespec_to_ns(&ts);
  55 }
  56
  57 static const short AKM8975_ST_Lower[3] = {-100, -100, -1000};
  58 static const short AKM8975_ST_Upper[3] = {100, 100, -300};
  59
  60 static const short AKM8972_ST_Lower[3] = {-50, -50, -500};
  61 static const short AKM8972_ST_Upper[3] = {50, 50, -100};
  62
  63 static const short AKM8963_ST_Lower[3] = {-200, -200, -3200};
  64 static const short AKM8963_ST_Upper[3] = {200, 200, -800};
  65
  66 /* This is for compatibility for power state. Should remove once HAL
  67    does not use power_state sysfs entry */
  68 static bool fake_asleep;
  69
  70 static const struct inv_hw_s hw_info[INV_NUM_PARTS] = {
  71         {119, "ITG3500"},
  72         { 63, "MPU3050"},
  73         {117, "MPU6050"},
  74         {118, "MPU9150"},
  75         {119, "MPU6500"},
  76         {118, "MPU9250"},
  77         {128, "MPU6515"},
  78 };
  79
  80 static void inv_setup_reg(struct inv_reg_map_s *reg)
  81 {
  82         reg->sample_rate_div    = REG_SAMPLE_RATE_DIV;
  83         reg->lpf                = REG_CONFIG;
  84         reg->bank_sel           = REG_BANK_SEL;
  85         reg->user_ctrl          = REG_USER_CTRL;
  86         reg->fifo_en            = REG_FIFO_EN;
  87         reg->gyro_config        = REG_GYRO_CONFIG;
  88         reg->accl_config        = REG_ACCEL_CONFIG;
  89         reg->fifo_count_h       = REG_FIFO_COUNT_H;
  90         reg->fifo_r_w           = REG_FIFO_R_W;
  91         reg->raw_gyro           = REG_RAW_GYRO;
  92         reg->raw_accl           = REG_RAW_ACCEL;
  93         reg->temperature        = REG_TEMPERATURE;
  94         reg->int_enable         = REG_INT_ENABLE;
  95         reg->int_status         = REG_INT_STATUS;
  96         reg->pwr_mgmt_1         = REG_PWR_MGMT_1;
  97         reg->pwr_mgmt_2         = REG_PWR_MGMT_2;
  98         reg->mem_start_addr     = REG_MEM_START_ADDR;
  99         reg->mem_r_w            = REG_MEM_RW;
 100         reg->prgm_strt_addrh    = REG_PRGM_STRT_ADDRH;
 101 };
 102
 103 /**
 104  *  inv_i2c_read() - Read one or more bytes from the device registers.
 105  *  @st:        Device driver instance.
 106  *  @reg:       First device register to be read from.
 107  *  @length:    Number of bytes to read.
 108  *  @data:      Data read from device.
 109  *  NOTE:This is not re-implementation of i2c_smbus_read because i2c
 110  *       address could be specified in this case. We could have two different
 111  *       i2c address due to secondary i2c interface.
 112  */
 113 int inv_i2c_read_base(struct inv_mpu_iio_s *st, u16 i2c_addr,
 114         u8 reg, u16 length, u8 *data)
 115 {
 116         struct i2c_msg msgs[2];
 117         int res;
 118
 119         if (!data)
 120                 return -EINVAL;
 121
 122         msgs[0].addr = i2c_addr;
 123         msgs[0].flags = 0;      /* write */
 124         msgs[0].buf = &reg;
 125         msgs[0].len = 1;
 126         /* msgs[0].scl_rate = 200*1000; */
 127
 128         msgs[1].addr = i2c_addr;
 129         msgs[1].flags = I2C_M_RD;
 130         msgs[1].buf = data;
 131         msgs[1].len = length;
 132         /* msgs[1].scl_rate = 200*1000; */
 133
 134         res = i2c_transfer(st->sl_handle, msgs, 2);
 135
 136         if (res < 2) {
 137                 if (res >= 0)
 138                         res = -EIO;
 139         } else
 140                 res = 0;
 141
 142         INV_I2C_INC_MPUWRITE(3);
 143         INV_I2C_INC_MPUREAD(length);
 144         {
 145                 char *read = 0;
 146                 pr_debug("%s RD%02X%02X%02X -> %s%s\n", st->hw->name,
 147                          i2c_addr, reg, length,
 148                          wr_pr_debug_begin(data, length, read),
 149                          wr_pr_debug_end(read));
 150         }
 151         return res;
 152 }
 153
 154 /**
 155  *  inv_i2c_single_write() - Write a byte to a device register.
 156  *  @st:        Device driver instance.
 157  *  @reg:       Device register to be written to.
 158  *  @data:      Byte to write to device.
 159  *  NOTE:This is not re-implementation of i2c_smbus_write because i2c
 160  *       address could be specified in this case. We could have two different
 161  *       i2c address due to secondary i2c interface.
 162  */
 163 int inv_i2c_single_write_base(struct inv_mpu_iio_s *st,
 164         u16 i2c_addr, u8 reg, u8 data)
 165 {
 166         u8 tmp[2];
 167         struct i2c_msg msg;
 168         int res;
 169         tmp[0] = reg;
 170         tmp[1] = data;
 171
 172         msg.addr = i2c_addr;
 173         msg.flags = 0;  /* write */
 174         msg.buf = tmp;
 175         msg.len = 2;
 176         /* msg.scl_rate = 200*1000; */
 177
 178         pr_debug("%s WR%02X%02X%02X\n", st->hw->name, i2c_addr, reg, data);
 179         INV_I2C_INC_MPUWRITE(3);
 180
 181         res = i2c_transfer(st->sl_handle, &msg, 1);
 182         if (res < 1) {
 183                 if (res == 0)
 184                         res = -EIO;
 185                 return res;
 186         } else
 187                 return 0;
 188 }
 189
 190 static int inv_switch_engine(struct inv_mpu_iio_s *st, bool en, u32 mask)
 191 {
 192         struct inv_reg_map_s *reg;
 193         u8 data, mgmt_1;
 194         int result;
 195         reg = &st->reg;
 196         /* switch clock needs to be careful. Only when gyro is on, can
 197            clock source be switched to gyro. Otherwise, it must be set to
 198            internal clock */
 199         if (BIT_PWR_GYRO_STBY == mask) {
 200                 result = inv_i2c_read(st, reg->pwr_mgmt_1, 1, &mgmt_1);
 201                 if (result)
 202                         return result;
 203
 204                 mgmt_1 &= ~BIT_CLK_MASK;
 205         }
 206
 207         if ((BIT_PWR_GYRO_STBY == mask) && (!en)) {
 208                 /* turning off gyro requires switch to internal clock first.
 209                    Then turn off gyro engine */
 210                 mgmt_1 |= INV_CLK_INTERNAL;
 211                 result = inv_i2c_single_write(st, reg->pwr_mgmt_1,
 212                                                 mgmt_1);
 213                 if (result)
 214                         return result;
 215         }
 216
 217         result = inv_i2c_read(st, reg->pwr_mgmt_2, 1, &data);
 218         if (result)
 219                 return result;
 220         if (en)
 221                 data &= (~mask);
 222         else
 223                 data |= mask;
 224         result = inv_i2c_single_write(st, reg->pwr_mgmt_2, data);
 225         if (result)
 226                 return result;
 227
 228         if ((BIT_PWR_GYRO_STBY == mask) && en) {
 229                 /* only gyro on needs sensor up time */
 230                 msleep(SENSOR_UP_TIME);
 231                 /* after gyro is on & stable, switch internal clock to PLL */
 232                 mgmt_1 |= INV_CLK_PLL;
 233                 result = inv_i2c_single_write(st, reg->pwr_mgmt_1,
 234                                                 mgmt_1);
 235                 if (result)
 236                         return result;
 237         }
 238         if ((BIT_PWR_ACCL_STBY == mask) && en)
 239                 msleep(REG_UP_TIME);
 240
 241         return 0;
 242 }
 243
 244 /**
 245  *  inv_lpa_freq() - store current low power frequency setting.
 246  */
 247 static int inv_lpa_freq(struct inv_mpu_iio_s *st, int lpa_freq)
 248 {
 249         unsigned long result;
 250         u8 d;
 251         const u8 mpu6500_lpa_mapping[] = {2, 4, 6, 7};
 252
 253         if (lpa_freq > MAX_LPA_FREQ_PARAM)
 254                 return -EINVAL;
 255
 256         if (INV_MPU6500 == st->chip_type) {
 257                 d = mpu6500_lpa_mapping[lpa_freq];
 258                 result = inv_i2c_single_write(st, REG_6500_LP_ACCEL_ODR, d);
 259                 if (result)
 260                         return result;
 261         }
 262         st->chip_config.lpa_freq = lpa_freq;
 263
 264         return 0;
 265 }
 266
 267 static int set_power_itg(struct inv_mpu_iio_s *st, bool power_on)
 268 {
 269         struct inv_reg_map_s *reg;
 270         u8 data;
 271         int result;
 272
 273         if ((!power_on) == st->chip_config.is_asleep)
 274                 return 0;
 275         reg = &st->reg;
 276         if (power_on)
 277                 data = 0;
 278         else
 279                 data = BIT_SLEEP;
 280         result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data);
 281         if (result)
 282                 return result;
 283
 284         if (power_on) {
 285                 if (INV_MPU6500 == st->chip_type)
 286                         msleep(POWER_UP_TIME);
 287                 else
 288                         msleep(REG_UP_TIME);
 289         }
 290
 291         st->chip_config.is_asleep = !power_on;
 292
 293         return 0;
 294 }
 295
 296 /**
 297  *  inv_init_config() - Initialize hardware, disable FIFO.
 298  *  @indio_dev: Device driver instance.
 299  *  Initial configuration:
 300  *  FSR: +/- 2000DPS
 301  *  DLPF: 42Hz
 302  *  FIFO rate: 50Hz
 303  */
 304 static int inv_init_config(struct iio_dev *indio_dev)
 305 {
 306         struct inv_reg_map_s *reg;
 307         int result;
 308         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 309
 310
 311         reg = &st->reg;
 312
 313 #if 0
 314         /* set int latch en */
 315         result = inv_i2c_single_write(st, REG_INT_PIN_CFG, 0x20);
 316         if (result)
 317                 return result;
 318 #endif
 319         result = inv_i2c_single_write(st, reg->gyro_config,
 320                 INV_FSR_2000DPS << GYRO_CONFIG_FSR_SHIFT);
 321         if (result)
 322                 return result;
 323
 324         st->chip_config.fsr = INV_FSR_2000DPS;
 325
 326         result = inv_i2c_single_write(st, reg->lpf, INV_FILTER_42HZ);
 327         if (result)
 328                 return result;
 329         st->chip_config.lpf = INV_FILTER_42HZ;
 330
 331         result = inv_i2c_single_write(st, reg->sample_rate_div,
 332                                         ONE_K_HZ / INIT_FIFO_RATE - 1);
 333         if (result)
 334                 return result;
 335         st->chip_config.fifo_rate = INIT_FIFO_RATE;
 336         st->chip_config.new_fifo_rate = INIT_FIFO_RATE;
 337         st->irq_dur_ns            = INIT_DUR_TIME;
 338         st->chip_config.prog_start_addr = DMP_START_ADDR;
 339         st->chip_config.dmp_output_rate = INIT_DMP_OUTPUT_RATE;
 340         st->self_test.samples = INIT_ST_SAMPLES;
 341         st->self_test.threshold = INIT_ST_THRESHOLD;
 342         if (INV_ITG3500 != st->chip_type) {
 343                 st->chip_config.accl_fs = INV_FS_02G;
 344                 result = inv_i2c_single_write(st, reg->accl_config,
 345                         (INV_FS_02G << ACCL_CONFIG_FSR_SHIFT));
 346                 if (result)
 347                         return result;
 348                 st->tap.time = INIT_TAP_TIME;
 349                 st->tap.thresh = INIT_TAP_THRESHOLD;
 350                 st->tap.min_count = INIT_TAP_MIN_COUNT;
 351                 st->smd.threshold = MPU_INIT_SMD_THLD;
 352                 st->smd.delay     = MPU_INIT_SMD_DELAY_THLD;
 353                 st->smd.delay2    = MPU_INIT_SMD_DELAY2_THLD;
 354
 355                 result = inv_i2c_single_write(st, REG_ACCEL_MOT_DUR,
 356                                                 INIT_MOT_DUR);
 357                 if (result)
 358                         return result;
 359                 st->mot_int.mot_dur = INIT_MOT_DUR;
 360
 361                 result = inv_i2c_single_write(st, REG_ACCEL_MOT_THR,
 362                                                 INIT_MOT_THR);
 363                 if (result)
 364                         return result;
 365                 st->mot_int.mot_thr = INIT_MOT_THR;
 366         }
 367
 368         return 0;
 369 }
 370
 371 /**
 372  *  inv_compass_scale_show() - show compass scale.
 373  */
 374 static int inv_compass_scale_show(struct inv_mpu_iio_s *st, int *scale)
 375 {
 376         if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id)
 377                 *scale = DATA_AKM8975_SCALE;
 378         else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id)
 379                 *scale = DATA_AKM8972_SCALE;
 380         else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id)
 381                 if (st->compass_scale)
 382                         *scale = DATA_AKM8963_SCALE1;
 383                 else
 384                         *scale = DATA_AKM8963_SCALE0;
 385         else
 386                 return -EINVAL;
 387
 388         return IIO_VAL_INT;
 389 }
 390
 391 /**
 392  *  inv_sensor_show() - Read gyro/accel data directly from registers.
 393  */
 394 static int inv_sensor_show(struct inv_mpu_iio_s  *st, int reg, int axis,
 395                                         int *val)
 396 {
 397         int ind, result;
 398         u8 d[2];
 399
 400         ind = (axis - IIO_MOD_X) * 2;
 401         result = i2c_smbus_read_i2c_block_data(st->client,
 402                                                reg + ind, 2, d);
 403         if (result != 2)
 404                 return -EINVAL;
 405         *val = (short)be16_to_cpup((__be16 *)(d));
 406
 407         return IIO_VAL_INT;
 408 }
 409
 410 /**
 411  *  mpu_read_raw() - read raw method.
 412  */
 413 static int mpu_read_raw(struct iio_dev *indio_dev,
 414                         struct iio_chan_spec const *chan,
 415                         int *val, int *val2, long mask)
 416 {
 417         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
 418         int result;
 419
 420         switch (mask) {
 421         case 0:
 422                 /* if enabled, power is on already */
 423                 if (!st->chip_config.enable)
 424                         return -EBUSY;
 425                 switch (chan->type) {
 426                 case IIO_ANGL_VEL:
 427                         if (!st->chip_config.gyro_enable)
 428                                 return -EPERM;
 429                         return inv_sensor_show(st, st->reg.raw_gyro,
 430                                                 chan->channel2, val);
 431                 case IIO_ACCEL:
 432                         if (!st->chip_config.accl_enable)
 433                                 return -EPERM;
 434                         return inv_sensor_show(st, st->reg.raw_accl,
 435                                                 chan->channel2, val);
 436                 case IIO_MAGN:
 437                         if (!st->chip_config.compass_enable)
 438                                 return -EPERM;
 439                         *val = st->raw_compass[chan->channel2 - IIO_MOD_X];
 440                         return IIO_VAL_INT;
 441                 case IIO_QUATERNION:
 442                         if (!(st->chip_config.dmp_on
 443                                 && st->chip_config.quaternion_on))
 444                                 return -EPERM;
 445                         if (IIO_MOD_R == chan->channel2)
 446                                 *val = st->raw_quaternion[0];
 447                         else
 448                                 *val = st->raw_quaternion[chan->channel2 -
 449                                                           IIO_MOD_X + 1];
 450                         return IIO_VAL_INT;
 451                 default:
 452                         return -EINVAL;
 453                 }
 454         case IIO_CHAN_INFO_SCALE:
 455                 switch (chan->type) {
 456                 case IIO_ANGL_VEL:
 457                 {
 458                         const s16 gyro_scale[] = {250, 500, 1000, 2000};
 459
 460                         *val = gyro_scale[st->chip_config.fsr];
 461
 462                         return IIO_VAL_INT;
 463                 }
 464                 case IIO_ACCEL:
 465                 {
 466                         const s16 accel_scale[] = {2, 4, 8, 16};
 467                         *val = accel_scale[st->chip_config.accl_fs] *
 468                                         st->chip_info.multi;
 469                         return IIO_VAL_INT;
 470                 }
 471                 case IIO_MAGN:
 472                         return inv_compass_scale_show(st, val);
 473                 default:
 474                         return -EINVAL;
 475                 }
 476         case IIO_CHAN_INFO_CALIBBIAS:
 477                 if (st->chip_config.self_test_run_once == 0) {
 478                         /* This can only be run when enable is zero */
 479                         if (st->chip_config.enable)
 480                                 return -EBUSY;
 481                         mutex_lock(&indio_dev->mlock);
 482
 483                         result = inv_power_up_self_test(st);
 484                         if (result)
 485                                 goto error_info_calibbias;
 486                         result = inv_do_test(st, 0,  st->gyro_bias,
 487                                 st->accel_bias);
 488                         if (result)
 489                                 goto error_info_calibbias;
 490                         st->chip_config.self_test_run_once = 1;
 491 error_info_calibbias:
 492                         /* Reset Accel and Gyro full scale range
 493                            back to default value */
 494                         inv_recover_setting(st);
 495                         mutex_unlock(&indio_dev->mlock);
 496                 }
 497
 498                 switch (chan->type) {
 499                 case IIO_ANGL_VEL:
 500                         *val = st->gyro_bias[chan->channel2 - IIO_MOD_X];
 501                         return IIO_VAL_INT;
 502                 case IIO_ACCEL:
 503                         *val = st->accel_bias[chan->channel2 - IIO_MOD_X] *
 504                                         st->chip_info.multi;
 505                         return IIO_VAL_INT;
 506                 default:
 507                         return -EINVAL;
 508                 }
 509         case IIO_CHAN_INFO_OFFSET:
 510                 switch (chan->type) {
 511                 case IIO_ACCEL:
 512                         *val = st->input_accel_bias[chan->channel2 - IIO_MOD_X];
 513                         return IIO_VAL_INT;
 514                 default:
 515                         return -EINVAL;
 516                 }
 517         default:
 518                 return -EINVAL;
 519         }
 520 }
 521
 522 /**
 523  *  inv_write_fsr() - Configure the gyro's scale range.
 524  */
 525 static int inv_write_fsr(struct inv_mpu_iio_s *st, int fsr)
 526 {
 527         struct inv_reg_map_s *reg;
 528         int result;
 529         reg = &st->reg;
 530         if ((fsr < 0) || (fsr > MAX_GYRO_FS_PARAM))
 531                 return -EINVAL;
 532         if (fsr == st->chip_config.fsr)
 533                 return 0;
 534
 535         if (INV_MPU3050 == st->chip_type)
 536                 result = inv_i2c_single_write(st, reg->lpf,
 537                         (fsr << GYRO_CONFIG_FSR_SHIFT) | st->chip_config.lpf);
 538         else
 539                 result = inv_i2c_single_write(st, reg->gyro_config,
 540                         fsr << GYRO_CONFIG_FSR_SHIFT);
 541
 542         if (result)
 543                 return result;
 544         st->chip_config.fsr = fsr;
 545
 546         return 0;
 547 }
 548
 549 /**
 550  *  inv_write_accel_fs() - Configure the accelerometer's scale range.
 551  */
 552 static int inv_write_accel_fs(struct inv_mpu_iio_s *st, int fs)
 553 {
 554         int result;
 555         struct inv_reg_map_s *reg;
 556
 557         reg = &st->reg;
 558         if (fs < 0 || fs > MAX_ACCL_FS_PARAM)
 559                 return -EINVAL;
 560         if (fs == st->chip_config.accl_fs)
 561                 return 0;
 562         if (INV_MPU3050 == st->chip_type)
 563                 result = st->mpu_slave->set_fs(st, fs);
 564         else
 565                 result = inv_i2c_single_write(st, reg->accl_config,
 566                                 (fs << ACCL_CONFIG_FSR_SHIFT));
 567         if (result)
 568                 return result;
 569
 570         st->chip_config.accl_fs = fs;
 571
 572         return 0;
 573 }
 574
 575 /**
 576  *  inv_write_compass_scale() - Configure the compass's scale range.
 577  */
 578 static int inv_write_compass_scale(struct inv_mpu_iio_s  *st, int data)
 579 {
 580         char d, en;
 581         int result;
 582         if (COMPASS_ID_AK8963 != st->plat_data.sec_slave_id)
 583                 return 0;
 584         en = !!data;
 585         if (st->compass_scale == en)
 586                 return 0;
 587         d = (DATA_AKM_MODE_SM | (st->compass_scale << AKM8963_SCALE_SHIFT));
 588         result = inv_i2c_single_write(st, REG_I2C_SLV1_DO, d);
 589         if (result)
 590                 return result;
 591         st->compass_scale = en;
 592
 593         return 0;
 594 }
 595
 596 /**
 597  *  mpu_write_raw() - write raw method.
 598  */
 599 static int mpu_write_raw(struct iio_dev *indio_dev,
 600                                struct iio_chan_spec const *chan,
 601                                int val,
 602                                int val2,
 603                                long mask) {
 604         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
 605         int result;
 606
 607         if (st->chip_config.enable)
 608                 return -EBUSY;
 609         mutex_lock(&indio_dev->mlock);
 610         result = st->set_power_state(st, true);
 611         if (result) {
 612                 mutex_unlock(&indio_dev->mlock);
 613                 return result;
 614         }
 615
 616         switch (mask) {
 617         case IIO_CHAN_INFO_SCALE:
 618                 switch (chan->type) {
 619                 case IIO_ANGL_VEL:
 620                         result = inv_write_fsr(st, val);
 621                         break;
 622                 case IIO_ACCEL:
 623                         result = inv_write_accel_fs(st, val);
 624                         break;
 625                 case IIO_MAGN:
 626                         result = inv_write_compass_scale(st, val);
 627                         break;
 628                 default:
 629                         result = -EINVAL;
 630                         break;
 631                 }
 632                 break;
 633         case IIO_CHAN_INFO_OFFSET:
 634                 switch (chan->type) {
 635                 case IIO_ACCEL:
 636                         if (!st->chip_config.firmware_loaded) {
 637                                 result = -EPERM;
 638                                 goto error_write_raw;
 639                         }
 640                         result = inv_set_accel_bias_dmp(st);
 641                         if (result)
 642                                 goto error_write_raw;
 643                         st->input_accel_bias[chan->channel2 - IIO_MOD_X] = val;
 644                         result = 0;
 645                         break;
 646                 default:
 647                         result = -EINVAL;
 648                         break;
 649                 }
 650                 break;
 651         default:
 652                 result = -EINVAL;
 653                 break;
 654         }
 655
 656 error_write_raw:
 657         result |= st->set_power_state(st, false);
 658         mutex_unlock(&indio_dev->mlock);
 659
 660         return result;
 661 }
 662
 663 /**
 664  *  inv_fifo_rate_store() - Set fifo rate.
 665  */
 666 static int inv_fifo_rate_store(struct inv_mpu_iio_s *st, int fifo_rate)
 667 {
 668         if ((fifo_rate < MIN_FIFO_RATE) || (fifo_rate > MAX_FIFO_RATE))
 669                 return -EINVAL;
 670
 671         if (st->chip_config.has_compass) {
 672                 st->compass_divider = COMPASS_RATE_SCALE * fifo_rate /
 673                                         ONE_K_HZ;
 674                 if (st->compass_divider > 0)
 675                         st->compass_divider -= 1;
 676                 st->compass_counter = 0;
 677         }
 678         st->irq_dur_ns = (ONE_K_HZ / fifo_rate) * NSEC_PER_MSEC;
 679         st->chip_config.new_fifo_rate = fifo_rate;
 680
 681         return 0;
 682 }
 683
 684 /**
 685  *  inv_reg_dump_show() - Register dump for testing.
 686  */
 687 static ssize_t inv_reg_dump_show(struct device *dev,
 688         struct device_attribute *attr, char *buf)
 689 {
 690         int ii;
 691         char data;
 692         ssize_t bytes_printed = 0;
 693         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 694         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 695
 696         mutex_lock(&indio_dev->mlock);
 697         if (!st->chip_config.enable)
 698                 st->set_power_state(st, true);
 699         for (ii = 0; ii < st->hw->num_reg; ii++) {
 700                 /* don't read fifo r/w register */
 701                 if (ii == st->reg.fifo_r_w)
 702                         data = 0;
 703                 else
 704                         inv_i2c_read(st, ii, 1, &data);
 705                 bytes_printed += sprintf(buf + bytes_printed, "%#2x: %#2x\n",
 706                                          ii, data);
 707         }
 708         if (!st->chip_config.enable)
 709                 st->set_power_state(st, false);
 710         mutex_unlock(&indio_dev->mlock);
 711
 712         return bytes_printed;
 713 }
 714
 715 int write_be32_key_to_mem(struct inv_mpu_iio_s *st,
 716                                         u32 data, int key)
 717 {
 718         cpu_to_be32s(&data);
 719         return mem_w_key(key, sizeof(data), (u8 *)&data);
 720 }
 721
 722 /**
 723  * inv_quaternion_on() -  calling this function will store
 724  *                                 current quaternion on
 725  */
 726 static int inv_quaternion_on(struct inv_mpu_iio_s *st,
 727                                  struct iio_buffer *ring, bool en)
 728 {
 729         st->chip_config.quaternion_on = en;
 730         if (!en) {
 731                 clear_bit(INV_MPU_SCAN_QUAT_R, ring->scan_mask);
 732                 clear_bit(INV_MPU_SCAN_QUAT_X, ring->scan_mask);
 733                 clear_bit(INV_MPU_SCAN_QUAT_Y, ring->scan_mask);
 734                 clear_bit(INV_MPU_SCAN_QUAT_Z, ring->scan_mask);
 735         }
 736
 737         return 0;
 738 }
 739
 740 /**
 741  * inv_dmp_attr_store() -  calling this function will store current
 742  *                        dmp parameter settings
 743  */
 744 static ssize_t inv_dmp_attr_store(struct device *dev,
 745         struct device_attribute *attr, const char *buf, size_t count)
 746 {
 747         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 748         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 749         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 750         int result, data;
 751
 752         mutex_lock(&indio_dev->mlock);
 753         if (st->chip_config.enable) {
 754                 result = -EBUSY;
 755                 goto dmp_attr_store_fail;
 756         }
 757         if (this_attr->address <= ATTR_DMP_DISPLAY_ORIENTATION_ON) {
 758                 if (!st->chip_config.firmware_loaded) {
 759                         result = -EINVAL;
 760                         goto dmp_attr_store_fail;
 761                 }
 762                 result = st->set_power_state(st, true);
 763                 if (result)
 764                         goto dmp_attr_store_fail;
 765         }
 766
 767         result = kstrtoint(buf, 10, &data);
 768         if (result)
 769                 goto dmp_attr_store_fail;
 770         switch (this_attr->address) {
 771         case ATTR_DMP_SMD_ENABLE:
 772         {
 773                 u8 on[] = {0, 1};
 774                 u8 off[] = {0, 0};
 775                 u8 *d;
 776                 if (data)
 777                         d = on;
 778                 else
 779                         d = off;
 780                 result = mem_w_key(KEY_SMD_ENABLE, ARRAY_SIZE(on), d);
 781                 if (result)
 782                         goto dmp_attr_store_fail;
 783                 st->chip_config.smd_enable = !!data;
 784         }
 785                 break;
 786         case ATTR_DMP_SMD_THLD:
 787                 if (data < 0 || data > SHRT_MAX)
 788                         goto dmp_attr_store_fail;
 789                 result = write_be32_key_to_mem(st, data << 16,
 790                                                 KEY_SMD_ACCEL_THLD);
 791                 if (result)
 792                         goto dmp_attr_store_fail;
 793                 st->smd.threshold = data;
 794                 break;
 795         case ATTR_DMP_SMD_DELAY_THLD:
 796                 if (data < 0 || data > INT_MAX / MPU_DEFAULT_DMP_FREQ)
 797                         goto dmp_attr_store_fail;
 798                 result = write_be32_key_to_mem(st, data * MPU_DEFAULT_DMP_FREQ,
 799                                                 KEY_SMD_DELAY_THLD);
 800                 if (result)
 801                         goto dmp_attr_store_fail;
 802                 st->smd.delay = data;
 803                 break;
 804         case ATTR_DMP_SMD_DELAY_THLD2:
 805                 if (data < 0 || data > INT_MAX / MPU_DEFAULT_DMP_FREQ)
 806                         goto dmp_attr_store_fail;
 807                 result = write_be32_key_to_mem(st, data * MPU_DEFAULT_DMP_FREQ,
 808                                                 KEY_SMD_DELAY2_THLD);
 809                 if (result)
 810                         goto dmp_attr_store_fail;
 811                 st->smd.delay2 = data;
 812                 break;
 813         case ATTR_DMP_TAP_ON:
 814                 result = inv_enable_tap_dmp(st, !!data);
 815                 if (result)
 816                         goto dmp_attr_store_fail;
 817                 st->chip_config.tap_on = !!data;
 818                 break;
 819         case ATTR_DMP_TAP_THRESHOLD: {
 820                 const char ax[] = {INV_TAP_AXIS_X, INV_TAP_AXIS_Y,
 821                                                         INV_TAP_AXIS_Z};
 822                 int i;
 823                 if (data < 0 || data > USHRT_MAX) {
 824                         result = -EINVAL;
 825                         goto dmp_attr_store_fail;
 826                 }
 827                 for (i = 0; i < ARRAY_SIZE(ax); i++) {
 828                         result = inv_set_tap_threshold_dmp(st, ax[i], data);
 829                         if (result)
 830                                 goto dmp_attr_store_fail;
 831                 }
 832                 st->tap.thresh = data;
 833                 break;
 834         }
 835         case ATTR_DMP_TAP_MIN_COUNT:
 836                 if (data < 0 || data > USHRT_MAX) {
 837                         result = -EINVAL;
 838                         goto dmp_attr_store_fail;
 839                 }
 840                 result = inv_set_min_taps_dmp(st, data);
 841                 if (result)
 842                         goto dmp_attr_store_fail;
 843                 st->tap.min_count = data;
 844                 break;
 845         case ATTR_DMP_TAP_TIME:
 846                 if (data < 0 || data > USHRT_MAX) {
 847                         result = -EINVAL;
 848                         goto dmp_attr_store_fail;
 849                 }
 850                 result = inv_set_tap_time_dmp(st, data);
 851                 if (result)
 852                         goto dmp_attr_store_fail;
 853                 st->tap.time = data;
 854                 break;
 855         case ATTR_DMP_DISPLAY_ORIENTATION_ON:
 856                 result = inv_set_display_orient_interrupt_dmp(st, !!data);
 857                 if (result)
 858                         goto dmp_attr_store_fail;
 859                 st->chip_config.display_orient_on = !!data;
 860                 break;
 861         /* from here, power of chip is not turned on */
 862         case ATTR_DMP_ON:
 863                 st->chip_config.dmp_on = !!data;
 864                 break;
 865         case ATTR_DMP_INT_ON:
 866                 st->chip_config.dmp_int_on = !!data;
 867                 break;
 868         case ATTR_DMP_EVENT_INT_ON:
 869                 st->chip_config.dmp_event_int_on = !!data;
 870                 break;
 871         case ATTR_DMP_OUTPUT_RATE:
 872                 if (data <= 0 || data > MAX_DMP_OUTPUT_RATE) {
 873                         result = -EINVAL;
 874                         goto dmp_attr_store_fail;
 875                 }
 876                 st->chip_config.dmp_output_rate = data;
 877                 if (st->chip_config.has_compass) {
 878                         st->compass_dmp_divider = COMPASS_RATE_SCALE * data /
 879                                                         ONE_K_HZ;
 880                         if (st->compass_dmp_divider > 0)
 881                                 st->compass_dmp_divider -= 1;
 882                         st->compass_counter = 0;
 883                 }
 884                 break;
 885         case ATTR_DMP_QUATERNION_ON:
 886                 result = inv_quaternion_on(st, indio_dev->buffer, !!data);
 887                 break;
 888 #ifdef CONFIG_INV_TESTING
 889         case ATTR_DEBUG_SMD_ENABLE_TESTP1:
 890         {
 891                 u8 d[] = {0x42};
 892                 result = st->set_power_state(st, true);
 893                 if (result)
 894                         goto dmp_attr_store_fail;
 895                 result = mem_w_key(KEY_SMD_ENABLE_TESTPT1, ARRAY_SIZE(d), d);
 896                 if (result)
 897                         goto dmp_attr_store_fail;
 898         }
 899                 break;
 900         case ATTR_DEBUG_SMD_ENABLE_TESTP2:
 901         {
 902                 u8 d[] = {0x42};
 903                 result = st->set_power_state(st, true);
 904                 if (result)
 905                         goto dmp_attr_store_fail;
 906                 result = mem_w_key(KEY_SMD_ENABLE_TESTPT2, ARRAY_SIZE(d), d);
 907                 if (result)
 908                         goto dmp_attr_store_fail;
 909         }
 910                 break;
 911 #endif
 912         default:
 913                 result = -EINVAL;
 914                 goto dmp_attr_store_fail;
 915         }
 916
 917 dmp_attr_store_fail:
 918         if ((this_attr->address <= ATTR_DMP_DISPLAY_ORIENTATION_ON) &&
 919                                         (!st->chip_config.enable))
 920                 result |= st->set_power_state(st, false);
 921         mutex_unlock(&indio_dev->mlock);
 922         if (result)
 923                 return result;
 924
 925         return count;
 926 }
 927
 928 /**
 929  * inv_attr_show() -  calling this function will show current
 930  *                        dmp parameters.
 931  */
 932 static ssize_t inv_attr_show(struct device *dev,
 933         struct device_attribute *attr, char *buf)
 934 {
 935         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 936         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 937         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 938         int result;
 939         s8 *m;
 940
 941         switch (this_attr->address) {
 942         case ATTR_DMP_SMD_ENABLE:
 943                 return sprintf(buf, "%d\n", st->chip_config.smd_enable);
 944         case ATTR_DMP_SMD_THLD:
 945                 return sprintf(buf, "%d\n", st->smd.threshold);
 946         case ATTR_DMP_SMD_DELAY_THLD:
 947                 return sprintf(buf, "%d\n", st->smd.delay);
 948         case ATTR_DMP_SMD_DELAY_THLD2:
 949                 return sprintf(buf, "%d\n", st->smd.delay2);
 950         case ATTR_DMP_TAP_ON:
 951                 return sprintf(buf, "%d\n", st->chip_config.tap_on);
 952         case ATTR_DMP_TAP_THRESHOLD:
 953                 return sprintf(buf, "%d\n", st->tap.thresh);
 954         case ATTR_DMP_TAP_MIN_COUNT:
 955                 return sprintf(buf, "%d\n", st->tap.min_count);
 956         case ATTR_DMP_TAP_TIME:
 957                 return sprintf(buf, "%d\n", st->tap.time);
 958         case ATTR_DMP_DISPLAY_ORIENTATION_ON:
 959                 return sprintf(buf, "%d\n",
 960                         st->chip_config.display_orient_on);
 961
 962         case ATTR_DMP_ON:
 963                 return sprintf(buf, "%d\n", st->chip_config.dmp_on);
 964         case ATTR_DMP_INT_ON:
 965                 return sprintf(buf, "%d\n", st->chip_config.dmp_int_on);
 966         case ATTR_DMP_EVENT_INT_ON:
 967                 return sprintf(buf, "%d\n", st->chip_config.dmp_event_int_on);
 968         case ATTR_DMP_OUTPUT_RATE:
 969                 return sprintf(buf, "%d\n",
 970                                 st->chip_config.dmp_output_rate);
 971         case ATTR_DMP_QUATERNION_ON:
 972                 return sprintf(buf, "%d\n", st->chip_config.quaternion_on);
 973
 974         case ATTR_MOTION_LPA_ON:
 975                 return sprintf(buf, "%d\n", st->mot_int.mot_on);
 976         case ATTR_MOTION_LPA_FREQ:{
 977                 const char *f[] = {"1.25", "5", "20", "40"};
 978                 return sprintf(buf, "%s\n", f[st->chip_config.lpa_freq]);
 979         }
 980         case ATTR_MOTION_LPA_THRESHOLD:
 981                 return sprintf(buf, "%d\n", st->mot_int.mot_thr);
 982
 983         case ATTR_SELF_TEST_SAMPLES:
 984                 return sprintf(buf, "%d\n", st->self_test.samples);
 985         case ATTR_SELF_TEST_THRESHOLD:
 986                 return sprintf(buf, "%d\n", st->self_test.threshold);
 987         case ATTR_GYRO_ENABLE:
 988                 return sprintf(buf, "%d\n", st->chip_config.gyro_enable);
 989         case ATTR_ACCL_ENABLE:
 990                 return sprintf(buf, "%d\n", st->chip_config.accl_enable);
 991         case ATTR_COMPASS_ENABLE:
 992                 return sprintf(buf, "%d\n", st->chip_config.compass_enable);
 993         case ATTR_POWER_STATE:
 994                 return sprintf(buf, "%d\n", !fake_asleep);
 995         case ATTR_FIRMWARE_LOADED:
 996                 return sprintf(buf, "%d\n", st->chip_config.firmware_loaded);
 997         case ATTR_SAMPLING_FREQ:
 998                 return sprintf(buf, "%d\n", st->chip_config.new_fifo_rate);
 999
1000         case ATTR_SELF_TEST:
1001                 if (st->chip_config.enable)
1002                         return -EBUSY;
1003                 mutex_lock(&indio_dev->mlock);
1004                 if (INV_MPU3050 == st->chip_type)
1005                         result = 1;
1006                 else
1007                         result = inv_hw_self_test(st);
1008                 mutex_unlock(&indio_dev->mlock);
1009                 return sprintf(buf, "%d\n", result);
1010
1011         case ATTR_GYRO_MATRIX:
1012                 m = st->plat_data.orientation;
1013                 return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
1014                         m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
1015         case ATTR_ACCL_MATRIX:
1016                 if (st->plat_data.sec_slave_type == SECONDARY_SLAVE_TYPE_ACCEL)
1017                         m = st->plat_data.secondary_orientation;
1018                 else
1019                         m = st->plat_data.orientation;
1020                 return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
1021                         m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
1022         case ATTR_COMPASS_MATRIX:
1023                 if (st->plat_data.sec_slave_type ==
1024                                 SECONDARY_SLAVE_TYPE_COMPASS)
1025                         m = st->plat_data.secondary_orientation;
1026                 else
1027                         return -ENODEV;
1028                 return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
1029                         m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
1030         case ATTR_SECONDARY_NAME:{
1031         const char *n[] = {"0", "AK8975", "AK8972", "AK8963", "BMA250"};
1032         if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id)
1033                 return sprintf(buf, "%s\n", n[1]);
1034         else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id)
1035                 return sprintf(buf, "%s\n", n[2]);
1036         else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id)
1037                 return sprintf(buf, "%s\n", n[3]);
1038         else if (ACCEL_ID_BMA250 == st->plat_data.sec_slave_id)
1039                 return sprintf(buf, "%s\n", n[4]);
1040         else
1041                 return sprintf(buf, "%s\n", n[0]);
1042         }
1043
1044 #ifdef CONFIG_INV_TESTING
1045         case ATTR_REG_WRITE:
1046                 return sprintf(buf, "1\n");
1047         case ATTR_DEBUG_SMD_EXE_STATE:
1048         {
1049                 u8 d[2];
1050
1051                 result = st->set_power_state(st, true);
1052                 mpu_memory_read(st, st->i2c_addr,
1053                                 inv_dmp_get_address(KEY_SMD_EXE_STATE), 2, d);
1054                 return sprintf(buf, "%d\n", (short)be16_to_cpup((__be16 *)(d)));
1055         }
1056         case ATTR_DEBUG_SMD_DELAY_CNTR:
1057         {
1058                 u8 d[4];
1059
1060                 result = st->set_power_state(st, true);
1061                 mpu_memory_read(st, st->i2c_addr,
1062                                 inv_dmp_get_address(KEY_SMD_DELAY_CNTR), 4, d);
1063                 return sprintf(buf, "%d\n", (int)be32_to_cpup((__be32 *)(d)));
1064         }
1065 #endif
1066         default:
1067                 return -EPERM;
1068         }
1069 }
1070
1071 /**
1072  * inv_dmp_display_orient_show() -  calling this function will
1073  *                      show orientation This event must use poll.
1074  */
1075 static ssize_t inv_dmp_display_orient_show(struct device *dev,
1076         struct device_attribute *attr, char *buf)
1077 {
1078         struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
1079         return sprintf(buf, "%d\n", st->display_orient_data);
1080 }
1081
1082 /**
1083  * inv_accel_motion_show() -  calling this function showes motion interrupt.
1084  *                         This event must use poll.
1085  */
1086 static ssize_t inv_accel_motion_show(struct device *dev,
1087         struct device_attribute *attr, char *buf)
1088 {
1089         return sprintf(buf, "1\n");
1090 }
1091
1092 /**
1093  * inv_smd_show() -  calling this function showes smd interrupt.
1094  *                         This event must use poll.
1095  */
1096 static ssize_t inv_smd_show(struct device *dev,
1097         struct device_attribute *attr, char *buf)
1098 {
1099         return sprintf(buf, "1\n");
1100 }
1101
1102 /**
1103  *  inv_temperature_show() - Read temperature data directly from registers.
1104  */
1105 static ssize_t inv_temperature_show(struct device *dev,
1106         struct device_attribute *attr, char *buf)
1107 {
1108
1109         struct iio_dev *indio_dev = dev_get_drvdata(dev);
1110         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1111         struct inv_reg_map_s *reg;
1112         int result, cur_scale, cur_off;
1113         short temp;
1114         long scale_t;
1115         u8 data[2];
1116         const long scale[] = {3834792L, 3158064L, 3340827L};
1117         const long offset[] = {5383314L, 2394184L, 1376256L};
1118
1119         reg = &st->reg;
1120         mutex_lock(&indio_dev->mlock);
1121         if (!st->chip_config.enable)
1122                 result = st->set_power_state(st, true);
1123         else
1124                 result = 0;
1125         if (result) {
1126                 mutex_unlock(&indio_dev->mlock);
1127                 return result;
1128         }
1129         result = inv_i2c_read(st, reg->temperature, 2, data);
1130         if (!st->chip_config.enable)
1131                 result |= st->set_power_state(st, false);
1132         mutex_unlock(&indio_dev->mlock);
1133         if (result) {
1134                 pr_err("Could not read temperature register.\n");
1135                 return result;
1136         }
1137         temp = (signed short)(be16_to_cpup((short *)&data[0]));
1138         switch (st->chip_type) {
1139         case INV_MPU3050:
1140                 cur_scale = scale[0];
1141                 cur_off   = offset[0];
1142                 break;
1143         case INV_MPU6050:
1144                 cur_scale = scale[1];
1145                 cur_off   = offset[1];
1146                 break;
1147         case INV_MPU6500:
1148                 cur_scale = scale[2];
1149                 cur_off   = offset[2];
1150                 break;
1151         default:
1152                 return -EINVAL;
1153         };
1154         scale_t = cur_off +
1155                 inv_q30_mult((int)temp << MPU_TEMP_SHIFT, cur_scale);
1156
1157         INV_I2C_INC_TEMPREAD(1);
1158
1159         return sprintf(buf, "%ld %lld\n", scale_t, get_time_ns());
1160 }
1161
1162 /**
1163  * inv_firmware_loaded() -  calling this function will change
1164  *                        firmware load
1165  */
1166 static int inv_firmware_loaded(struct inv_mpu_iio_s *st, int data)
1167 {
1168         if (data)
1169                 return -EINVAL;
1170         st->chip_config.firmware_loaded = 0;
1171         st->chip_config.dmp_on = 0;
1172         st->chip_config.quaternion_on = 0;
1173
1174         return 0;
1175 }
1176
1177 static int inv_switch_gyro_engine(struct inv_mpu_iio_s *st, bool en)
1178 {
1179         return inv_switch_engine(st, en, BIT_PWR_GYRO_STBY);
1180 }
1181
1182 static int inv_switch_accl_engine(struct inv_mpu_iio_s *st, bool en)
1183 {
1184         return inv_switch_engine(st, en, BIT_PWR_ACCL_STBY);
1185 }
1186
1187 /**
1188  *  inv_gyro_enable() - Enable/disable gyro.
1189  */
1190 static int inv_gyro_enable(struct inv_mpu_iio_s *st,
1191                                  struct iio_buffer *ring, bool en)
1192 {
1193         if (en == st->chip_config.gyro_enable)
1194                 return 0;
1195         if (!en) {
1196                 st->chip_config.gyro_fifo_enable = 0;
1197                 clear_bit(INV_MPU_SCAN_GYRO_X, ring->scan_mask);
1198                 clear_bit(INV_MPU_SCAN_GYRO_Y, ring->scan_mask);
1199                 clear_bit(INV_MPU_SCAN_GYRO_Z, ring->scan_mask);
1200         }
1201         st->chip_config.gyro_enable = en;
1202
1203         return 0;
1204 }
1205
1206 /**
1207  *  inv_accl_enable() - Enable/disable accl.
1208  */
1209 static int inv_accl_enable(struct inv_mpu_iio_s *st,
1210                                  struct iio_buffer *ring, bool en)
1211 {
1212         if (en == st->chip_config.accl_enable)
1213                 return 0;
1214         if (!en) {
1215                 st->chip_config.accl_fifo_enable = 0;
1216                 clear_bit(INV_MPU_SCAN_ACCL_X, ring->scan_mask);
1217                 clear_bit(INV_MPU_SCAN_ACCL_Y, ring->scan_mask);
1218                 clear_bit(INV_MPU_SCAN_ACCL_Z, ring->scan_mask);
1219         }
1220         st->chip_config.accl_enable = en;
1221
1222         return 0;
1223 }
1224
1225 /**
1226  * inv_compass_enable() -  calling this function will store compass
1227  *                         enable
1228  */
1229 static int inv_compass_enable(struct inv_mpu_iio_s *st,
1230                                  struct iio_buffer *ring, bool en)
1231 {
1232         if (en == st->chip_config.compass_enable)
1233                 return 0;
1234         if (!en) {
1235                 st->chip_config.compass_fifo_enable = 0;
1236                 clear_bit(INV_MPU_SCAN_MAGN_X, ring->scan_mask);
1237                 clear_bit(INV_MPU_SCAN_MAGN_Y, ring->scan_mask);
1238                 clear_bit(INV_MPU_SCAN_MAGN_Z, ring->scan_mask);
1239         }
1240         st->chip_config.compass_enable = en;
1241
1242         return 0;
1243 }
1244
1245 /**
1246  * inv_attr_store() -  calling this function will store current
1247  *                        non-dmp parameter settings
1248  */
1249 static ssize_t inv_attr_store(struct device *dev,
1250         struct device_attribute *attr, const char *buf, size_t count)
1251 {
1252         struct iio_dev *indio_dev = dev_get_drvdata(dev);
1253         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1254         struct iio_buffer *ring = indio_dev->buffer;
1255         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
1256         int data;
1257         u8  d;
1258         int result;
1259
1260         mutex_lock(&indio_dev->mlock);
1261         if (st->chip_config.enable) {
1262                 result = -EBUSY;
1263                 goto attr_store_fail;
1264         }
1265         if (this_attr->address <= ATTR_MOTION_LPA_THRESHOLD) {
1266                 result = st->set_power_state(st, true);
1267                 if (result)
1268                         goto attr_store_fail;
1269         }
1270
1271         result = kstrtoint(buf, 10, &data);
1272         if (result)
1273                 goto attr_store_fail;
1274         switch (this_attr->address) {
1275         case ATTR_MOTION_LPA_ON:
1276                 if (INV_MPU6500 == st->chip_type) {
1277                         if (data)
1278                                 /* enable and put in MPU6500 mode */
1279                                 d = BIT_ACCEL_INTEL_ENABLE
1280                                         | BIT_ACCEL_INTEL_MODE;
1281                         else
1282                                 d = 0;
1283                         result = inv_i2c_single_write(st,
1284                                                 REG_6500_ACCEL_INTEL_CTRL, d);
1285                         if (result)
1286                                 goto attr_store_fail;
1287                 }
1288                 st->mot_int.mot_on = !!data;
1289                 st->chip_config.lpa_mode = !!data;
1290                 break;
1291         case ATTR_MOTION_LPA_FREQ:
1292                 result = inv_lpa_freq(st, data);
1293                 break;
1294         case ATTR_MOTION_LPA_THRESHOLD:
1295                 if ((data > MPU6XXX_MAX_MOTION_THRESH) || (data < 0)) {
1296                         result = -EINVAL;
1297                         goto attr_store_fail;
1298                 }
1299                 d = (u8)(data >> MPU6XXX_MOTION_THRESH_SHIFT);
1300                 data = (d << MPU6XXX_MOTION_THRESH_SHIFT);
1301                 result = inv_i2c_single_write(st, REG_ACCEL_MOT_THR, d);
1302                 if (result)
1303                         goto attr_store_fail;
1304                 st->mot_int.mot_thr = data;
1305                 break;
1306         /* from now on, power is not turned on */
1307         case ATTR_SELF_TEST_SAMPLES:
1308                 if (data > ST_MAX_SAMPLES || data < 0) {
1309                         result = -EINVAL;
1310                         goto attr_store_fail;
1311                 }
1312                 st->self_test.samples = data;
1313                 break;
1314         case ATTR_SELF_TEST_THRESHOLD:
1315                 if (data > ST_MAX_THRESHOLD || data < 0) {
1316                         result = -EINVAL;
1317                         goto attr_store_fail;
1318                 }
1319                 st->self_test.threshold = data;
1320         case ATTR_GYRO_ENABLE:
1321                 result = st->gyro_en(st, ring, !!data);
1322                 break;
1323         case ATTR_ACCL_ENABLE:
1324                 result = st->accl_en(st, ring, !!data);
1325                 break;
1326         case ATTR_COMPASS_ENABLE:
1327                 result = inv_compass_enable(st, ring, !!data);
1328                 break;
1329         case ATTR_POWER_STATE:
1330                 fake_asleep = !data;
1331                 break;
1332         case ATTR_FIRMWARE_LOADED:
1333                 result = inv_firmware_loaded(st, data);
1334                 break;
1335         case ATTR_SAMPLING_FREQ:
1336                 result = inv_fifo_rate_store(st, data);
1337                 break;
1338         default:
1339                 result = -EINVAL;
1340                 goto attr_store_fail;
1341         };
1342
1343 attr_store_fail:
1344         if ((this_attr->address <= ATTR_MOTION_LPA_THRESHOLD) &&
1345                                         (!st->chip_config.enable))
1346                 result |= st->set_power_state(st, false);
1347         mutex_unlock(&indio_dev->mlock);
1348         if (result)
1349                 return result;
1350
1351         return count;
1352 }
1353
1354 #ifdef CONFIG_INV_TESTING
1355 /**
1356  * inv_reg_write_store() - register write command for testing.
1357  *                         Format: WSRRDD, where RR is the register in hex,
1358  *                                         and DD is the data in hex.
1359  */
1360 static ssize_t inv_reg_write_store(struct device *dev,
1361         struct device_attribute *attr, const char *buf, size_t count)
1362 {
1363         struct iio_dev *indio_dev = dev_get_drvdata(dev);
1364         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1365         u32 result;
1366         u8 wreg, wval;
1367         int temp;
1368         char local_buf[10];
1369
1370         if ((buf[0] != 'W' && buf[0] != 'w') ||
1371             (buf[1] != 'S' && buf[1] != 's'))
1372                 return -EINVAL;
1373         if (strlen(buf) < 6)
1374                 return -EINVAL;
1375
1376         strncpy(local_buf, buf, 7);
1377         local_buf[6] = 0;
1378         result = sscanf(&local_buf[4], "%x", &temp);
1379         if (result == 0)
1380                 return -EINVAL;
1381         wval = temp;
1382         local_buf[4] = 0;
1383         sscanf(&local_buf[2], "%x", &temp);
1384         if (result == 0)
1385                 return -EINVAL;
1386         wreg = temp;
1387
1388         result = inv_i2c_single_write(st, wreg, wval);
1389         if (result)
1390                 return result;
1391
1392         return count;
1393 }
1394 #endif /* CONFIG_INV_TESTING */
1395
1396 #define IIO_ST(si, rb, sb, sh)                                          \
1397         { .sign = si, .realbits = rb, .storagebits = sb, .shift = sh }
1398 #define INV_MPU_CHAN(_type, _channel2, _index)                \
1399         {                                                         \
1400                 .type = _type,                                        \
1401                 .modified = 1,                                        \
1402                 .channel2 = _channel2,                                \
1403                 .info_mask_separate = BIT(IIO_CHAN_INFO_CALIBBIAS), \
1404                 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
1405                 .scan_index = _index,                                 \
1406                 .scan_type  = IIO_ST('s', 16, 16, 0)                  \
1407         }
1408
1409 #define INV_ACCL_CHAN(_type, _channel2, _index)                \
1410         {                                                         \
1411                 .type = _type,                                        \
1412                 .modified = 1,                                        \
1413                 .channel2 = _channel2,                                \
1414                 .info_mask_separate = BIT(IIO_CHAN_INFO_CALIBBIAS) | \
1415                                       BIT(IIO_CHAN_INFO_OFFSET), \
1416                 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
1417                 .scan_index = _index,                                 \
1418                 .scan_type  = IIO_ST('s', 16, 16, 0)                  \
1419         }
1420
1421 #define INV_MPU_QUATERNION_CHAN(_channel2, _index)            \
1422         {                                                         \
1423                 .type = IIO_QUATERNION,                               \
1424                 .modified = 1,                                        \
1425                 .channel2 = _channel2,                                \
1426                 .scan_index = _index,                                 \
1427                 .scan_type  = IIO_ST('s', 32, 32, 0)                  \
1428         }
1429
1430 #define INV_MPU_MAGN_CHAN(_channel2, _index)                  \
1431         {                                                         \
1432                 .type = IIO_MAGN,                                     \
1433                 .modified = 1,                                        \
1434                 .channel2 = _channel2,                                \
1435                 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
1436                 .scan_index = _index,                                 \
1437                 .scan_type  = IIO_ST('s', 16, 16, 0)                  \
1438         }
1439
1440 static const struct iio_chan_spec inv_mpu_channels[] = {
1441         IIO_CHAN_SOFT_TIMESTAMP(INV_MPU_SCAN_TIMESTAMP),
1442         INV_MPU_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU_SCAN_GYRO_X),
1443         INV_MPU_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU_SCAN_GYRO_Y),
1444         INV_MPU_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU_SCAN_GYRO_Z),
1445
1446         INV_ACCL_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU_SCAN_ACCL_X),
1447         INV_ACCL_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU_SCAN_ACCL_Y),
1448         INV_ACCL_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU_SCAN_ACCL_Z),
1449
1450         INV_MPU_QUATERNION_CHAN(IIO_MOD_R, INV_MPU_SCAN_QUAT_R),
1451         INV_MPU_QUATERNION_CHAN(IIO_MOD_X, INV_MPU_SCAN_QUAT_X),
1452         INV_MPU_QUATERNION_CHAN(IIO_MOD_Y, INV_MPU_SCAN_QUAT_Y),
1453         INV_MPU_QUATERNION_CHAN(IIO_MOD_Z, INV_MPU_SCAN_QUAT_Z),
1454
1455         INV_MPU_MAGN_CHAN(IIO_MOD_X, INV_MPU_SCAN_MAGN_X),
1456         INV_MPU_MAGN_CHAN(IIO_MOD_Y, INV_MPU_SCAN_MAGN_Y),
1457         INV_MPU_MAGN_CHAN(IIO_MOD_Z, INV_MPU_SCAN_MAGN_Z),
1458 };
1459
1460
1461 /*constant IIO attribute */
1462 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500");
1463
1464 /* special sysfs */
1465 static DEVICE_ATTR(reg_dump, S_IRUGO, inv_reg_dump_show, NULL);
1466 static DEVICE_ATTR(temperature, S_IRUGO, inv_temperature_show, NULL);
1467
1468 /* event based sysfs, needs poll to read */
1469 static DEVICE_ATTR(event_display_orientation, S_IRUGO,
1470         inv_dmp_display_orient_show, NULL);
1471 static DEVICE_ATTR(event_accel_motion, S_IRUGO, inv_accel_motion_show, NULL);
1472 static DEVICE_ATTR(event_smd, S_IRUGO, inv_smd_show, NULL);
1473
1474 /* DMP sysfs with power on/off */
1475 static IIO_DEVICE_ATTR(smd_enable, S_IRUGO | S_IWUSR,
1476         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_ENABLE);
1477 static IIO_DEVICE_ATTR(smd_threshold, S_IRUGO | S_IWUSR,
1478         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_THLD);
1479 static IIO_DEVICE_ATTR(smd_delay_threshold, S_IRUGO | S_IWUSR,
1480         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_DELAY_THLD);
1481 static IIO_DEVICE_ATTR(smd_delay_threshold2, S_IRUGO | S_IWUSR,
1482         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_DELAY_THLD2);
1483 static IIO_DEVICE_ATTR(display_orientation_on, S_IRUGO | S_IWUSR,
1484         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_DISPLAY_ORIENTATION_ON);
1485
1486 /* DMP sysfs without power on/off */
1487 static IIO_DEVICE_ATTR(dmp_on, S_IRUGO | S_IWUSR, inv_attr_show,
1488         inv_dmp_attr_store, ATTR_DMP_ON);
1489 static IIO_DEVICE_ATTR(dmp_int_on, S_IRUGO | S_IWUSR, inv_attr_show,
1490         inv_dmp_attr_store, ATTR_DMP_INT_ON);
1491 static IIO_DEVICE_ATTR(dmp_event_int_on, S_IRUGO | S_IWUSR, inv_attr_show,
1492         inv_dmp_attr_store, ATTR_DMP_EVENT_INT_ON);
1493 static IIO_DEVICE_ATTR(dmp_output_rate, S_IRUGO | S_IWUSR, inv_attr_show,
1494         inv_dmp_attr_store, ATTR_DMP_OUTPUT_RATE);
1495 static IIO_DEVICE_ATTR(quaternion_on, S_IRUGO | S_IWUSR, inv_attr_show,
1496         inv_dmp_attr_store, ATTR_DMP_QUATERNION_ON);
1497
1498 /* non DMP sysfs with power on/off */
1499 static IIO_DEVICE_ATTR(motion_lpa_on, S_IRUGO | S_IWUSR, inv_attr_show,
1500         inv_attr_store, ATTR_MOTION_LPA_ON);
1501 static IIO_DEVICE_ATTR(motion_lpa_freq, S_IRUGO | S_IWUSR, inv_attr_show,
1502         inv_attr_store, ATTR_MOTION_LPA_FREQ);
1503 static IIO_DEVICE_ATTR(motion_lpa_threshold, S_IRUGO | S_IWUSR, inv_attr_show,
1504         inv_attr_store, ATTR_MOTION_LPA_THRESHOLD);
1505
1506 /* non DMP sysfs without power on/off */
1507 static IIO_DEVICE_ATTR(self_test_samples, S_IRUGO | S_IWUSR, inv_attr_show,
1508         inv_attr_store, ATTR_SELF_TEST_SAMPLES);
1509 static IIO_DEVICE_ATTR(self_test_threshold, S_IRUGO | S_IWUSR, inv_attr_show,
1510         inv_attr_store, ATTR_SELF_TEST_THRESHOLD);
1511 static IIO_DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR, inv_attr_show,
1512         inv_attr_store, ATTR_GYRO_ENABLE);
1513 static IIO_DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR, inv_attr_show,
1514         inv_attr_store, ATTR_ACCL_ENABLE);
1515 static IIO_DEVICE_ATTR(compass_enable, S_IRUGO | S_IWUSR, inv_attr_show,
1516         inv_attr_store, ATTR_COMPASS_ENABLE);
1517 static IIO_DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR, inv_attr_show,
1518         inv_attr_store, ATTR_POWER_STATE);
1519 static IIO_DEVICE_ATTR(firmware_loaded, S_IRUGO | S_IWUSR, inv_attr_show,
1520         inv_attr_store, ATTR_FIRMWARE_LOADED);
1521 static IIO_DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR, inv_attr_show,
1522         inv_attr_store, ATTR_SAMPLING_FREQ);
1523
1524 /* show method only sysfs but with power on/off */
1525 static IIO_DEVICE_ATTR(self_test, S_IRUGO, inv_attr_show, NULL,
1526         ATTR_SELF_TEST);
1527
1528 /* show method only sysfs */
1529 static IIO_DEVICE_ATTR(gyro_matrix, S_IRUGO, inv_attr_show, NULL,
1530         ATTR_GYRO_MATRIX);
1531 static IIO_DEVICE_ATTR(accl_matrix, S_IRUGO, inv_attr_show, NULL,
1532         ATTR_ACCL_MATRIX);
1533 static IIO_DEVICE_ATTR(compass_matrix, S_IRUGO, inv_attr_show, NULL,
1534         ATTR_COMPASS_MATRIX);
1535 static IIO_DEVICE_ATTR(secondary_name, S_IRUGO, inv_attr_show, NULL,
1536         ATTR_SECONDARY_NAME);
1537
1538 #ifdef CONFIG_INV_TESTING
1539 static IIO_DEVICE_ATTR(reg_write, S_IRUGO | S_IWUSR, inv_attr_show,
1540         inv_reg_write_store, ATTR_REG_WRITE);
1541 /* smd debug related sysfs */
1542 static IIO_DEVICE_ATTR(debug_smd_enable_testp1, S_IWUSR, NULL,
1543         inv_dmp_attr_store, ATTR_DEBUG_SMD_ENABLE_TESTP1);
1544 static IIO_DEVICE_ATTR(debug_smd_enable_testp2, S_IWUSR, NULL,
1545         inv_dmp_attr_store, ATTR_DEBUG_SMD_ENABLE_TESTP2);
1546 static IIO_DEVICE_ATTR(debug_smd_exe_state, S_IRUGO, inv_attr_show,
1547         NULL, ATTR_DEBUG_SMD_EXE_STATE);
1548 static IIO_DEVICE_ATTR(debug_smd_delay_cntr, S_IRUGO, inv_attr_show,
1549         NULL, ATTR_DEBUG_SMD_DELAY_CNTR);
1550 #endif
1551
1552 static const struct attribute *inv_gyro_attributes[] = {
1553         &iio_const_attr_sampling_frequency_available.dev_attr.attr,
1554         &dev_attr_reg_dump.attr,
1555         &dev_attr_temperature.attr,
1556         &iio_dev_attr_self_test_samples.dev_attr.attr,
1557         &iio_dev_attr_self_test_threshold.dev_attr.attr,
1558         &iio_dev_attr_gyro_enable.dev_attr.attr,
1559         &iio_dev_attr_power_state.dev_attr.attr,
1560         &iio_dev_attr_sampling_frequency.dev_attr.attr,
1561         &iio_dev_attr_self_test.dev_attr.attr,
1562         &iio_dev_attr_gyro_matrix.dev_attr.attr,
1563         &iio_dev_attr_secondary_name.dev_attr.attr,
1564 #ifdef CONFIG_INV_TESTING
1565         &iio_dev_attr_reg_write.dev_attr.attr,
1566         &iio_dev_attr_debug_smd_enable_testp1.dev_attr.attr,
1567         &iio_dev_attr_debug_smd_enable_testp2.dev_attr.attr,
1568         &iio_dev_attr_debug_smd_exe_state.dev_attr.attr,
1569         &iio_dev_attr_debug_smd_delay_cntr.dev_attr.attr,
1570 #endif
1571 };
1572
1573 static const struct attribute *inv_mpu6050_attributes[] = {
1574         &dev_attr_event_display_orientation.attr,
1575         &dev_attr_event_smd.attr,
1576         &iio_dev_attr_smd_enable.dev_attr.attr,
1577         &iio_dev_attr_smd_threshold.dev_attr.attr,
1578         &iio_dev_attr_smd_delay_threshold.dev_attr.attr,
1579         &iio_dev_attr_smd_delay_threshold2.dev_attr.attr,
1580         &iio_dev_attr_display_orientation_on.dev_attr.attr,
1581         &iio_dev_attr_dmp_on.dev_attr.attr,
1582         &iio_dev_attr_dmp_int_on.dev_attr.attr,
1583         &iio_dev_attr_dmp_event_int_on.dev_attr.attr,
1584         &iio_dev_attr_dmp_output_rate.dev_attr.attr,
1585         &iio_dev_attr_quaternion_on.dev_attr.attr,
1586         &iio_dev_attr_accl_enable.dev_attr.attr,
1587         &iio_dev_attr_firmware_loaded.dev_attr.attr,
1588         &iio_dev_attr_accl_matrix.dev_attr.attr,
1589
1590 };
1591
1592 static const struct attribute *inv_mpu6500_attributes[] = {
1593         &dev_attr_event_accel_motion.attr,
1594         &iio_dev_attr_motion_lpa_on.dev_attr.attr,
1595         &iio_dev_attr_motion_lpa_freq.dev_attr.attr,
1596         &iio_dev_attr_motion_lpa_threshold.dev_attr.attr,
1597 };
1598
1599 static const struct attribute *inv_compass_attributes[] = {
1600         &iio_dev_attr_compass_enable.dev_attr.attr,
1601         &iio_dev_attr_compass_matrix.dev_attr.attr,
1602 };
1603
1604 static const struct attribute *inv_mpu3050_attributes[] = {
1605         &iio_dev_attr_accl_enable.dev_attr.attr,
1606         &iio_dev_attr_accl_matrix.dev_attr.attr,
1607 };
1608
1609 static struct attribute *inv_attributes[ARRAY_SIZE(inv_gyro_attributes) +
1610                                 ARRAY_SIZE(inv_mpu6050_attributes) +
1611                                 ARRAY_SIZE(inv_mpu6500_attributes) +
1612                                 ARRAY_SIZE(inv_compass_attributes) + 1];
1613
1614 static const struct attribute_group inv_attribute_group = {
1615         .name = "mpu",
1616         .attrs = inv_attributes
1617 };
1618
1619 static const struct iio_info mpu_info = {
1620         .driver_module = THIS_MODULE,
1621         .read_raw = &mpu_read_raw,
1622         .write_raw = &mpu_write_raw,
1623         .attrs = &inv_attribute_group,
1624 };
1625
1626 /**
1627  *  inv_setup_compass() - Configure compass.
1628  */
1629 static int inv_setup_compass(struct inv_mpu_iio_s *st)
1630 {
1631         int result;
1632         u8 data[4];
1633
1634         if (INV_MPU6050 == st->chip_type) {
1635                 result = inv_i2c_read(st, REG_YGOFFS_TC, 1, data);
1636                 if (result)
1637                         return result;
1638                 data[0] &= ~BIT_I2C_MST_VDDIO;
1639                 if (st->plat_data.level_shifter)
1640                         data[0] |= BIT_I2C_MST_VDDIO;
1641                 /*set up VDDIO register */
1642                 result = inv_i2c_single_write(st, REG_YGOFFS_TC, data[0]);
1643                 if (result)
1644                         return result;
1645         }
1646         /* set to bypass mode */
1647         result = inv_i2c_single_write(st, REG_INT_PIN_CFG,
1648                                 st->plat_data.int_config | BIT_BYPASS_EN);
1649         if (result)
1650                 return result;
1651         /*read secondary i2c ID register */
1652         result = inv_secondary_read(REG_AKM_ID, 1, data);
1653         if (result)
1654                 return result;
1655         if (data[0] != DATA_AKM_ID)
1656                 return -ENXIO;
1657         /*set AKM to Fuse ROM access mode */
1658         result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_FR);
1659         if (result)
1660                 return result;
1661         result = inv_secondary_read(REG_AKM_SENSITIVITY, THREE_AXIS,
1662                                         st->chip_info.compass_sens);
1663         if (result)
1664                 return result;
1665         /*revert to power down mode */
1666         result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PD);
1667         if (result)
1668                 return result;
1669         pr_debug("%s senx=%d, seny=%d, senz=%d\n",
1670                  st->hw->name,
1671                  st->chip_info.compass_sens[0],
1672                  st->chip_info.compass_sens[1],
1673                  st->chip_info.compass_sens[2]);
1674         /*restore to non-bypass mode */
1675         result = inv_i2c_single_write(st, REG_INT_PIN_CFG,
1676                         st->plat_data.int_config);
1677         if (result)
1678                 return result;
1679
1680         /*setup master mode and master clock and ES bit*/
1681         result = inv_i2c_single_write(st, REG_I2C_MST_CTRL, BIT_WAIT_FOR_ES);
1682         if (result)
1683                 return result;
1684         /* slave 0 is used to read data from compass */
1685         /*read mode */
1686         result = inv_i2c_single_write(st, REG_I2C_SLV0_ADDR, BIT_I2C_READ|
1687                 st->plat_data.secondary_i2c_addr);
1688         if (result)
1689                 return result;
1690         /* AKM status register address is 2 */
1691         result = inv_i2c_single_write(st, REG_I2C_SLV0_REG, REG_AKM_STATUS);
1692         if (result)
1693                 return result;
1694         /* slave 0 is enabled at the beginning, read 8 bytes from here */
1695         result = inv_i2c_single_write(st, REG_I2C_SLV0_CTRL, BIT_SLV_EN |
1696                                 NUM_BYTES_COMPASS_SLAVE);
1697         if (result)
1698                 return result;
1699         /*slave 1 is used for AKM mode change only*/
1700         result = inv_i2c_single_write(st, REG_I2C_SLV1_ADDR,
1701                 st->plat_data.secondary_i2c_addr);
1702         if (result)
1703                 return result;
1704         /* AKM mode register address is 0x0A */
1705         result = inv_i2c_single_write(st, REG_I2C_SLV1_REG, REG_AKM_MODE);
1706         if (result)
1707                 return result;
1708         /* slave 1 is enabled, byte length is 1 */
1709         result = inv_i2c_single_write(st, REG_I2C_SLV1_CTRL, BIT_SLV_EN | 1);
1710         if (result)
1711                 return result;
1712         /* output data for slave 1 is fixed, single measure mode*/
1713         st->compass_scale = 1;
1714         if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id) {
1715                 st->compass_st_upper = AKM8975_ST_Upper;
1716                 st->compass_st_lower = AKM8975_ST_Lower;
1717                 data[0] = DATA_AKM_MODE_SM;
1718         } else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id) {
1719                 st->compass_st_upper = AKM8972_ST_Upper;
1720                 st->compass_st_lower = AKM8972_ST_Lower;
1721                 data[0] = DATA_AKM_MODE_SM;
1722         } else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) {
1723                 st->compass_st_upper = AKM8963_ST_Upper;
1724                 st->compass_st_lower = AKM8963_ST_Lower;
1725                 data[0] = DATA_AKM_MODE_SM |
1726                           (st->compass_scale << AKM8963_SCALE_SHIFT);
1727         }
1728         result = inv_i2c_single_write(st, REG_I2C_SLV1_DO, data[0]);
1729         if (result)
1730                 return result;
1731         /* slave 0 and 1 timer action is enabled every sample*/
1732         result = inv_i2c_single_write(st, REG_I2C_MST_DELAY_CTRL,
1733                                 BIT_SLV0_DLY_EN | BIT_SLV1_DLY_EN);
1734         return result;
1735 }
1736
1737 static void inv_setup_func_ptr(struct inv_mpu_iio_s *st)
1738 {
1739         if (st->chip_type == INV_MPU3050) {
1740                 st->set_power_state    = set_power_mpu3050;
1741                 st->switch_gyro_engine = inv_switch_3050_gyro_engine;
1742                 st->switch_accl_engine = inv_switch_3050_accl_engine;
1743                 st->init_config        = inv_init_config_mpu3050;
1744                 st->setup_reg          = inv_setup_reg_mpu3050;
1745         } else {
1746                 st->set_power_state    = set_power_itg;
1747                 st->switch_gyro_engine = inv_switch_gyro_engine;
1748                 st->switch_accl_engine = inv_switch_accl_engine;
1749                 st->init_config        = inv_init_config;
1750                 st->setup_reg          = inv_setup_reg;
1751                 /*MPU6XXX special functions */
1752                 st->compass_en         = inv_compass_enable;
1753                 st->quaternion_en      = inv_quaternion_on;
1754         }
1755         st->gyro_en            = inv_gyro_enable;
1756         st->accl_en            = inv_accl_enable;
1757 }
1758
1759 static int inv_detect_6xxx(struct inv_mpu_iio_s *st)
1760 {
1761         int result;
1762         u8 d;
1763
1764         result = inv_i2c_read(st, REG_WHOAMI, 1, &d);
1765         if (result)
1766                 return result;
1767         if ((d == MPU6500_ID) || (d == MPU6515_ID)) {
1768                 st->chip_type = INV_MPU6500;
1769                 strcpy(st->name, "mpu6500");
1770         } else {
1771                 strcpy(st->name, "mpu6050");
1772         }
1773
1774         return 0;
1775 }
1776
1777 /**
1778  *  inv_check_chip_type() - check and setup chip type.
1779  */
1780 static int inv_check_chip_type(struct inv_mpu_iio_s *st,
1781                 const struct i2c_device_id *id)
1782 {
1783         struct inv_reg_map_s *reg;
1784         int result;
1785         int t_ind;
1786
1787         if (!strcmp(id->name, "itg3500"))
1788                 st->chip_type = INV_ITG3500;
1789         else if (!strcmp(id->name, "mpu3050"))
1790                 st->chip_type = INV_MPU3050;
1791         else if (!strcmp(id->name, "mpu6050"))
1792                 st->chip_type = INV_MPU6050;
1793         else if (!strcmp(id->name, "mpu9150"))
1794                 st->chip_type = INV_MPU6050;
1795         else if (!strcmp(id->name, "mpu6500"))
1796                 st->chip_type = INV_MPU6500;
1797         else if (!strcmp(id->name, "mpu9250"))
1798                 st->chip_type = INV_MPU6500;
1799         else if (!strcmp(id->name, "mpu6xxx"))
1800                 st->chip_type = INV_MPU6050;
1801         else if (!strcmp(id->name, "mpu6515"))
1802                 st->chip_type = INV_MPU6500;
1803         else
1804                 return -EPERM;
1805         inv_setup_func_ptr(st);
1806         st->hw  = &hw_info[st->chip_type];
1807         st->mpu_slave = NULL;
1808         reg = &st->reg;
1809         st->setup_reg(reg);
1810         /* reset to make sure previous state are not there */
1811         result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET);
1812         if (result)
1813                 return result;
1814         msleep(POWER_UP_TIME);
1815         /* toggle power state */
1816         result = st->set_power_state(st, false);
1817         if (result)
1818                 return result;
1819
1820         result = st->set_power_state(st, true);
1821         if (result)
1822                 return result;
1823
1824         if (!strcmp(id->name, "mpu6xxx")) {
1825                 /* for MPU6500, reading register need more time */
1826                 msleep(POWER_UP_TIME);
1827                 result = inv_detect_6xxx(st);
1828                 if (result)
1829                         return result;
1830         }
1831
1832         switch (st->chip_type) {
1833         case INV_ITG3500:
1834                 st->num_channels = INV_CHANNEL_NUM_GYRO;
1835                 break;
1836         case INV_MPU6050:
1837         case INV_MPU6500:
1838                 if (SECONDARY_SLAVE_TYPE_COMPASS ==
1839                     st->plat_data.sec_slave_type) {
1840                         st->chip_config.has_compass = 1;
1841                         st->num_channels =
1842                                 INV_CHANNEL_NUM_GYRO_ACCL_QUANTERNION_MAGN;
1843                 } else {
1844                         st->chip_config.has_compass = 0;
1845                         st->num_channels =
1846                                 INV_CHANNEL_NUM_GYRO_ACCL_QUANTERNION;
1847                 }
1848                 break;
1849         case INV_MPU3050:
1850                 if (SECONDARY_SLAVE_TYPE_ACCEL ==
1851                     st->plat_data.sec_slave_type) {
1852                         if (ACCEL_ID_BMA250 == st->plat_data.sec_slave_id)
1853                                 inv_register_mpu3050_slave(st);
1854                         st->num_channels = INV_CHANNEL_NUM_GYRO_ACCL;
1855                 } else {
1856                         st->num_channels = INV_CHANNEL_NUM_GYRO;
1857                 }
1858                 break;
1859         default:
1860                 result = st->set_power_state(st, false);
1861                 return -ENODEV;
1862         }
1863         st->chip_info.multi = 1;
1864         switch (st->chip_type) {
1865         case INV_MPU6050:
1866                 result = inv_get_silicon_rev_mpu6050(st);
1867                 break;
1868         case INV_MPU6500:
1869                 result = inv_get_silicon_rev_mpu6500(st);
1870                 break;
1871         default:
1872                 result = 0;
1873                 break;
1874         }
1875         if (result) {
1876                 pr_err("read silicon rev error\n");
1877                 st->set_power_state(st, false);
1878                 return result;
1879         }
1880         /* turn off the gyro engine after OTP reading */
1881         result = st->switch_gyro_engine(st, false);
1882         if (result)
1883                 return result;
1884         result = st->switch_accl_engine(st, false);
1885         if (result)
1886                 return result;
1887         if (st->chip_config.has_compass) {
1888                 result = inv_setup_compass(st);
1889                 if (result) {
1890                         pr_err("compass setup failed\n");
1891                         st->set_power_state(st, false);
1892                         return result;
1893                 }
1894         }
1895
1896         t_ind = 0;
1897         memcpy(&inv_attributes[t_ind], inv_gyro_attributes,
1898                 sizeof(inv_gyro_attributes));
1899         t_ind += ARRAY_SIZE(inv_gyro_attributes);
1900
1901         if (INV_MPU3050 == st->chip_type && st->mpu_slave != NULL) {
1902                 memcpy(&inv_attributes[t_ind], inv_mpu3050_attributes,
1903                        sizeof(inv_mpu3050_attributes));
1904                 t_ind += ARRAY_SIZE(inv_mpu3050_attributes);
1905                 inv_attributes[t_ind] = NULL;
1906                 return 0;
1907         }
1908
1909         if ((INV_MPU6050 == st->chip_type) || (INV_MPU6500 == st->chip_type)) {
1910                 memcpy(&inv_attributes[t_ind], inv_mpu6050_attributes,
1911                        sizeof(inv_mpu6050_attributes));
1912                 t_ind += ARRAY_SIZE(inv_mpu6050_attributes);
1913         }
1914
1915         if (INV_MPU6500 == st->chip_type) {
1916                 memcpy(&inv_attributes[t_ind], inv_mpu6500_attributes,
1917                        sizeof(inv_mpu6500_attributes));
1918                 t_ind += ARRAY_SIZE(inv_mpu6500_attributes);
1919         }
1920
1921         if (st->chip_config.has_compass) {
1922                 memcpy(&inv_attributes[t_ind], inv_compass_attributes,
1923                        sizeof(inv_compass_attributes));
1924                 t_ind += ARRAY_SIZE(inv_compass_attributes);
1925         }
1926         inv_attributes[t_ind] = NULL;
1927
1928         return 0;
1929 }
1930
1931 /**
1932  *  inv_create_dmp_sysfs() - create binary sysfs dmp entry.
1933  */
1934 static const struct bin_attribute dmp_firmware = {
1935         .attr = {
1936                 .name = "dmp_firmware",
1937                 .mode = S_IRUGO | S_IWUSR
1938         },
1939         .size = 4096,
1940         .read = inv_dmp_firmware_read,
1941         .write = inv_dmp_firmware_write,
1942 };
1943
1944 static int inv_create_dmp_sysfs(struct iio_dev *ind)
1945 {
1946         int result;
1947         result = sysfs_create_bin_file(&ind->dev.kobj, &dmp_firmware);
1948
1949         return result;
1950 }
1951
1952 #include <linux/gpio.h>
1953 #include <linux/of_gpio.h>
1954 #include <linux/of.h>
1955
1956 static struct mpu_platform_data mpu_data = {
1957         .int_config  = 0x00,
1958         .level_shifter = 0,
1959         .orientation = {
1960                         0,  1,  0,
1961                         -1,  0,  0,
1962                         0,  0, -1,
1963         },
1964         /*
1965         .key = {
1966                 221,  22, 205,   7, 217, 186, 151, 55,
1967                 206, 254,  35, 144, 225, 102,  47, 50,
1968         },
1969         */
1970 };
1971
1972 static int of_inv_parse_platform_data(struct i2c_client *client,
1973                                       struct mpu_platform_data *pdata)
1974 {
1975         struct device_node *np = client->dev.of_node;
1976         unsigned long irq_flags;
1977         int irq_pin;
1978         int gpio_pin;
1979
1980         gpio_pin = of_get_named_gpio_flags(np, "irq-gpio", 0, (enum of_gpio_flags *)&irq_flags);
1981         gpio_request(gpio_pin, "mpu6500");
1982         irq_pin = gpio_to_irq(gpio_pin);
1983         client->irq = irq_pin;
1984         i2c_set_clientdata(client, &mpu_data);
1985
1986         pr_info("%s: %s, %x, %x\n", __func__, client->name, client->addr, client->irq);
1987
1988         return 0;
1989 }
1990
1991 /**
1992  *  inv_mpu_probe() - probe function.
1993  */
1994 static int inv_mpu_probe(struct i2c_client *client,
1995         const struct i2c_device_id *id)
1996 {
1997         struct inv_mpu_iio_s *st;
1998         struct iio_dev *indio_dev;
1999         int result;
2000
2001         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
2002                 result = -ENOSYS;
2003                 pr_err("I2c function error\n");
2004                 goto out_no_free;
2005         }
2006 #ifdef INV_KERNEL_3_10
2007     indio_dev = iio_device_alloc(sizeof(*st));
2008 #else
2009         indio_dev = iio_allocate_device(sizeof(*st));
2010 #endif
2011         if (indio_dev == NULL) {
2012                 pr_err("memory allocation failed\n");
2013                 result =  -ENOMEM;
2014                 goto out_no_free;
2015         }
2016         st = iio_priv(indio_dev);
2017         st->client = client;
2018         st->sl_handle = client->adapter;
2019         st->i2c_addr = client->addr;
2020         if (client->dev.of_node) {
2021                 result = of_inv_parse_platform_data(client, &st->plat_data);
2022                 if (result)
2023                         goto out_free;
2024                 st->plat_data = mpu_data;
2025                 pr_info("secondary_i2c_addr=%x\n", st->plat_data.secondary_i2c_addr);
2026         } else
2027                 st->plat_data =
2028                         *(struct mpu_platform_data *)dev_get_platdata(&client->dev);
2029         /* power is turned on inside check chip type*/
2030         result = inv_check_chip_type(st, id);
2031         if (result)
2032                 goto out_free;
2033
2034         result = st->init_config(indio_dev);
2035         if (result) {
2036                 dev_err(&client->adapter->dev,
2037                         "Could not initialize device.\n");
2038                 goto out_free;
2039         }
2040         result = st->set_power_state(st, false);
2041         if (result) {
2042                 dev_err(&client->adapter->dev,
2043                         "%s could not be turned off.\n", st->hw->name);
2044                 goto out_free;
2045         }
2046
2047         /* Make state variables available to all _show and _store functions. */
2048         i2c_set_clientdata(client, indio_dev);
2049         indio_dev->dev.parent = &client->dev;
2050         if (!strcmp(id->name, "mpu6xxx"))
2051                 indio_dev->name = st->name;
2052         else
2053                 indio_dev->name = id->name;
2054         indio_dev->channels = inv_mpu_channels;
2055         indio_dev->num_channels = st->num_channels;
2056
2057         indio_dev->info = &mpu_info;
2058         indio_dev->modes = INDIO_DIRECT_MODE;
2059         indio_dev->currentmode = INDIO_DIRECT_MODE;
2060
2061         result = inv_mpu_configure_ring(indio_dev);
2062         if (result) {
2063                 pr_err("configure ring buffer fail\n");
2064                 goto out_free;
2065         }
2066         st->irq = client->irq;
2067         result = inv_mpu_probe_trigger(indio_dev);
2068         if (result) {
2069                 pr_err("trigger probe fail\n");
2070                 goto out_unreg_ring;
2071         }
2072
2073         /* Tell the i2c counter, we have an IRQ */
2074         INV_I2C_SETIRQ(IRQ_MPU, client->irq);
2075
2076         result = iio_device_register(indio_dev);
2077         if (result) {
2078                 pr_err("IIO device register fail\n");
2079                 goto out_remove_trigger;
2080         }
2081
2082         if (INV_MPU6050 == st->chip_type ||
2083             INV_MPU6500 == st->chip_type) {
2084                 result = inv_create_dmp_sysfs(indio_dev);
2085                 if (result) {
2086                         pr_err("create dmp sysfs failed\n");
2087                         goto out_unreg_iio;
2088                 }
2089         }
2090
2091         INIT_KFIFO(st->timestamps);
2092         spin_lock_init(&st->time_stamp_lock);
2093         dev_info(&client->dev, "%s is ready to go!\n",
2094                                         indio_dev->name);
2095
2096         return 0;
2097 out_unreg_iio:
2098         iio_device_unregister(indio_dev);
2099 out_remove_trigger:
2100         if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
2101                 inv_mpu_remove_trigger(indio_dev);
2102 out_unreg_ring:
2103         inv_mpu_unconfigure_ring(indio_dev);
2104 out_free:
2105 #ifdef INV_KERNEL_3_10
2106     iio_device_free(indio_dev);
2107 #else
2108         iio_free_device(indio_dev);
2109 #endif
2110 out_no_free:
2111         dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
2112
2113         return -EIO;
2114 }
2115
2116 static void inv_mpu_shutdown(struct i2c_client *client)
2117 {
2118         struct iio_dev *indio_dev = i2c_get_clientdata(client);
2119         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
2120         struct inv_reg_map_s *reg;
2121         int result;
2122
2123         reg = &st->reg;
2124         dev_dbg(&client->adapter->dev, "Shutting down %s...\n", st->hw->name);
2125
2126         /* reset to make sure previous state are not there */
2127         result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET);
2128         if (result)
2129                 dev_err(&client->adapter->dev, "Failed to reset %s\n",
2130                         st->hw->name);
2131         msleep(POWER_UP_TIME);
2132         /* turn off power to ensure gyro engine is off */
2133         result = st->set_power_state(st, false);
2134         if (result)
2135                 dev_err(&client->adapter->dev, "Failed to turn off %s\n",
2136                         st->hw->name);
2137 }
2138
2139 /**
2140  *  inv_mpu_remove() - remove function.
2141  */
2142 static int inv_mpu_remove(struct i2c_client *client)
2143 {
2144         struct iio_dev *indio_dev = i2c_get_clientdata(client);
2145         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
2146         kfifo_free(&st->timestamps);
2147         iio_device_unregister(indio_dev);
2148         if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
2149                 inv_mpu_remove_trigger(indio_dev);
2150         inv_mpu_unconfigure_ring(indio_dev);
2151 #ifdef INV_KERNEL_3_10
2152     iio_device_free(indio_dev);
2153 #else
2154         iio_free_device(indio_dev);
2155 #endif
2156
2157         dev_info(&client->adapter->dev, "inv-mpu-iio module removed.\n");
2158
2159         return 0;
2160 }
2161
2162 #ifdef CONFIG_PM
2163 static int inv_mpu_resume(struct device *dev)
2164 {
2165         struct inv_mpu_iio_s *st =
2166                         iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
2167         pr_debug("%s inv_mpu_resume\n", st->hw->name);
2168         return st->set_power_state(st, true);
2169 }
2170
2171 static int inv_mpu_suspend(struct device *dev)
2172 {
2173         struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
2174         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
2175         int result;
2176
2177         pr_debug("%s inv_mpu_suspend\n", st->hw->name);
2178         mutex_lock(&indio_dev->mlock);
2179         result = 0;
2180         if ((!st->chip_config.dmp_on) ||
2181                 (!st->chip_config.enable) ||
2182                 (!st->chip_config.dmp_event_int_on))
2183                 result = st->set_power_state(st, false);
2184         mutex_unlock(&indio_dev->mlock);
2185
2186         return result;
2187 }
2188 static const struct dev_pm_ops inv_mpu_pmops = {
2189         SET_SYSTEM_SLEEP_PM_OPS(inv_mpu_suspend, inv_mpu_resume)
2190 };
2191 #define INV_MPU_PMOPS (&inv_mpu_pmops)
2192 #else
2193 #define INV_MPU_PMOPS NULL
2194 #endif /* CONFIG_PM */
2195
2196 static const u16 normal_i2c[] = { I2C_CLIENT_END };
2197 /* device id table is used to identify what device can be
2198  * supported by this driver
2199  */
2200 static const struct i2c_device_id inv_mpu_id[] = {
2201         {"itg3500", INV_ITG3500},
2202         {"mpu3050", INV_MPU3050},
2203         {"mpu6050", INV_MPU6050},
2204         {"mpu9150", INV_MPU9150},
2205         {"mpu6500", INV_MPU6500},
2206         {"mpu9250", INV_MPU9250},
2207         {"mpu6xxx", INV_MPU6XXX},
2208         {"mpu6515", INV_MPU6515},
2209         {}
2210 };
2211
2212 MODULE_DEVICE_TABLE(i2c, inv_mpu_id);
2213
2214 static const struct of_device_id inv_mpu_of_match[] = {
2215         { .compatible = "invensense,itg3500", },
2216         { .compatible = "invensense,mpu3050", },
2217         { .compatible = "invensense,mpu6050", },
2218         { .compatible = "invensense,mpu9150", },
2219         { .compatible = "invensense,mpu6500", },
2220         { .compatible = "invensense,mpu9250", },
2221         { .compatible = "invensense,mpu6xxx", },
2222         { .compatible = "invensense,mpu9350", },
2223         { .compatible = "invensense,mpu6515", },
2224         {}
2225 };
2226
2227 MODULE_DEVICE_TABLE(of, inv_mpu_of_match);
2228
2229 static struct i2c_driver inv_mpu_driver = {
2230         .class = I2C_CLASS_HWMON,
2231         .probe          =       inv_mpu_probe,
2232         .remove         =       inv_mpu_remove,
2233         .shutdown       =       inv_mpu_shutdown,
2234         .id_table       =       inv_mpu_id,
2235         .driver = {
2236                 .owner  =       THIS_MODULE,
2237                 .name   =       "inv-mpu-iio",
2238                 .pm     =       INV_MPU_PMOPS,
2239                 .of_match_table = of_match_ptr(inv_mpu_of_match),
2240         },
2241         .address_list = normal_i2c,
2242 };
2243
2244 static int __init inv_mpu_init(void)
2245 {
2246         int result = i2c_add_driver(&inv_mpu_driver);
2247     pr_info("%s:%d\n", __func__, __LINE__);
2248         if (result) {
2249                 pr_err("failed\n");
2250                 return result;
2251         }
2252         return 0;
2253 }
2254
2255 static void __exit inv_mpu_exit(void)
2256 {
2257         i2c_del_driver(&inv_mpu_driver);
2258 }
2259
2260 module_init(inv_mpu_init);
2261 module_exit(inv_mpu_exit);
2262
2263 MODULE_AUTHOR("Invensense Corporation");
2264 MODULE_DESCRIPTION("Invensense device driver");
2265 MODULE_LICENSE("GPL");
2266 MODULE_ALIAS("inv-mpu-iio");
2267
2268 /**
2269  *  @}
2270  */