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 static int inv_switch_engine(struct inv_mpu_iio_s *st, bool en, u32 mask)
 104 {
 105         struct inv_reg_map_s *reg;
 106         u8 data, mgmt_1;
 107         int result;
 108         reg = &st->reg;
 109         /* switch clock needs to be careful. Only when gyro is on, can
 110            clock source be switched to gyro. Otherwise, it must be set to
 111            internal clock */
 112         if (BIT_PWR_GYRO_STBY == mask) {
 113                 result = inv_plat_read(st, reg->pwr_mgmt_1, 1, &mgmt_1);
 114                 if (result)
 115                         return result;
 116
 117                 mgmt_1 &= ~BIT_CLK_MASK;
 118         }
 119
 120         if ((BIT_PWR_GYRO_STBY == mask) && (!en)) {
 121                 /* turning off gyro requires switch to internal clock first.
 122                    Then turn off gyro engine */
 123                 mgmt_1 |= INV_CLK_INTERNAL;
 124                 result = inv_plat_single_write(st, reg->pwr_mgmt_1,
 125                                                 mgmt_1);
 126                 if (result)
 127                         return result;
 128         }
 129
 130         result = inv_plat_read(st, reg->pwr_mgmt_2, 1, &data);
 131         if (result)
 132                 return result;
 133         if (en)
 134                 data &= (~mask);
 135         else
 136                 data |= mask;
 137         result = inv_plat_single_write(st, reg->pwr_mgmt_2, data);
 138         if (result)
 139                 return result;
 140
 141         if ((BIT_PWR_GYRO_STBY == mask) && en) {
 142                 /* only gyro on needs sensor up time */
 143                 msleep(SENSOR_UP_TIME);
 144                 /* after gyro is on & stable, switch internal clock to PLL */
 145                 mgmt_1 |= INV_CLK_PLL;
 146                 result = inv_plat_single_write(st, reg->pwr_mgmt_1,
 147                                                 mgmt_1);
 148                 if (result)
 149                         return result;
 150         }
 151         if ((BIT_PWR_ACCL_STBY == mask) && en)
 152                 msleep(REG_UP_TIME);
 153
 154         return 0;
 155 }
 156
 157 /**
 158  *  inv_lpa_freq() - store current low power frequency setting.
 159  */
 160 static int inv_lpa_freq(struct inv_mpu_iio_s *st, int lpa_freq)
 161 {
 162         unsigned long result;
 163         u8 d;
 164         const u8 mpu6500_lpa_mapping[] = {2, 4, 6, 7};
 165
 166         if (lpa_freq > MAX_LPA_FREQ_PARAM)
 167                 return -EINVAL;
 168
 169         if (INV_MPU6500 == st->chip_type) {
 170                 d = mpu6500_lpa_mapping[lpa_freq];
 171                 result = inv_plat_single_write(st, REG_6500_LP_ACCEL_ODR, d);
 172                 if (result)
 173                         return result;
 174         }
 175         st->chip_config.lpa_freq = lpa_freq;
 176
 177         return 0;
 178 }
 179
 180 static int set_power_itg(struct inv_mpu_iio_s *st, bool power_on)
 181 {
 182         struct inv_reg_map_s *reg;
 183         u8 data;
 184         int result;
 185
 186         if ((!power_on) == st->chip_config.is_asleep)
 187                 return 0;
 188         reg = &st->reg;
 189         if (power_on)
 190                 data = 0;
 191         else
 192                 data = BIT_SLEEP;
 193         result = inv_plat_single_write(st, reg->pwr_mgmt_1, data);
 194         if (result)
 195                 return result;
 196
 197         if (power_on) {
 198                 if (INV_MPU6500 == st->chip_type)
 199                         msleep(POWER_UP_TIME);
 200                 else
 201                         msleep(REG_UP_TIME);
 202         }
 203
 204         st->chip_config.is_asleep = !power_on;
 205
 206         return 0;
 207 }
 208
 209 /**
 210  *  inv_init_config() - Initialize hardware, disable FIFO.
 211  *  @indio_dev: Device driver instance.
 212  *  Initial configuration:
 213  *  FSR: +/- 2000DPS
 214  *  DLPF: 42Hz
 215  *  FIFO rate: 50Hz
 216  */
 217 static int inv_init_config(struct iio_dev *indio_dev)
 218 {
 219         struct inv_reg_map_s *reg;
 220         int result;
 221         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 222
 223
 224         reg = &st->reg;
 225
 226 #if 0
 227         /* set int latch en */
 228         result = inv_plat_single_write(st, REG_INT_PIN_CFG, 0x20);
 229         if (result)
 230                 return result;
 231 #endif
 232         result = inv_plat_single_write(st, reg->gyro_config,
 233                 INV_FSR_2000DPS << GYRO_CONFIG_FSR_SHIFT);
 234         if (result)
 235                 return result;
 236
 237         st->chip_config.fsr = INV_FSR_2000DPS;
 238
 239         result = inv_plat_single_write(st, reg->lpf, INV_FILTER_42HZ);
 240         if (result)
 241                 return result;
 242         st->chip_config.lpf = INV_FILTER_42HZ;
 243
 244         result = inv_plat_single_write(st, reg->sample_rate_div,
 245                                         ONE_K_HZ / INIT_FIFO_RATE - 1);
 246         if (result)
 247                 return result;
 248         st->chip_config.fifo_rate = INIT_FIFO_RATE;
 249         st->chip_config.new_fifo_rate = INIT_FIFO_RATE;
 250         st->irq_dur_ns            = INIT_DUR_TIME;
 251         st->chip_config.prog_start_addr = DMP_START_ADDR;
 252         st->chip_config.dmp_output_rate = INIT_DMP_OUTPUT_RATE;
 253         st->self_test.samples = INIT_ST_SAMPLES;
 254         st->self_test.threshold = INIT_ST_THRESHOLD;
 255         if (INV_ITG3500 != st->chip_type) {
 256                 st->chip_config.accl_fs = INV_FS_02G;
 257                 result = inv_plat_single_write(st, reg->accl_config,
 258                         (INV_FS_02G << ACCL_CONFIG_FSR_SHIFT));
 259                 if (result)
 260                         return result;
 261                 st->tap.time = INIT_TAP_TIME;
 262                 st->tap.thresh = INIT_TAP_THRESHOLD;
 263                 st->tap.min_count = INIT_TAP_MIN_COUNT;
 264                 st->smd.threshold = MPU_INIT_SMD_THLD;
 265                 st->smd.delay     = MPU_INIT_SMD_DELAY_THLD;
 266                 st->smd.delay2    = MPU_INIT_SMD_DELAY2_THLD;
 267
 268                 result = inv_plat_single_write(st, REG_ACCEL_MOT_DUR,
 269                                                 INIT_MOT_DUR);
 270                 if (result)
 271                         return result;
 272                 st->mot_int.mot_dur = INIT_MOT_DUR;
 273
 274                 result = inv_plat_single_write(st, REG_ACCEL_MOT_THR,
 275                                                 INIT_MOT_THR);
 276                 if (result)
 277                         return result;
 278                 st->mot_int.mot_thr = INIT_MOT_THR;
 279         }
 280
 281         return 0;
 282 }
 283
 284 /**
 285  *  inv_compass_scale_show() - show compass scale.
 286  */
 287 static int inv_compass_scale_show(struct inv_mpu_iio_s *st, int *scale)
 288 {
 289         if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id)
 290                 *scale = DATA_AKM8975_SCALE;
 291         else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id)
 292                 *scale = DATA_AKM8972_SCALE;
 293         else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id)
 294                 if (st->compass_scale)
 295                         *scale = DATA_AKM8963_SCALE1;
 296                 else
 297                         *scale = DATA_AKM8963_SCALE0;
 298         else
 299                 return -EINVAL;
 300
 301         return IIO_VAL_INT;
 302 }
 303
 304 /**
 305  *  inv_sensor_show() - Read gyro/accel data directly from registers.
 306  */
 307 static int inv_sensor_show(struct inv_mpu_iio_s  *st, int reg, int axis,
 308                                         int *val)
 309 {
 310         int ind, result;
 311         u8 d[2];
 312
 313         ind = (axis - IIO_MOD_X) * 2;
 314         result = i2c_smbus_read_i2c_block_data(st->client,
 315                                                reg + ind, 2, d);
 316         if (result != 2)
 317                 return -EINVAL;
 318         *val = (short)be16_to_cpup((__be16 *)(d));
 319
 320         return IIO_VAL_INT;
 321 }
 322
 323 /**
 324  *  mpu_read_raw() - read raw method.
 325  */
 326 static int mpu_read_raw(struct iio_dev *indio_dev,
 327                         struct iio_chan_spec const *chan,
 328                         int *val, int *val2, long mask)
 329 {
 330         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
 331         int result;
 332
 333         switch (mask) {
 334         case 0:
 335                 /* if enabled, power is on already */
 336                 if (!st->chip_config.enable)
 337                         return -EBUSY;
 338                 switch (chan->type) {
 339                 case IIO_ANGL_VEL:
 340                         if (!st->chip_config.gyro_enable)
 341                                 return -EPERM;
 342                         return inv_sensor_show(st, st->reg.raw_gyro,
 343                                                 chan->channel2, val);
 344                 case IIO_ACCEL:
 345                         if (!st->chip_config.accl_enable)
 346                                 return -EPERM;
 347                         return inv_sensor_show(st, st->reg.raw_accl,
 348                                                 chan->channel2, val);
 349                 case IIO_MAGN:
 350                         if (!st->chip_config.compass_enable)
 351                                 return -EPERM;
 352                         *val = st->raw_compass[chan->channel2 - IIO_MOD_X];
 353                         return IIO_VAL_INT;
 354                 case IIO_QUATERNION:
 355                         if (!(st->chip_config.dmp_on
 356                                 && st->chip_config.quaternion_on))
 357                                 return -EPERM;
 358                         if (IIO_MOD_R == chan->channel2)
 359                                 *val = st->raw_quaternion[0];
 360                         else
 361                                 *val = st->raw_quaternion[chan->channel2 -
 362                                                           IIO_MOD_X + 1];
 363                         return IIO_VAL_INT;
 364                 default:
 365                         return -EINVAL;
 366                 }
 367         case IIO_CHAN_INFO_SCALE:
 368                 switch (chan->type) {
 369                 case IIO_ANGL_VEL:
 370                 {
 371                         const s16 gyro_scale[] = {250, 500, 1000, 2000};
 372
 373                         *val = gyro_scale[st->chip_config.fsr];
 374
 375                         return IIO_VAL_INT;
 376                 }
 377                 case IIO_ACCEL:
 378                 {
 379                         const s16 accel_scale[] = {2, 4, 8, 16};
 380                         *val = accel_scale[st->chip_config.accl_fs] *
 381                                         st->chip_info.multi;
 382                         return IIO_VAL_INT;
 383                 }
 384                 case IIO_MAGN:
 385                         return inv_compass_scale_show(st, val);
 386                 default:
 387                         return -EINVAL;
 388                 }
 389         case IIO_CHAN_INFO_CALIBBIAS:
 390                 if (st->chip_config.self_test_run_once == 0) {
 391                         /* This can only be run when enable is zero */
 392                         if (st->chip_config.enable)
 393                                 return -EBUSY;
 394                         mutex_lock(&indio_dev->mlock);
 395
 396                         result = inv_power_up_self_test(st);
 397                         if (result)
 398                                 goto error_info_calibbias;
 399                         result = inv_do_test(st, 0,  st->gyro_bias,
 400                                 st->accel_bias);
 401                         if (result)
 402                                 goto error_info_calibbias;
 403                         st->chip_config.self_test_run_once = 1;
 404 error_info_calibbias:
 405                         /* Reset Accel and Gyro full scale range
 406                            back to default value */
 407                         inv_recover_setting(st);
 408                         mutex_unlock(&indio_dev->mlock);
 409                 }
 410
 411                 switch (chan->type) {
 412                 case IIO_ANGL_VEL:
 413                         *val = st->gyro_bias[chan->channel2 - IIO_MOD_X];
 414                         return IIO_VAL_INT;
 415                 case IIO_ACCEL:
 416                         *val = st->accel_bias[chan->channel2 - IIO_MOD_X] *
 417                                         st->chip_info.multi;
 418                         return IIO_VAL_INT;
 419                 default:
 420                         return -EINVAL;
 421                 }
 422         case IIO_CHAN_INFO_OFFSET:
 423                 switch (chan->type) {
 424                 case IIO_ACCEL:
 425                         *val = st->input_accel_bias[chan->channel2 - IIO_MOD_X];
 426                         return IIO_VAL_INT;
 427                 default:
 428                         return -EINVAL;
 429                 }
 430         default:
 431                 return -EINVAL;
 432         }
 433 }
 434
 435 /**
 436  *  inv_write_fsr() - Configure the gyro's scale range.
 437  */
 438 static int inv_write_fsr(struct inv_mpu_iio_s *st, int fsr)
 439 {
 440         struct inv_reg_map_s *reg;
 441         int result;
 442         reg = &st->reg;
 443         if ((fsr < 0) || (fsr > MAX_GYRO_FS_PARAM))
 444                 return -EINVAL;
 445         if (fsr == st->chip_config.fsr)
 446                 return 0;
 447
 448         if (INV_MPU3050 == st->chip_type)
 449                 result = inv_plat_single_write(st, reg->lpf,
 450                         (fsr << GYRO_CONFIG_FSR_SHIFT) | st->chip_config.lpf);
 451         else
 452                 result = inv_plat_single_write(st, reg->gyro_config,
 453                         fsr << GYRO_CONFIG_FSR_SHIFT);
 454
 455         if (result)
 456                 return result;
 457         st->chip_config.fsr = fsr;
 458
 459         return 0;
 460 }
 461
 462 /**
 463  *  inv_write_accel_fs() - Configure the accelerometer's scale range.
 464  */
 465 static int inv_write_accel_fs(struct inv_mpu_iio_s *st, int fs)
 466 {
 467         int result;
 468         struct inv_reg_map_s *reg;
 469
 470         reg = &st->reg;
 471         if (fs < 0 || fs > MAX_ACCL_FS_PARAM)
 472                 return -EINVAL;
 473         if (fs == st->chip_config.accl_fs)
 474                 return 0;
 475         if (INV_MPU3050 == st->chip_type)
 476                 result = st->mpu_slave->set_fs(st, fs);
 477         else
 478                 result = inv_plat_single_write(st, reg->accl_config,
 479                                 (fs << ACCL_CONFIG_FSR_SHIFT));
 480         if (result)
 481                 return result;
 482
 483         st->chip_config.accl_fs = fs;
 484
 485         return 0;
 486 }
 487
 488 /**
 489  *  inv_write_compass_scale() - Configure the compass's scale range.
 490  */
 491 static int inv_write_compass_scale(struct inv_mpu_iio_s  *st, int data)
 492 {
 493         char d, en;
 494         int result;
 495         if (COMPASS_ID_AK8963 != st->plat_data.sec_slave_id)
 496                 return 0;
 497         en = !!data;
 498         if (st->compass_scale == en)
 499                 return 0;
 500         d = (DATA_AKM_MODE_SM | (st->compass_scale << AKM8963_SCALE_SHIFT));
 501         result = inv_plat_single_write(st, REG_I2C_SLV1_DO, d);
 502         if (result)
 503                 return result;
 504         st->compass_scale = en;
 505
 506         return 0;
 507 }
 508
 509 /**
 510  *  mpu_write_raw() - write raw method.
 511  */
 512 static int mpu_write_raw(struct iio_dev *indio_dev,
 513                                struct iio_chan_spec const *chan,
 514                                int val,
 515                                int val2,
 516                                long mask) {
 517         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
 518         int result;
 519
 520         if (st->chip_config.enable)
 521                 return -EBUSY;
 522         mutex_lock(&indio_dev->mlock);
 523         result = st->set_power_state(st, true);
 524         if (result) {
 525                 mutex_unlock(&indio_dev->mlock);
 526                 return result;
 527         }
 528
 529         switch (mask) {
 530         case IIO_CHAN_INFO_SCALE:
 531                 switch (chan->type) {
 532                 case IIO_ANGL_VEL:
 533                         result = inv_write_fsr(st, val);
 534                         break;
 535                 case IIO_ACCEL:
 536                         result = inv_write_accel_fs(st, val);
 537                         break;
 538                 case IIO_MAGN:
 539                         result = inv_write_compass_scale(st, val);
 540                         break;
 541                 default:
 542                         result = -EINVAL;
 543                         break;
 544                 }
 545                 break;
 546         case IIO_CHAN_INFO_OFFSET:
 547                 switch (chan->type) {
 548                 case IIO_ACCEL:
 549                         if (!st->chip_config.firmware_loaded) {
 550                                 result = -EPERM;
 551                                 goto error_write_raw;
 552                         }
 553                         result = inv_set_accel_bias_dmp(st);
 554                         if (result)
 555                                 goto error_write_raw;
 556                         st->input_accel_bias[chan->channel2 - IIO_MOD_X] = val;
 557                         result = 0;
 558                         break;
 559                 default:
 560                         result = -EINVAL;
 561                         break;
 562                 }
 563                 break;
 564         default:
 565                 result = -EINVAL;
 566                 break;
 567         }
 568
 569 error_write_raw:
 570         result |= st->set_power_state(st, false);
 571         mutex_unlock(&indio_dev->mlock);
 572
 573         return result;
 574 }
 575
 576 /**
 577  *  inv_fifo_rate_store() - Set fifo rate.
 578  */
 579 static int inv_fifo_rate_store(struct inv_mpu_iio_s *st, int fifo_rate)
 580 {
 581         if ((fifo_rate < MIN_FIFO_RATE) || (fifo_rate > MAX_FIFO_RATE))
 582                 return -EINVAL;
 583
 584         if (st->chip_config.has_compass) {
 585                 st->compass_divider = COMPASS_RATE_SCALE * fifo_rate /
 586                                         ONE_K_HZ;
 587                 if (st->compass_divider > 0)
 588                         st->compass_divider -= 1;
 589                 st->compass_counter = 0;
 590         }
 591         st->irq_dur_ns = (ONE_K_HZ / fifo_rate) * NSEC_PER_MSEC;
 592         st->chip_config.new_fifo_rate = fifo_rate;
 593
 594         return 0;
 595 }
 596
 597 /**
 598  *  inv_reg_dump_show() - Register dump for testing.
 599  */
 600 static ssize_t inv_reg_dump_show(struct device *dev,
 601         struct device_attribute *attr, char *buf)
 602 {
 603         int ii;
 604         char data;
 605         ssize_t bytes_printed = 0;
 606         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 607         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 608
 609         mutex_lock(&indio_dev->mlock);
 610         if (!st->chip_config.enable)
 611                 st->set_power_state(st, true);
 612         for (ii = 0; ii < st->hw->num_reg; ii++) {
 613                 /* don't read fifo r/w register */
 614                 if (ii == st->reg.fifo_r_w)
 615                         data = 0;
 616                 else
 617                         inv_plat_read(st, ii, 1, &data);
 618                 bytes_printed += sprintf(buf + bytes_printed, "%#2x: %#2x\n",
 619                                          ii, data);
 620         }
 621         if (!st->chip_config.enable)
 622                 st->set_power_state(st, false);
 623         mutex_unlock(&indio_dev->mlock);
 624
 625         return bytes_printed;
 626 }
 627
 628 int write_be32_key_to_mem(struct inv_mpu_iio_s *st,
 629                                         u32 data, int key)
 630 {
 631         cpu_to_be32s(&data);
 632         return mem_w_key(key, sizeof(data), (u8 *)&data);
 633 }
 634
 635 /**
 636  * inv_quaternion_on() -  calling this function will store
 637  *                                 current quaternion on
 638  */
 639 static int inv_quaternion_on(struct inv_mpu_iio_s *st,
 640                                  struct iio_buffer *ring, bool en)
 641 {
 642         st->chip_config.quaternion_on = en;
 643         if (!en) {
 644                 clear_bit(INV_MPU_SCAN_QUAT_R, ring->scan_mask);
 645                 clear_bit(INV_MPU_SCAN_QUAT_X, ring->scan_mask);
 646                 clear_bit(INV_MPU_SCAN_QUAT_Y, ring->scan_mask);
 647                 clear_bit(INV_MPU_SCAN_QUAT_Z, ring->scan_mask);
 648         }
 649
 650         return 0;
 651 }
 652
 653 /**
 654  * inv_dmp_attr_store() -  calling this function will store current
 655  *                        dmp parameter settings
 656  */
 657 static ssize_t inv_dmp_attr_store(struct device *dev,
 658         struct device_attribute *attr, const char *buf, size_t count)
 659 {
 660         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 661         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 662         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 663         int result, data;
 664
 665         mutex_lock(&indio_dev->mlock);
 666         if (st->chip_config.enable) {
 667                 result = -EBUSY;
 668                 goto dmp_attr_store_fail;
 669         }
 670         if (this_attr->address <= ATTR_DMP_DISPLAY_ORIENTATION_ON) {
 671                 if (!st->chip_config.firmware_loaded) {
 672                         result = -EINVAL;
 673                         goto dmp_attr_store_fail;
 674                 }
 675                 result = st->set_power_state(st, true);
 676                 if (result)
 677                         goto dmp_attr_store_fail;
 678         }
 679
 680         result = kstrtoint(buf, 10, &data);
 681         if (result)
 682                 goto dmp_attr_store_fail;
 683         switch (this_attr->address) {
 684         case ATTR_DMP_SMD_ENABLE:
 685         {
 686                 u8 on[] = {0, 1};
 687                 u8 off[] = {0, 0};
 688                 u8 *d;
 689                 if (data)
 690                         d = on;
 691                 else
 692                         d = off;
 693                 result = mem_w_key(KEY_SMD_ENABLE, ARRAY_SIZE(on), d);
 694                 if (result)
 695                         goto dmp_attr_store_fail;
 696                 st->chip_config.smd_enable = !!data;
 697         }
 698                 break;
 699         case ATTR_DMP_SMD_THLD:
 700                 if (data < 0 || data > SHRT_MAX)
 701                         goto dmp_attr_store_fail;
 702                 result = write_be32_key_to_mem(st, data << 16,
 703                                                 KEY_SMD_ACCEL_THLD);
 704                 if (result)
 705                         goto dmp_attr_store_fail;
 706                 st->smd.threshold = data;
 707                 break;
 708         case ATTR_DMP_SMD_DELAY_THLD:
 709                 if (data < 0 || data > INT_MAX / MPU_DEFAULT_DMP_FREQ)
 710                         goto dmp_attr_store_fail;
 711                 result = write_be32_key_to_mem(st, data * MPU_DEFAULT_DMP_FREQ,
 712                                                 KEY_SMD_DELAY_THLD);
 713                 if (result)
 714                         goto dmp_attr_store_fail;
 715                 st->smd.delay = data;
 716                 break;
 717         case ATTR_DMP_SMD_DELAY_THLD2:
 718                 if (data < 0 || data > INT_MAX / MPU_DEFAULT_DMP_FREQ)
 719                         goto dmp_attr_store_fail;
 720                 result = write_be32_key_to_mem(st, data * MPU_DEFAULT_DMP_FREQ,
 721                                                 KEY_SMD_DELAY2_THLD);
 722                 if (result)
 723                         goto dmp_attr_store_fail;
 724                 st->smd.delay2 = data;
 725                 break;
 726         case ATTR_DMP_TAP_ON:
 727                 result = inv_enable_tap_dmp(st, !!data);
 728                 if (result)
 729                         goto dmp_attr_store_fail;
 730                 st->chip_config.tap_on = !!data;
 731                 break;
 732         case ATTR_DMP_TAP_THRESHOLD: {
 733                 const char ax[] = {INV_TAP_AXIS_X, INV_TAP_AXIS_Y,
 734                                                         INV_TAP_AXIS_Z};
 735                 int i;
 736                 if (data < 0 || data > USHRT_MAX) {
 737                         result = -EINVAL;
 738                         goto dmp_attr_store_fail;
 739                 }
 740                 for (i = 0; i < ARRAY_SIZE(ax); i++) {
 741                         result = inv_set_tap_threshold_dmp(st, ax[i], data);
 742                         if (result)
 743                                 goto dmp_attr_store_fail;
 744                 }
 745                 st->tap.thresh = data;
 746                 break;
 747         }
 748         case ATTR_DMP_TAP_MIN_COUNT:
 749                 if (data < 0 || data > USHRT_MAX) {
 750                         result = -EINVAL;
 751                         goto dmp_attr_store_fail;
 752                 }
 753                 result = inv_set_min_taps_dmp(st, data);
 754                 if (result)
 755                         goto dmp_attr_store_fail;
 756                 st->tap.min_count = data;
 757                 break;
 758         case ATTR_DMP_TAP_TIME:
 759                 if (data < 0 || data > USHRT_MAX) {
 760                         result = -EINVAL;
 761                         goto dmp_attr_store_fail;
 762                 }
 763                 result = inv_set_tap_time_dmp(st, data);
 764                 if (result)
 765                         goto dmp_attr_store_fail;
 766                 st->tap.time = data;
 767                 break;
 768         case ATTR_DMP_DISPLAY_ORIENTATION_ON:
 769                 result = inv_set_display_orient_interrupt_dmp(st, !!data);
 770                 if (result)
 771                         goto dmp_attr_store_fail;
 772                 st->chip_config.display_orient_on = !!data;
 773                 break;
 774         /* from here, power of chip is not turned on */
 775         case ATTR_DMP_ON:
 776                 st->chip_config.dmp_on = !!data;
 777                 break;
 778         case ATTR_DMP_INT_ON:
 779                 st->chip_config.dmp_int_on = !!data;
 780                 break;
 781         case ATTR_DMP_EVENT_INT_ON:
 782                 st->chip_config.dmp_event_int_on = !!data;
 783                 break;
 784         case ATTR_DMP_OUTPUT_RATE:
 785                 if (data <= 0 || data > MAX_DMP_OUTPUT_RATE) {
 786                         result = -EINVAL;
 787                         goto dmp_attr_store_fail;
 788                 }
 789                 st->chip_config.dmp_output_rate = data;
 790                 if (st->chip_config.has_compass) {
 791                         st->compass_dmp_divider = COMPASS_RATE_SCALE * data /
 792                                                         ONE_K_HZ;
 793                         if (st->compass_dmp_divider > 0)
 794                                 st->compass_dmp_divider -= 1;
 795                         st->compass_counter = 0;
 796                 }
 797                 break;
 798         case ATTR_DMP_QUATERNION_ON:
 799                 result = inv_quaternion_on(st, indio_dev->buffer, !!data);
 800                 break;
 801 #ifdef CONFIG_INV_TESTING
 802         case ATTR_DEBUG_SMD_ENABLE_TESTP1:
 803         {
 804                 u8 d[] = {0x42};
 805                 result = st->set_power_state(st, true);
 806                 if (result)
 807                         goto dmp_attr_store_fail;
 808                 result = mem_w_key(KEY_SMD_ENABLE_TESTPT1, ARRAY_SIZE(d), d);
 809                 if (result)
 810                         goto dmp_attr_store_fail;
 811         }
 812                 break;
 813         case ATTR_DEBUG_SMD_ENABLE_TESTP2:
 814         {
 815                 u8 d[] = {0x42};
 816                 result = st->set_power_state(st, true);
 817                 if (result)
 818                         goto dmp_attr_store_fail;
 819                 result = mem_w_key(KEY_SMD_ENABLE_TESTPT2, ARRAY_SIZE(d), d);
 820                 if (result)
 821                         goto dmp_attr_store_fail;
 822         }
 823                 break;
 824 #endif
 825         default:
 826                 result = -EINVAL;
 827                 goto dmp_attr_store_fail;
 828         }
 829
 830 dmp_attr_store_fail:
 831         if ((this_attr->address <= ATTR_DMP_DISPLAY_ORIENTATION_ON) &&
 832                                         (!st->chip_config.enable))
 833                 result |= st->set_power_state(st, false);
 834         mutex_unlock(&indio_dev->mlock);
 835         if (result)
 836                 return result;
 837
 838         return count;
 839 }
 840
 841 /**
 842  * inv_attr_show() -  calling this function will show current
 843  *                        dmp parameters.
 844  */
 845 static ssize_t inv_attr_show(struct device *dev,
 846         struct device_attribute *attr, char *buf)
 847 {
 848         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 849         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 850         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 851         int result;
 852         s8 *m;
 853
 854         switch (this_attr->address) {
 855         case ATTR_DMP_SMD_ENABLE:
 856                 return sprintf(buf, "%d\n", st->chip_config.smd_enable);
 857         case ATTR_DMP_SMD_THLD:
 858                 return sprintf(buf, "%d\n", st->smd.threshold);
 859         case ATTR_DMP_SMD_DELAY_THLD:
 860                 return sprintf(buf, "%d\n", st->smd.delay);
 861         case ATTR_DMP_SMD_DELAY_THLD2:
 862                 return sprintf(buf, "%d\n", st->smd.delay2);
 863         case ATTR_DMP_TAP_ON:
 864                 return sprintf(buf, "%d\n", st->chip_config.tap_on);
 865         case ATTR_DMP_TAP_THRESHOLD:
 866                 return sprintf(buf, "%d\n", st->tap.thresh);
 867         case ATTR_DMP_TAP_MIN_COUNT:
 868                 return sprintf(buf, "%d\n", st->tap.min_count);
 869         case ATTR_DMP_TAP_TIME:
 870                 return sprintf(buf, "%d\n", st->tap.time);
 871         case ATTR_DMP_DISPLAY_ORIENTATION_ON:
 872                 return sprintf(buf, "%d\n",
 873                         st->chip_config.display_orient_on);
 874
 875         case ATTR_DMP_ON:
 876                 return sprintf(buf, "%d\n", st->chip_config.dmp_on);
 877         case ATTR_DMP_INT_ON:
 878                 return sprintf(buf, "%d\n", st->chip_config.dmp_int_on);
 879         case ATTR_DMP_EVENT_INT_ON:
 880                 return sprintf(buf, "%d\n", st->chip_config.dmp_event_int_on);
 881         case ATTR_DMP_OUTPUT_RATE:
 882                 return sprintf(buf, "%d\n",
 883                                 st->chip_config.dmp_output_rate);
 884         case ATTR_DMP_QUATERNION_ON:
 885                 return sprintf(buf, "%d\n", st->chip_config.quaternion_on);
 886
 887         case ATTR_MOTION_LPA_ON:
 888                 return sprintf(buf, "%d\n", st->mot_int.mot_on);
 889         case ATTR_MOTION_LPA_FREQ:{
 890                 const char *f[] = {"1.25", "5", "20", "40"};
 891                 return sprintf(buf, "%s\n", f[st->chip_config.lpa_freq]);
 892         }
 893         case ATTR_MOTION_LPA_THRESHOLD:
 894                 return sprintf(buf, "%d\n", st->mot_int.mot_thr);
 895
 896         case ATTR_SELF_TEST_SAMPLES:
 897                 return sprintf(buf, "%d\n", st->self_test.samples);
 898         case ATTR_SELF_TEST_THRESHOLD:
 899                 return sprintf(buf, "%d\n", st->self_test.threshold);
 900         case ATTR_GYRO_ENABLE:
 901                 return sprintf(buf, "%d\n", st->chip_config.gyro_enable);
 902         case ATTR_ACCL_ENABLE:
 903                 return sprintf(buf, "%d\n", st->chip_config.accl_enable);
 904         case ATTR_COMPASS_ENABLE:
 905                 return sprintf(buf, "%d\n", st->chip_config.compass_enable);
 906         case ATTR_POWER_STATE:
 907                 return sprintf(buf, "%d\n", !fake_asleep);
 908         case ATTR_FIRMWARE_LOADED:
 909                 return sprintf(buf, "%d\n", st->chip_config.firmware_loaded);
 910         case ATTR_SAMPLING_FREQ:
 911                 return sprintf(buf, "%d\n", st->chip_config.new_fifo_rate);
 912
 913         case ATTR_SELF_TEST:
 914                 if (st->chip_config.enable)
 915                         return -EBUSY;
 916                 mutex_lock(&indio_dev->mlock);
 917                 if (INV_MPU3050 == st->chip_type)
 918                         result = 1;
 919                 else
 920                         result = inv_hw_self_test(st);
 921                 mutex_unlock(&indio_dev->mlock);
 922                 return sprintf(buf, "%d\n", result);
 923
 924         case ATTR_GYRO_MATRIX:
 925                 m = st->plat_data.orientation;
 926                 return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
 927                         m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
 928         case ATTR_ACCL_MATRIX:
 929                 if (st->plat_data.sec_slave_type == SECONDARY_SLAVE_TYPE_ACCEL)
 930                         m = st->plat_data.secondary_orientation;
 931                 else
 932                         m = st->plat_data.orientation;
 933                 return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
 934                         m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
 935         case ATTR_COMPASS_MATRIX:
 936                 if (st->plat_data.sec_slave_type ==
 937                                 SECONDARY_SLAVE_TYPE_COMPASS)
 938                         m = st->plat_data.secondary_orientation;
 939                 else
 940                         return -ENODEV;
 941                 return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
 942                         m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
 943         case ATTR_SECONDARY_NAME:{
 944         const char *n[] = {"0", "AK8975", "AK8972", "AK8963", "BMA250"};
 945         if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id)
 946                 return sprintf(buf, "%s\n", n[1]);
 947         else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id)
 948                 return sprintf(buf, "%s\n", n[2]);
 949         else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id)
 950                 return sprintf(buf, "%s\n", n[3]);
 951         else if (ACCEL_ID_BMA250 == st->plat_data.sec_slave_id)
 952                 return sprintf(buf, "%s\n", n[4]);
 953         else
 954                 return sprintf(buf, "%s\n", n[0]);
 955         }
 956
 957 #ifdef CONFIG_INV_TESTING
 958         case ATTR_REG_WRITE:
 959                 return sprintf(buf, "1\n");
 960         case ATTR_DEBUG_SMD_EXE_STATE:
 961         {
 962                 u8 d[2];
 963
 964                 result = st->set_power_state(st, true);
 965                 mpu_memory_read(st, st->i2c_addr,
 966                                 inv_dmp_get_address(KEY_SMD_EXE_STATE), 2, d);
 967                 return sprintf(buf, "%d\n", (short)be16_to_cpup((__be16 *)(d)));
 968         }
 969         case ATTR_DEBUG_SMD_DELAY_CNTR:
 970         {
 971                 u8 d[4];
 972
 973                 result = st->set_power_state(st, true);
 974                 mpu_memory_read(st, st->i2c_addr,
 975                                 inv_dmp_get_address(KEY_SMD_DELAY_CNTR), 4, d);
 976                 return sprintf(buf, "%d\n", (int)be32_to_cpup((__be32 *)(d)));
 977         }
 978 #endif
 979         default:
 980                 return -EPERM;
 981         }
 982 }
 983
 984 /**
 985  * inv_dmp_display_orient_show() -  calling this function will
 986  *                      show orientation This event must use poll.
 987  */
 988 static ssize_t inv_dmp_display_orient_show(struct device *dev,
 989         struct device_attribute *attr, char *buf)
 990 {
 991         struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
 992         return sprintf(buf, "%d\n", st->display_orient_data);
 993 }
 994
 995 /**
 996  * inv_accel_motion_show() -  calling this function showes motion interrupt.
 997  *                         This event must use poll.
 998  */
 999 static ssize_t inv_accel_motion_show(struct device *dev,
1000         struct device_attribute *attr, char *buf)
1001 {
1002         return sprintf(buf, "1\n");
1003 }
1004
1005 /**
1006  * inv_smd_show() -  calling this function showes smd interrupt.
1007  *                         This event must use poll.
1008  */
1009 static ssize_t inv_smd_show(struct device *dev,
1010         struct device_attribute *attr, char *buf)
1011 {
1012         return sprintf(buf, "1\n");
1013 }
1014
1015 /**
1016  *  inv_temperature_show() - Read temperature data directly from registers.
1017  */
1018 static ssize_t inv_temperature_show(struct device *dev,
1019         struct device_attribute *attr, char *buf)
1020 {
1021
1022         struct iio_dev *indio_dev = dev_get_drvdata(dev);
1023         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1024         struct inv_reg_map_s *reg;
1025         int result, cur_scale, cur_off;
1026         short temp;
1027         long scale_t;
1028         u8 data[2];
1029         const long scale[] = {3834792L, 3158064L, 3340827L};
1030         const long offset[] = {5383314L, 2394184L, 1376256L};
1031
1032         reg = &st->reg;
1033         mutex_lock(&indio_dev->mlock);
1034         if (!st->chip_config.enable)
1035                 result = st->set_power_state(st, true);
1036         else
1037                 result = 0;
1038         if (result) {
1039                 mutex_unlock(&indio_dev->mlock);
1040                 return result;
1041         }
1042         result = inv_plat_read(st, reg->temperature, 2, data);
1043         if (!st->chip_config.enable)
1044                 result |= st->set_power_state(st, false);
1045         mutex_unlock(&indio_dev->mlock);
1046         if (result) {
1047                 pr_err("Could not read temperature register.\n");
1048                 return result;
1049         }
1050         temp = (signed short)(be16_to_cpup((short *)&data[0]));
1051         switch (st->chip_type) {
1052         case INV_MPU3050:
1053                 cur_scale = scale[0];
1054                 cur_off   = offset[0];
1055                 break;
1056         case INV_MPU6050:
1057                 cur_scale = scale[1];
1058                 cur_off   = offset[1];
1059                 break;
1060         case INV_MPU6500:
1061                 cur_scale = scale[2];
1062                 cur_off   = offset[2];
1063                 break;
1064         default:
1065                 return -EINVAL;
1066         };
1067         scale_t = cur_off +
1068                 inv_q30_mult((int)temp << MPU_TEMP_SHIFT, cur_scale);
1069
1070         INV_I2C_INC_TEMPREAD(1);
1071
1072         return sprintf(buf, "%ld %lld\n", scale_t, get_time_ns());
1073 }
1074
1075 /**
1076  * inv_firmware_loaded() -  calling this function will change
1077  *                        firmware load
1078  */
1079 static int inv_firmware_loaded(struct inv_mpu_iio_s *st, int data)
1080 {
1081         if (data)
1082                 return -EINVAL;
1083         st->chip_config.firmware_loaded = 0;
1084         st->chip_config.dmp_on = 0;
1085         st->chip_config.quaternion_on = 0;
1086
1087         return 0;
1088 }
1089
1090 static int inv_switch_gyro_engine(struct inv_mpu_iio_s *st, bool en)
1091 {
1092         return inv_switch_engine(st, en, BIT_PWR_GYRO_STBY);
1093 }
1094
1095 static int inv_switch_accl_engine(struct inv_mpu_iio_s *st, bool en)
1096 {
1097         return inv_switch_engine(st, en, BIT_PWR_ACCL_STBY);
1098 }
1099
1100 /**
1101  *  inv_gyro_enable() - Enable/disable gyro.
1102  */
1103 static int inv_gyro_enable(struct inv_mpu_iio_s *st,
1104                                  struct iio_buffer *ring, bool en)
1105 {
1106         if (en == st->chip_config.gyro_enable)
1107                 return 0;
1108         if (!en) {
1109                 st->chip_config.gyro_fifo_enable = 0;
1110                 clear_bit(INV_MPU_SCAN_GYRO_X, ring->scan_mask);
1111                 clear_bit(INV_MPU_SCAN_GYRO_Y, ring->scan_mask);
1112                 clear_bit(INV_MPU_SCAN_GYRO_Z, ring->scan_mask);
1113         }
1114         st->chip_config.gyro_enable = en;
1115
1116         return 0;
1117 }
1118
1119 /**
1120  *  inv_accl_enable() - Enable/disable accl.
1121  */
1122 static int inv_accl_enable(struct inv_mpu_iio_s *st,
1123                                  struct iio_buffer *ring, bool en)
1124 {
1125         if (en == st->chip_config.accl_enable)
1126                 return 0;
1127         if (!en) {
1128                 st->chip_config.accl_fifo_enable = 0;
1129                 clear_bit(INV_MPU_SCAN_ACCL_X, ring->scan_mask);
1130                 clear_bit(INV_MPU_SCAN_ACCL_Y, ring->scan_mask);
1131                 clear_bit(INV_MPU_SCAN_ACCL_Z, ring->scan_mask);
1132         }
1133         st->chip_config.accl_enable = en;
1134
1135         return 0;
1136 }
1137
1138 /**
1139  * inv_compass_enable() -  calling this function will store compass
1140  *                         enable
1141  */
1142 static int inv_compass_enable(struct inv_mpu_iio_s *st,
1143                                  struct iio_buffer *ring, bool en)
1144 {
1145         if (en == st->chip_config.compass_enable)
1146                 return 0;
1147         if (!en) {
1148                 st->chip_config.compass_fifo_enable = 0;
1149                 clear_bit(INV_MPU_SCAN_MAGN_X, ring->scan_mask);
1150                 clear_bit(INV_MPU_SCAN_MAGN_Y, ring->scan_mask);
1151                 clear_bit(INV_MPU_SCAN_MAGN_Z, ring->scan_mask);
1152         }
1153         st->chip_config.compass_enable = en;
1154
1155         return 0;
1156 }
1157
1158 /**
1159  * inv_attr_store() -  calling this function will store current
1160  *                        non-dmp parameter settings
1161  */
1162 static ssize_t inv_attr_store(struct device *dev,
1163         struct device_attribute *attr, const char *buf, size_t count)
1164 {
1165         struct iio_dev *indio_dev = dev_get_drvdata(dev);
1166         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1167         struct iio_buffer *ring = indio_dev->buffer;
1168         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
1169         int data;
1170         u8  d;
1171         int result;
1172
1173         mutex_lock(&indio_dev->mlock);
1174         if (st->chip_config.enable) {
1175                 result = -EBUSY;
1176                 goto attr_store_fail;
1177         }
1178         if (this_attr->address <= ATTR_MOTION_LPA_THRESHOLD) {
1179                 result = st->set_power_state(st, true);
1180                 if (result)
1181                         goto attr_store_fail;
1182         }
1183
1184         result = kstrtoint(buf, 10, &data);
1185         if (result)
1186                 goto attr_store_fail;
1187         switch (this_attr->address) {
1188         case ATTR_MOTION_LPA_ON:
1189                 if (INV_MPU6500 == st->chip_type) {
1190                         if (data)
1191                                 /* enable and put in MPU6500 mode */
1192                                 d = BIT_ACCEL_INTEL_ENABLE
1193                                         | BIT_ACCEL_INTEL_MODE;
1194                         else
1195                                 d = 0;
1196                         result = inv_plat_single_write(st,
1197                                                 REG_6500_ACCEL_INTEL_CTRL, d);
1198                         if (result)
1199                                 goto attr_store_fail;
1200                 }
1201                 st->mot_int.mot_on = !!data;
1202                 st->chip_config.lpa_mode = !!data;
1203                 break;
1204         case ATTR_MOTION_LPA_FREQ:
1205                 result = inv_lpa_freq(st, data);
1206                 break;
1207         case ATTR_MOTION_LPA_THRESHOLD:
1208                 if ((data > MPU6XXX_MAX_MOTION_THRESH) || (data < 0)) {
1209                         result = -EINVAL;
1210                         goto attr_store_fail;
1211                 }
1212                 d = (u8)(data >> MPU6XXX_MOTION_THRESH_SHIFT);
1213                 data = (d << MPU6XXX_MOTION_THRESH_SHIFT);
1214                 result = inv_plat_single_write(st, REG_ACCEL_MOT_THR, d);
1215                 if (result)
1216                         goto attr_store_fail;
1217                 st->mot_int.mot_thr = data;
1218                 break;
1219         /* from now on, power is not turned on */
1220         case ATTR_SELF_TEST_SAMPLES:
1221                 if (data > ST_MAX_SAMPLES || data < 0) {
1222                         result = -EINVAL;
1223                         goto attr_store_fail;
1224                 }
1225                 st->self_test.samples = data;
1226                 break;
1227         case ATTR_SELF_TEST_THRESHOLD:
1228                 if (data > ST_MAX_THRESHOLD || data < 0) {
1229                         result = -EINVAL;
1230                         goto attr_store_fail;
1231                 }
1232                 st->self_test.threshold = data;
1233         case ATTR_GYRO_ENABLE:
1234                 result = st->gyro_en(st, ring, !!data);
1235                 break;
1236         case ATTR_ACCL_ENABLE:
1237                 result = st->accl_en(st, ring, !!data);
1238                 break;
1239         case ATTR_COMPASS_ENABLE:
1240                 result = inv_compass_enable(st, ring, !!data);
1241                 break;
1242         case ATTR_POWER_STATE:
1243                 fake_asleep = !data;
1244                 break;
1245         case ATTR_FIRMWARE_LOADED:
1246                 result = inv_firmware_loaded(st, data);
1247                 break;
1248         case ATTR_SAMPLING_FREQ:
1249                 result = inv_fifo_rate_store(st, data);
1250                 break;
1251         default:
1252                 result = -EINVAL;
1253                 goto attr_store_fail;
1254         };
1255
1256 attr_store_fail:
1257         if ((this_attr->address <= ATTR_MOTION_LPA_THRESHOLD) &&
1258                                         (!st->chip_config.enable))
1259                 result |= st->set_power_state(st, false);
1260         mutex_unlock(&indio_dev->mlock);
1261         if (result)
1262                 return result;
1263
1264         return count;
1265 }
1266
1267 #ifdef CONFIG_INV_TESTING
1268 /**
1269  * inv_reg_write_store() - register write command for testing.
1270  *                         Format: WSRRDD, where RR is the register in hex,
1271  *                                         and DD is the data in hex.
1272  */
1273 static ssize_t inv_reg_write_store(struct device *dev,
1274         struct device_attribute *attr, const char *buf, size_t count)
1275 {
1276         struct iio_dev *indio_dev = dev_get_drvdata(dev);
1277         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1278         u32 result;
1279         u8 wreg, wval;
1280         int temp;
1281         char local_buf[10];
1282
1283         if ((buf[0] != 'W' && buf[0] != 'w') ||
1284             (buf[1] != 'S' && buf[1] != 's'))
1285                 return -EINVAL;
1286         if (strlen(buf) < 6)
1287                 return -EINVAL;
1288
1289         strncpy(local_buf, buf, 7);
1290         local_buf[6] = 0;
1291         result = sscanf(&local_buf[4], "%x", &temp);
1292         if (result == 0)
1293                 return -EINVAL;
1294         wval = temp;
1295         local_buf[4] = 0;
1296         sscanf(&local_buf[2], "%x", &temp);
1297         if (result == 0)
1298                 return -EINVAL;
1299         wreg = temp;
1300
1301         result = inv_plat_single_write(st, wreg, wval);
1302         if (result)
1303                 return result;
1304
1305         return count;
1306 }
1307 #endif /* CONFIG_INV_TESTING */
1308
1309 #define IIO_ST(si, rb, sb, sh)                                          \
1310         { .sign = si, .realbits = rb, .storagebits = sb, .shift = sh }
1311 #define INV_MPU_CHAN(_type, _channel2, _index)                \
1312         {                                                         \
1313                 .type = _type,                                        \
1314                 .modified = 1,                                        \
1315                 .channel2 = _channel2,                                \
1316                 .info_mask_separate = BIT(IIO_CHAN_INFO_CALIBBIAS), \
1317                 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
1318                 .scan_index = _index,                                 \
1319                 .scan_type  = IIO_ST('s', 16, 16, 0)                  \
1320         }
1321
1322 #define INV_ACCL_CHAN(_type, _channel2, _index)                \
1323         {                                                         \
1324                 .type = _type,                                        \
1325                 .modified = 1,                                        \
1326                 .channel2 = _channel2,                                \
1327                 .info_mask_separate = BIT(IIO_CHAN_INFO_CALIBBIAS) | \
1328                                       BIT(IIO_CHAN_INFO_OFFSET), \
1329                 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
1330                 .scan_index = _index,                                 \
1331                 .scan_type  = IIO_ST('s', 16, 16, 0)                  \
1332         }
1333
1334 #define INV_MPU_QUATERNION_CHAN(_channel2, _index)            \
1335         {                                                         \
1336                 .type = IIO_QUATERNION,                               \
1337                 .modified = 1,                                        \
1338                 .channel2 = _channel2,                                \
1339                 .scan_index = _index,                                 \
1340                 .scan_type  = IIO_ST('s', 32, 32, 0)                  \
1341         }
1342
1343 #define INV_MPU_MAGN_CHAN(_channel2, _index)                  \
1344         {                                                         \
1345                 .type = IIO_MAGN,                                     \
1346                 .modified = 1,                                        \
1347                 .channel2 = _channel2,                                \
1348                 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
1349                 .scan_index = _index,                                 \
1350                 .scan_type  = IIO_ST('s', 16, 16, 0)                  \
1351         }
1352
1353 static const struct iio_chan_spec inv_mpu_channels[] = {
1354         IIO_CHAN_SOFT_TIMESTAMP(INV_MPU_SCAN_TIMESTAMP),
1355         INV_MPU_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU_SCAN_GYRO_X),
1356         INV_MPU_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU_SCAN_GYRO_Y),
1357         INV_MPU_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU_SCAN_GYRO_Z),
1358
1359         INV_ACCL_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU_SCAN_ACCL_X),
1360         INV_ACCL_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU_SCAN_ACCL_Y),
1361         INV_ACCL_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU_SCAN_ACCL_Z),
1362
1363         INV_MPU_QUATERNION_CHAN(IIO_MOD_R, INV_MPU_SCAN_QUAT_R),
1364         INV_MPU_QUATERNION_CHAN(IIO_MOD_X, INV_MPU_SCAN_QUAT_X),
1365         INV_MPU_QUATERNION_CHAN(IIO_MOD_Y, INV_MPU_SCAN_QUAT_Y),
1366         INV_MPU_QUATERNION_CHAN(IIO_MOD_Z, INV_MPU_SCAN_QUAT_Z),
1367
1368         INV_MPU_MAGN_CHAN(IIO_MOD_X, INV_MPU_SCAN_MAGN_X),
1369         INV_MPU_MAGN_CHAN(IIO_MOD_Y, INV_MPU_SCAN_MAGN_Y),
1370         INV_MPU_MAGN_CHAN(IIO_MOD_Z, INV_MPU_SCAN_MAGN_Z),
1371 };
1372
1373
1374 /*constant IIO attribute */
1375 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500");
1376
1377 /* special sysfs */
1378 static DEVICE_ATTR(reg_dump, S_IRUGO, inv_reg_dump_show, NULL);
1379 static DEVICE_ATTR(temperature, S_IRUGO, inv_temperature_show, NULL);
1380
1381 /* event based sysfs, needs poll to read */
1382 static DEVICE_ATTR(event_display_orientation, S_IRUGO,
1383         inv_dmp_display_orient_show, NULL);
1384 static DEVICE_ATTR(event_accel_motion, S_IRUGO, inv_accel_motion_show, NULL);
1385 static DEVICE_ATTR(event_smd, S_IRUGO, inv_smd_show, NULL);
1386
1387 /* DMP sysfs with power on/off */
1388 static IIO_DEVICE_ATTR(smd_enable, S_IRUGO | S_IWUSR,
1389         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_ENABLE);
1390 static IIO_DEVICE_ATTR(smd_threshold, S_IRUGO | S_IWUSR,
1391         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_THLD);
1392 static IIO_DEVICE_ATTR(smd_delay_threshold, S_IRUGO | S_IWUSR,
1393         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_DELAY_THLD);
1394 static IIO_DEVICE_ATTR(smd_delay_threshold2, S_IRUGO | S_IWUSR,
1395         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_DELAY_THLD2);
1396 static IIO_DEVICE_ATTR(display_orientation_on, S_IRUGO | S_IWUSR,
1397         inv_attr_show, inv_dmp_attr_store, ATTR_DMP_DISPLAY_ORIENTATION_ON);
1398
1399 /* DMP sysfs without power on/off */
1400 static IIO_DEVICE_ATTR(dmp_on, S_IRUGO | S_IWUSR, inv_attr_show,
1401         inv_dmp_attr_store, ATTR_DMP_ON);
1402 static IIO_DEVICE_ATTR(dmp_int_on, S_IRUGO | S_IWUSR, inv_attr_show,
1403         inv_dmp_attr_store, ATTR_DMP_INT_ON);
1404 static IIO_DEVICE_ATTR(dmp_event_int_on, S_IRUGO | S_IWUSR, inv_attr_show,
1405         inv_dmp_attr_store, ATTR_DMP_EVENT_INT_ON);
1406 static IIO_DEVICE_ATTR(dmp_output_rate, S_IRUGO | S_IWUSR, inv_attr_show,
1407         inv_dmp_attr_store, ATTR_DMP_OUTPUT_RATE);
1408 static IIO_DEVICE_ATTR(quaternion_on, S_IRUGO | S_IWUSR, inv_attr_show,
1409         inv_dmp_attr_store, ATTR_DMP_QUATERNION_ON);
1410
1411 /* non DMP sysfs with power on/off */
1412 static IIO_DEVICE_ATTR(motion_lpa_on, S_IRUGO | S_IWUSR, inv_attr_show,
1413         inv_attr_store, ATTR_MOTION_LPA_ON);
1414 static IIO_DEVICE_ATTR(motion_lpa_freq, S_IRUGO | S_IWUSR, inv_attr_show,
1415         inv_attr_store, ATTR_MOTION_LPA_FREQ);
1416 static IIO_DEVICE_ATTR(motion_lpa_threshold, S_IRUGO | S_IWUSR, inv_attr_show,
1417         inv_attr_store, ATTR_MOTION_LPA_THRESHOLD);
1418
1419 /* non DMP sysfs without power on/off */
1420 static IIO_DEVICE_ATTR(self_test_samples, S_IRUGO | S_IWUSR, inv_attr_show,
1421         inv_attr_store, ATTR_SELF_TEST_SAMPLES);
1422 static IIO_DEVICE_ATTR(self_test_threshold, S_IRUGO | S_IWUSR, inv_attr_show,
1423         inv_attr_store, ATTR_SELF_TEST_THRESHOLD);
1424 static IIO_DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR, inv_attr_show,
1425         inv_attr_store, ATTR_GYRO_ENABLE);
1426 static IIO_DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR, inv_attr_show,
1427         inv_attr_store, ATTR_ACCL_ENABLE);
1428 static IIO_DEVICE_ATTR(compass_enable, S_IRUGO | S_IWUSR, inv_attr_show,
1429         inv_attr_store, ATTR_COMPASS_ENABLE);
1430 static IIO_DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR, inv_attr_show,
1431         inv_attr_store, ATTR_POWER_STATE);
1432 static IIO_DEVICE_ATTR(firmware_loaded, S_IRUGO | S_IWUSR, inv_attr_show,
1433         inv_attr_store, ATTR_FIRMWARE_LOADED);
1434 static IIO_DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR, inv_attr_show,
1435         inv_attr_store, ATTR_SAMPLING_FREQ);
1436
1437 /* show method only sysfs but with power on/off */
1438 static IIO_DEVICE_ATTR(self_test, S_IRUGO, inv_attr_show, NULL,
1439         ATTR_SELF_TEST);
1440
1441 /* show method only sysfs */
1442 static IIO_DEVICE_ATTR(gyro_matrix, S_IRUGO, inv_attr_show, NULL,
1443         ATTR_GYRO_MATRIX);
1444 static IIO_DEVICE_ATTR(accl_matrix, S_IRUGO, inv_attr_show, NULL,
1445         ATTR_ACCL_MATRIX);
1446 static IIO_DEVICE_ATTR(compass_matrix, S_IRUGO, inv_attr_show, NULL,
1447         ATTR_COMPASS_MATRIX);
1448 static IIO_DEVICE_ATTR(secondary_name, S_IRUGO, inv_attr_show, NULL,
1449         ATTR_SECONDARY_NAME);
1450
1451 #ifdef CONFIG_INV_TESTING
1452 static IIO_DEVICE_ATTR(reg_write, S_IRUGO | S_IWUSR, inv_attr_show,
1453         inv_reg_write_store, ATTR_REG_WRITE);
1454 /* smd debug related sysfs */
1455 static IIO_DEVICE_ATTR(debug_smd_enable_testp1, S_IWUSR, NULL,
1456         inv_dmp_attr_store, ATTR_DEBUG_SMD_ENABLE_TESTP1);
1457 static IIO_DEVICE_ATTR(debug_smd_enable_testp2, S_IWUSR, NULL,
1458         inv_dmp_attr_store, ATTR_DEBUG_SMD_ENABLE_TESTP2);
1459 static IIO_DEVICE_ATTR(debug_smd_exe_state, S_IRUGO, inv_attr_show,
1460         NULL, ATTR_DEBUG_SMD_EXE_STATE);
1461 static IIO_DEVICE_ATTR(debug_smd_delay_cntr, S_IRUGO, inv_attr_show,
1462         NULL, ATTR_DEBUG_SMD_DELAY_CNTR);
1463 #endif
1464
1465 static const struct attribute *inv_gyro_attributes[] = {
1466         &iio_const_attr_sampling_frequency_available.dev_attr.attr,
1467         &dev_attr_reg_dump.attr,
1468         &dev_attr_temperature.attr,
1469         &iio_dev_attr_self_test_samples.dev_attr.attr,
1470         &iio_dev_attr_self_test_threshold.dev_attr.attr,
1471         &iio_dev_attr_gyro_enable.dev_attr.attr,
1472         &iio_dev_attr_power_state.dev_attr.attr,
1473         &iio_dev_attr_sampling_frequency.dev_attr.attr,
1474         &iio_dev_attr_self_test.dev_attr.attr,
1475         &iio_dev_attr_gyro_matrix.dev_attr.attr,
1476         &iio_dev_attr_secondary_name.dev_attr.attr,
1477 #ifdef CONFIG_INV_TESTING
1478         &iio_dev_attr_reg_write.dev_attr.attr,
1479         &iio_dev_attr_debug_smd_enable_testp1.dev_attr.attr,
1480         &iio_dev_attr_debug_smd_enable_testp2.dev_attr.attr,
1481         &iio_dev_attr_debug_smd_exe_state.dev_attr.attr,
1482         &iio_dev_attr_debug_smd_delay_cntr.dev_attr.attr,
1483 #endif
1484 };
1485
1486 static const struct attribute *inv_mpu6050_attributes[] = {
1487         &dev_attr_event_display_orientation.attr,
1488         &dev_attr_event_smd.attr,
1489         &iio_dev_attr_smd_enable.dev_attr.attr,
1490         &iio_dev_attr_smd_threshold.dev_attr.attr,
1491         &iio_dev_attr_smd_delay_threshold.dev_attr.attr,
1492         &iio_dev_attr_smd_delay_threshold2.dev_attr.attr,
1493         &iio_dev_attr_display_orientation_on.dev_attr.attr,
1494         &iio_dev_attr_dmp_on.dev_attr.attr,
1495         &iio_dev_attr_dmp_int_on.dev_attr.attr,
1496         &iio_dev_attr_dmp_event_int_on.dev_attr.attr,
1497         &iio_dev_attr_dmp_output_rate.dev_attr.attr,
1498         &iio_dev_attr_quaternion_on.dev_attr.attr,
1499         &iio_dev_attr_accl_enable.dev_attr.attr,
1500         &iio_dev_attr_firmware_loaded.dev_attr.attr,
1501         &iio_dev_attr_accl_matrix.dev_attr.attr,
1502
1503 };
1504
1505 static const struct attribute *inv_mpu6500_attributes[] = {
1506         &dev_attr_event_accel_motion.attr,
1507         &iio_dev_attr_motion_lpa_on.dev_attr.attr,
1508         &iio_dev_attr_motion_lpa_freq.dev_attr.attr,
1509         &iio_dev_attr_motion_lpa_threshold.dev_attr.attr,
1510 };
1511
1512 static const struct attribute *inv_compass_attributes[] = {
1513         &iio_dev_attr_compass_enable.dev_attr.attr,
1514         &iio_dev_attr_compass_matrix.dev_attr.attr,
1515 };
1516
1517 static const struct attribute *inv_mpu3050_attributes[] = {
1518         &iio_dev_attr_accl_enable.dev_attr.attr,
1519         &iio_dev_attr_accl_matrix.dev_attr.attr,
1520 };
1521
1522 static struct attribute *inv_attributes[ARRAY_SIZE(inv_gyro_attributes) +
1523                                 ARRAY_SIZE(inv_mpu6050_attributes) +
1524                                 ARRAY_SIZE(inv_mpu6500_attributes) +
1525                                 ARRAY_SIZE(inv_compass_attributes) + 1];
1526
1527 static const struct attribute_group inv_attribute_group = {
1528         .name = "mpu",
1529         .attrs = inv_attributes
1530 };
1531
1532 static const struct iio_info mpu_info = {
1533         .driver_module = THIS_MODULE,
1534         .read_raw = &mpu_read_raw,
1535         .write_raw = &mpu_write_raw,
1536         .attrs = &inv_attribute_group,
1537 };
1538
1539 void inv_set_iio_info(struct inv_mpu_iio_s *st, struct iio_dev *indio_dev)
1540 {
1541         indio_dev->channels = inv_mpu_channels;
1542         indio_dev->num_channels = st->num_channels;
1543
1544         indio_dev->info = &mpu_info;
1545         indio_dev->modes = INDIO_DIRECT_MODE;
1546         indio_dev->currentmode = INDIO_DIRECT_MODE;
1547 }
1548
1549 /**
1550  *  inv_setup_compass() - Configure compass.
1551  */
1552 static int inv_setup_compass(struct inv_mpu_iio_s *st)
1553 {
1554         int result;
1555         u8 data[4];
1556
1557         if (INV_MPU6050 == st->chip_type) {
1558                 result = inv_plat_read(st, REG_YGOFFS_TC, 1, data);
1559                 if (result)
1560                         return result;
1561                 data[0] &= ~BIT_I2C_MST_VDDIO;
1562                 if (st->plat_data.level_shifter)
1563                         data[0] |= BIT_I2C_MST_VDDIO;
1564                 /*set up VDDIO register */
1565                 result = inv_plat_single_write(st, REG_YGOFFS_TC, data[0]);
1566                 if (result)
1567                         return result;
1568         }
1569         /* set to bypass mode */
1570         result = inv_plat_single_write(st, REG_INT_PIN_CFG,
1571                                 st->plat_data.int_config | BIT_BYPASS_EN);
1572         if (result)
1573                 return result;
1574         /*read secondary i2c ID register */
1575         result = inv_secondary_read(st, REG_AKM_ID, 1, data);
1576         if (result)
1577                 return result;
1578         if (data[0] != DATA_AKM_ID)
1579                 return -ENXIO;
1580         /*set AKM to Fuse ROM access mode */
1581         result = inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_FR);
1582         if (result)
1583                 return result;
1584         result = inv_secondary_read(st, REG_AKM_SENSITIVITY, THREE_AXIS,
1585                                         st->chip_info.compass_sens);
1586         if (result)
1587                 return result;
1588         /*revert to power down mode */
1589         result = inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_PD);
1590         if (result)
1591                 return result;
1592         pr_debug("%s senx=%d, seny=%d, senz=%d\n",
1593                  st->hw->name,
1594                  st->chip_info.compass_sens[0],
1595                  st->chip_info.compass_sens[1],
1596                  st->chip_info.compass_sens[2]);
1597         /*restore to non-bypass mode */
1598         result = inv_plat_single_write(st, REG_INT_PIN_CFG,
1599                         st->plat_data.int_config);
1600         if (result)
1601                 return result;
1602
1603         /*setup master mode and master clock and ES bit*/
1604         result = inv_plat_single_write(st, REG_I2C_MST_CTRL, BIT_WAIT_FOR_ES);
1605         if (result)
1606                 return result;
1607         /* slave 0 is used to read data from compass */
1608         /*read mode */
1609         result = inv_plat_single_write(st, REG_I2C_SLV0_ADDR, BIT_I2C_READ|
1610                 st->plat_data.secondary_i2c_addr);
1611         if (result)
1612                 return result;
1613         /* AKM status register address is 2 */
1614         result = inv_plat_single_write(st, REG_I2C_SLV0_REG, REG_AKM_STATUS);
1615         if (result)
1616                 return result;
1617         /* slave 0 is enabled at the beginning, read 8 bytes from here */
1618         result = inv_plat_single_write(st, REG_I2C_SLV0_CTRL, BIT_SLV_EN |
1619                                 NUM_BYTES_COMPASS_SLAVE);
1620         if (result)
1621                 return result;
1622         /*slave 1 is used for AKM mode change only*/
1623         result = inv_plat_single_write(st, REG_I2C_SLV1_ADDR,
1624                 st->plat_data.secondary_i2c_addr);
1625         if (result)
1626                 return result;
1627         /* AKM mode register address is 0x0A */
1628         result = inv_plat_single_write(st, REG_I2C_SLV1_REG, REG_AKM_MODE);
1629         if (result)
1630                 return result;
1631         /* slave 1 is enabled, byte length is 1 */
1632         result = inv_plat_single_write(st, REG_I2C_SLV1_CTRL, BIT_SLV_EN | 1);
1633         if (result)
1634                 return result;
1635         /* output data for slave 1 is fixed, single measure mode*/
1636         st->compass_scale = 1;
1637         if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id) {
1638                 st->compass_st_upper = AKM8975_ST_Upper;
1639                 st->compass_st_lower = AKM8975_ST_Lower;
1640                 data[0] = DATA_AKM_MODE_SM;
1641         } else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id) {
1642                 st->compass_st_upper = AKM8972_ST_Upper;
1643                 st->compass_st_lower = AKM8972_ST_Lower;
1644                 data[0] = DATA_AKM_MODE_SM;
1645         } else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) {
1646                 st->compass_st_upper = AKM8963_ST_Upper;
1647                 st->compass_st_lower = AKM8963_ST_Lower;
1648                 data[0] = DATA_AKM_MODE_SM |
1649                           (st->compass_scale << AKM8963_SCALE_SHIFT);
1650         }
1651         result = inv_plat_single_write(st, REG_I2C_SLV1_DO, data[0]);
1652         if (result)
1653                 return result;
1654         /* slave 0 and 1 timer action is enabled every sample*/
1655         result = inv_plat_single_write(st, REG_I2C_MST_DELAY_CTRL,
1656                                 BIT_SLV0_DLY_EN | BIT_SLV1_DLY_EN);
1657         return result;
1658 }
1659
1660 static void inv_setup_func_ptr(struct inv_mpu_iio_s *st)
1661 {
1662         if (st->chip_type == INV_MPU3050) {
1663                 st->set_power_state    = set_power_mpu3050;
1664                 st->switch_gyro_engine = inv_switch_3050_gyro_engine;
1665                 st->switch_accl_engine = inv_switch_3050_accl_engine;
1666                 st->init_config        = inv_init_config_mpu3050;
1667                 st->setup_reg          = inv_setup_reg_mpu3050;
1668         } else {
1669                 st->set_power_state    = set_power_itg;
1670                 st->switch_gyro_engine = inv_switch_gyro_engine;
1671                 st->switch_accl_engine = inv_switch_accl_engine;
1672                 st->init_config        = inv_init_config;
1673                 st->setup_reg          = inv_setup_reg;
1674                 /*MPU6XXX special functions */
1675                 st->compass_en         = inv_compass_enable;
1676                 st->quaternion_en      = inv_quaternion_on;
1677         }
1678         st->gyro_en            = inv_gyro_enable;
1679         st->accl_en            = inv_accl_enable;
1680 }
1681
1682 static int inv_detect_6xxx(struct inv_mpu_iio_s *st)
1683 {
1684         int result;
1685         u8 d;
1686
1687         result = inv_plat_read(st, REG_WHOAMI, 1, &d);
1688         if (result)
1689                 return result;
1690         if ((d == MPU6500_ID) || (d == MPU6515_ID)) {
1691                 st->chip_type = INV_MPU6500;
1692                 strcpy(st->name, "mpu6500");
1693         } else {
1694                 strcpy(st->name, "mpu6050");
1695         }
1696
1697         return 0;
1698 }
1699
1700 /**
1701  *  inv_check_chip_type() - check and setup chip type.
1702  */
1703 int inv_check_chip_type(struct inv_mpu_iio_s *st, const char *name)
1704 {
1705         struct inv_reg_map_s *reg;
1706         int result;
1707         int t_ind;
1708
1709         if (!strcmp(name, "itg3500"))
1710                 st->chip_type = INV_ITG3500;
1711         else if (!strcmp(name, "mpu3050"))
1712                 st->chip_type = INV_MPU3050;
1713         else if (!strcmp(name, "mpu6050"))
1714                 st->chip_type = INV_MPU6050;
1715         else if (!strcmp(name, "mpu9150"))
1716                 st->chip_type = INV_MPU6050;
1717         else if (!strcmp(name, "mpu6500"))
1718                 st->chip_type = INV_MPU6500;
1719         else if (!strcmp(name, "mpu9250"))
1720                 st->chip_type = INV_MPU6500;
1721         else if (!strcmp(name, "mpu6xxx"))
1722                 st->chip_type = INV_MPU6050;
1723         else if (!strcmp(name, "mpu6515"))
1724                 st->chip_type = INV_MPU6500;
1725         else
1726                 return -EPERM;
1727         inv_setup_func_ptr(st);
1728         st->hw  = &hw_info[st->chip_type];
1729         st->mpu_slave = NULL;
1730         reg = &st->reg;
1731         st->setup_reg(reg);
1732         /* reset to make sure previous state are not there */
1733         result = inv_plat_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET);
1734         if (result)
1735                 return result;
1736         msleep(POWER_UP_TIME);
1737         /* toggle power state */
1738         result = st->set_power_state(st, false);
1739         if (result)
1740                 return result;
1741
1742         result = st->set_power_state(st, true);
1743         if (result)
1744                 return result;
1745
1746         if (!strcmp(name, "mpu6xxx")) {
1747                 /* for MPU6500, reading register need more time */
1748                 msleep(POWER_UP_TIME);
1749                 result = inv_detect_6xxx(st);
1750                 if (result)
1751                         return result;
1752         }
1753
1754         switch (st->chip_type) {
1755         case INV_ITG3500:
1756                 st->num_channels = INV_CHANNEL_NUM_GYRO;
1757                 break;
1758         case INV_MPU6050:
1759         case INV_MPU6500:
1760                 if (SECONDARY_SLAVE_TYPE_COMPASS ==
1761                     st->plat_data.sec_slave_type) {
1762                         st->chip_config.has_compass = 1;
1763                         st->num_channels =
1764                                 INV_CHANNEL_NUM_GYRO_ACCL_QUANTERNION_MAGN;
1765                 } else {
1766                         st->chip_config.has_compass = 0;
1767                         st->num_channels =
1768                                 INV_CHANNEL_NUM_GYRO_ACCL_QUANTERNION;
1769                 }
1770                 break;
1771         case INV_MPU3050:
1772                 if (SECONDARY_SLAVE_TYPE_ACCEL ==
1773                     st->plat_data.sec_slave_type) {
1774                         if (ACCEL_ID_BMA250 == st->plat_data.sec_slave_id)
1775                                 inv_register_mpu3050_slave(st);
1776                         st->num_channels = INV_CHANNEL_NUM_GYRO_ACCL;
1777                 } else {
1778                         st->num_channels = INV_CHANNEL_NUM_GYRO;
1779                 }
1780                 break;
1781         default:
1782                 result = st->set_power_state(st, false);
1783                 return -ENODEV;
1784         }
1785         st->chip_info.multi = 1;
1786         switch (st->chip_type) {
1787         case INV_MPU6050:
1788                 result = inv_get_silicon_rev_mpu6050(st);
1789                 break;
1790         case INV_MPU6500:
1791                 result = inv_get_silicon_rev_mpu6500(st);
1792                 break;
1793         default:
1794                 result = 0;
1795                 break;
1796         }
1797         if (result) {
1798                 pr_err("read silicon rev error\n");
1799                 st->set_power_state(st, false);
1800                 return result;
1801         }
1802         /* turn off the gyro engine after OTP reading */
1803         result = st->switch_gyro_engine(st, false);
1804         if (result)
1805                 return result;
1806         result = st->switch_accl_engine(st, false);
1807         if (result)
1808                 return result;
1809         if (st->chip_config.has_compass) {
1810                 result = inv_setup_compass(st);
1811                 if (result) {
1812                         pr_err("compass setup failed\n");
1813                         st->set_power_state(st, false);
1814                         return result;
1815                 }
1816         }
1817
1818         t_ind = 0;
1819         memcpy(&inv_attributes[t_ind], inv_gyro_attributes,
1820                 sizeof(inv_gyro_attributes));
1821         t_ind += ARRAY_SIZE(inv_gyro_attributes);
1822
1823         if (INV_MPU3050 == st->chip_type && st->mpu_slave != NULL) {
1824                 memcpy(&inv_attributes[t_ind], inv_mpu3050_attributes,
1825                        sizeof(inv_mpu3050_attributes));
1826                 t_ind += ARRAY_SIZE(inv_mpu3050_attributes);
1827                 inv_attributes[t_ind] = NULL;
1828                 return 0;
1829         }
1830
1831         if ((INV_MPU6050 == st->chip_type) || (INV_MPU6500 == st->chip_type)) {
1832                 memcpy(&inv_attributes[t_ind], inv_mpu6050_attributes,
1833                        sizeof(inv_mpu6050_attributes));
1834                 t_ind += ARRAY_SIZE(inv_mpu6050_attributes);
1835         }
1836
1837         if (INV_MPU6500 == st->chip_type) {
1838                 memcpy(&inv_attributes[t_ind], inv_mpu6500_attributes,
1839                        sizeof(inv_mpu6500_attributes));
1840                 t_ind += ARRAY_SIZE(inv_mpu6500_attributes);
1841         }
1842
1843         if (st->chip_config.has_compass) {
1844                 memcpy(&inv_attributes[t_ind], inv_compass_attributes,
1845                        sizeof(inv_compass_attributes));
1846                 t_ind += ARRAY_SIZE(inv_compass_attributes);
1847         }
1848         inv_attributes[t_ind] = NULL;
1849
1850         return 0;
1851 }
1852
1853 /**
1854  *  inv_create_dmp_sysfs() - create binary sysfs dmp entry.
1855  */
1856 static const struct bin_attribute dmp_firmware = {
1857         .attr = {
1858                 .name = "dmp_firmware",
1859                 .mode = S_IRUGO | S_IWUSR
1860         },
1861         .size = 4096,
1862         .read = inv_dmp_firmware_read,
1863         .write = inv_dmp_firmware_write,
1864 };
1865
1866 int inv_create_dmp_sysfs(struct iio_dev *ind)
1867 {
1868         int result;
1869         result = sysfs_create_bin_file(&ind->dev.kobj, &dmp_firmware);
1870
1871         return result;
1872 }
1873
1874 /**
1875  *  @}
1876  */