drivers/staging/iio/imu/inv_mpu/inv_mpu_misc.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  *  @addtogroup  DRIVERS
  16  *  @brief       Hardware drivers.
  17  *
  18  *  @{
  19  *      @file    inv_mpu_misc.c
  20  *      @brief   A sysfs device driver for Invensense mpu.
  21  *      @details This file is part of invensense mpu driver code
  22  */
  23
  24 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  25
  26 #include <linux/module.h>
  27 #include <linux/init.h>
  28 #include <linux/slab.h>
  29 #include <linux/i2c.h>
  30 #include <linux/err.h>
  31 #include <linux/delay.h>
  32 #include <linux/sysfs.h>
  33 #include <linux/jiffies.h>
  34 #include <linux/irq.h>
  35 #include <linux/interrupt.h>
  36 #include <linux/kfifo.h>
  37 #include <linux/poll.h>
  38 #include <linux/miscdevice.h>
  39 #include <linux/crc32.h>
  40
  41 #include "inv_mpu_iio.h"
  42 #include "inv_counters.h"
  43
  44 /* DMP defines */
  45 #define DMP_ORIENTATION_TIME            500
  46 #define DMP_ORIENTATION_ANGLE           60
  47 #define DMP_DEFAULT_FIFO_RATE           200
  48 #define DMP_TAP_SCALE                   (767603923 / 5)
  49 #define DMP_MULTI_SHIFT                 30
  50 #define DMP_MULTI_TAP_TIME              500
  51 #define DMP_SHAKE_REJECT_THRESH         100
  52 #define DMP_SHAKE_REJECT_TIME           10
  53 #define DMP_SHAKE_REJECT_TIMEOUT        10
  54 #define DMP_ANGLE_SCALE                 15
  55 #define DMP_PRECISION                   1000
  56 #define DMP_MAX_DIVIDER                 4
  57 #define DMP_MAX_MIN_TAPS                4
  58 #define DMP_IMAGE_CRC_VALUE             0x665f5a73
  59 #define DMP_IMAGE_SIZE                  2913
  60
  61 /*--- Test parameters defaults --- */
  62 #define DEF_OLDEST_SUPP_PROD_REV    8
  63 #define DEF_OLDEST_SUPP_SW_REV      2
  64
  65 /* sample rate */
  66 #define DEF_SELFTEST_SAMPLE_RATE             0
  67 /* full scale setting dps */
  68 #define DEF_SELFTEST_GYRO_FS         (0 << 3)
  69 #define DEF_SELFTEST_ACCEL_FS         (2 << 3)
  70 #define DEF_SELFTEST_GYRO_SENS            (32768 / 250)
  71 /* wait time before collecting data */
  72 #define DEF_GYRO_WAIT_TIME          10
  73 #define DEF_ST_STABLE_TIME          200
  74 #define DEF_ST_6500_STABLE_TIME     20
  75 #define DEF_GYRO_SCALE              131
  76 #define DEF_ST_PRECISION            1000
  77 #define DEF_ST_ACCEL_FS_MG          8000UL
  78 #define DEF_ST_SCALE                (1L << 15)
  79 #define DEF_ST_TRY_TIMES            2
  80 #define DEF_ST_COMPASS_RESULT_SHIFT 2
  81 #define DEF_ST_ACCEL_RESULT_SHIFT   1
  82 #define DEF_ST_OTP0_THRESH          60
  83 #define DEF_ST_ABS_THRESH           20
  84 #define DEF_ST_TOR                  2
  85
  86 #define DEF_ST_COMPASS_WAIT_MIN     (10 * 1000)
  87 #define DEF_ST_COMPASS_WAIT_MAX     (15 * 1000)
  88 #define DEF_ST_COMPASS_TRY_TIMES    10
  89 #define DEF_ST_COMPASS_8963_SHIFT   2
  90
  91 #define X                           0
  92 #define Y                           1
  93 #define Z                           2
  94 /*---- MPU6050 notable product revisions ----*/
  95 #define MPU_PRODUCT_KEY_B1_E1_5      105
  96 #define MPU_PRODUCT_KEY_B2_F1        431
  97 /* accelerometer Hw self test min and max bias shift (mg) */
  98 #define DEF_ACCEL_ST_SHIFT_MIN       300
  99 #define DEF_ACCEL_ST_SHIFT_MAX       950
 100
 101 #define DEF_ACCEL_ST_SHIFT_DELTA     140
 102 #define DEF_GYRO_CT_SHIFT_DELTA      140
 103 /* gyroscope Coriolis self test min and max bias shift (dps) */
 104 #define DEF_GYRO_CT_SHIFT_MIN        10
 105 #define DEF_GYRO_CT_SHIFT_MAX        105
 106
 107 /*---- MPU6500 Self Test Pass/Fail Criteria ----*/
 108 /* Gyro Offset Max Value (dps) */
 109 #define DEF_GYRO_OFFSET_MAX             20
 110 /* Gyro Self Test Absolute Limits ST_AL (dps) */
 111 #define DEF_GYRO_ST_AL                  60
 112 /* Accel Self Test Absolute Limits ST_AL (mg) */
 113 #define DEF_ACCEL_ST_AL_MIN             225
 114 #define DEF_ACCEL_ST_AL_MAX             675
 115 #define DEF_6500_ACCEL_ST_SHIFT_DELTA   500
 116 #define DEF_6500_GYRO_CT_SHIFT_DELTA    500
 117 #define DEF_ST_MPU6500_ACCEL_LPF        2
 118 #define DEF_ST_6500_ACCEL_FS_MG         2000UL
 119 #define DEF_SELFTEST_6500_ACCEL_FS      (0 << 3)
 120
 121 /* Note: The ST_AL values are only used when ST_OTP = 0,
 122  * i.e no factory self test values for reference
 123  */
 124
 125 /* NOTE: product entries are in chronological order */
 126 static const struct prod_rev_map_t prod_rev_map[] = {
 127         /* prod_ver = 0 */
 128         {MPL_PROD_KEY(0,   1), MPU_SILICON_REV_A2, 131, 16384},
 129         {MPL_PROD_KEY(0,   2), MPU_SILICON_REV_A2, 131, 16384},
 130         {MPL_PROD_KEY(0,   3), MPU_SILICON_REV_A2, 131, 16384},
 131         {MPL_PROD_KEY(0,   4), MPU_SILICON_REV_A2, 131, 16384},
 132         {MPL_PROD_KEY(0,   5), MPU_SILICON_REV_A2, 131, 16384},
 133         {MPL_PROD_KEY(0,   6), MPU_SILICON_REV_A2, 131, 16384},
 134         /* prod_ver = 1 */
 135         {MPL_PROD_KEY(0,   7), MPU_SILICON_REV_A2, 131, 16384},
 136         {MPL_PROD_KEY(0,   8), MPU_SILICON_REV_A2, 131, 16384},
 137         {MPL_PROD_KEY(0,   9), MPU_SILICON_REV_A2, 131, 16384},
 138         {MPL_PROD_KEY(0,  10), MPU_SILICON_REV_A2, 131, 16384},
 139         {MPL_PROD_KEY(0,  11), MPU_SILICON_REV_A2, 131, 16384},
 140         {MPL_PROD_KEY(0,  12), MPU_SILICON_REV_A2, 131, 16384},
 141         {MPL_PROD_KEY(0,  13), MPU_SILICON_REV_A2, 131, 16384},
 142         {MPL_PROD_KEY(0,  14), MPU_SILICON_REV_A2, 131, 16384},
 143         {MPL_PROD_KEY(0,  15), MPU_SILICON_REV_A2, 131, 16384},
 144         {MPL_PROD_KEY(0,  27), MPU_SILICON_REV_A2, 131, 16384},
 145         /* prod_ver = 1 */
 146         {MPL_PROD_KEY(1,  16), MPU_SILICON_REV_B1, 131, 16384},
 147         {MPL_PROD_KEY(1,  17), MPU_SILICON_REV_B1, 131, 16384},
 148         {MPL_PROD_KEY(1,  18), MPU_SILICON_REV_B1, 131, 16384},
 149         {MPL_PROD_KEY(1,  19), MPU_SILICON_REV_B1, 131, 16384},
 150         {MPL_PROD_KEY(1,  20), MPU_SILICON_REV_B1, 131, 16384},
 151         {MPL_PROD_KEY(1,  28), MPU_SILICON_REV_B1, 131, 16384},
 152         {MPL_PROD_KEY(1,   1), MPU_SILICON_REV_B1, 131, 16384},
 153         {MPL_PROD_KEY(1,   2), MPU_SILICON_REV_B1, 131, 16384},
 154         {MPL_PROD_KEY(1,   3), MPU_SILICON_REV_B1, 131, 16384},
 155         {MPL_PROD_KEY(1,   4), MPU_SILICON_REV_B1, 131, 16384},
 156         {MPL_PROD_KEY(1,   5), MPU_SILICON_REV_B1, 131, 16384},
 157         {MPL_PROD_KEY(1,   6), MPU_SILICON_REV_B1, 131, 16384},
 158         /* prod_ver = 2 */
 159         {MPL_PROD_KEY(2,   7), MPU_SILICON_REV_B1, 131, 16384},
 160         {MPL_PROD_KEY(2,   8), MPU_SILICON_REV_B1, 131, 16384},
 161         {MPL_PROD_KEY(2,   9), MPU_SILICON_REV_B1, 131, 16384},
 162         {MPL_PROD_KEY(2,  10), MPU_SILICON_REV_B1, 131, 16384},
 163         {MPL_PROD_KEY(2,  11), MPU_SILICON_REV_B1, 131, 16384},
 164         {MPL_PROD_KEY(2,  12), MPU_SILICON_REV_B1, 131, 16384},
 165         {MPL_PROD_KEY(2,  29), MPU_SILICON_REV_B1, 131, 16384},
 166         /* prod_ver = 3 */
 167         {MPL_PROD_KEY(3,  30), MPU_SILICON_REV_B1, 131, 16384},
 168         /* prod_ver = 4 */
 169         {MPL_PROD_KEY(4,  31), MPU_SILICON_REV_B1, 131,  8192},
 170         {MPL_PROD_KEY(4,   1), MPU_SILICON_REV_B1, 131,  8192},
 171         {MPL_PROD_KEY(4,   3), MPU_SILICON_REV_B1, 131,  8192},
 172         /* prod_ver = 5 */
 173         {MPL_PROD_KEY(5,   3), MPU_SILICON_REV_B1, 131, 16384},
 174         /* prod_ver = 6 */
 175         {MPL_PROD_KEY(6,  19), MPU_SILICON_REV_B1, 131, 16384},
 176         /* prod_ver = 7 */
 177         {MPL_PROD_KEY(7,  19), MPU_SILICON_REV_B1, 131, 16384},
 178         /* prod_ver = 8 */
 179         {MPL_PROD_KEY(8,  19), MPU_SILICON_REV_B1, 131, 16384},
 180         /* prod_ver = 9 */
 181         {MPL_PROD_KEY(9,  19), MPU_SILICON_REV_B1, 131, 16384},
 182         /* prod_ver = 10 */
 183         {MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384}
 184 };
 185
 186 /*
 187 *   List of product software revisions
 188 *
 189 *   NOTE :
 190 *   software revision 0 falls back to the old detection method
 191 *   based off the product version and product revision per the
 192 *   table above
 193 */
 194 static const struct prod_rev_map_t sw_rev_map[] = {
 195         {0,                  0,   0,     0},
 196         {1, MPU_SILICON_REV_B1, 131,  8192},    /* rev C */
 197         {2, MPU_SILICON_REV_B1, 131, 16384}     /* rev D */
 198 };
 199
 200 static const u16 mpu_6500_st_tb[256] = {
 201         2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
 202         2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
 203         3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
 204         3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
 205         3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
 206         3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
 207         4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
 208         4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
 209         4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
 210         5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
 211         5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
 212         6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
 213         6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
 214         7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
 215         7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
 216         8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
 217         9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
 218         10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
 219         10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
 220         11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
 221         12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
 222         13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
 223         15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
 224         16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
 225         17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
 226         19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
 227         20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
 228         22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
 229         24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
 230         26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
 231         28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
 232         30903, 31212, 31524, 31839, 32157, 32479, 32804
 233 };
 234
 235 static const int accl_st_tb[31] = {
 236         340, 351, 363, 375, 388, 401, 414, 428,
 237         443, 458, 473, 489, 506, 523, 541, 559,
 238         578, 597, 617, 638, 660, 682, 705, 729,
 239         753, 779, 805, 832, 860, 889, 919};
 240
 241 static const int gyro_6050_st_tb[31] = {
 242         3275, 3425, 3583, 3748, 3920, 4100, 4289, 4486,
 243         4693, 4909, 5134, 5371, 5618, 5876, 6146, 6429,
 244         6725, 7034, 7358, 7696, 8050, 8421, 8808, 9213,
 245         9637, 10080, 10544, 11029, 11537, 12067, 12622};
 246 static const int gyro_3500_st_tb[255] = {
 247         2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
 248         2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
 249         3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
 250         3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
 251         3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
 252         3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
 253         4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
 254         4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
 255         4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
 256         5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
 257         5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
 258         6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
 259         6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
 260         7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
 261         7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
 262         8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
 263         9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
 264         10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
 265         10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
 266         11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
 267         12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
 268         13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
 269         15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
 270         16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
 271         17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
 272         19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
 273         20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
 274         22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
 275         24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
 276         26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
 277         28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
 278         30903, 31212, 31524, 31839, 32157, 32479, 32804};
 279
 280 char *wr_pr_debug_begin(u8 const *data, u32 len, char *string)
 281 {
 282         int ii;
 283         string = kmalloc(len * 2 + 1, GFP_KERNEL);
 284         for (ii = 0; ii < len; ii++)
 285                 sprintf(&string[ii * 2], "%02X", data[ii]);
 286         string[len * 2] = 0;
 287         return string;
 288 }
 289
 290 char *wr_pr_debug_end(char *string)
 291 {
 292         kfree(string);
 293         return "";
 294 }
 295
 296 int mpu_memory_write_unaligned(struct inv_mpu_iio_s *st, u16 key, int len,
 297                                                                 u8 const *d)
 298 {
 299         u32 addr;
 300         int start, end;
 301         int len1, len2;
 302         int result = 0;
 303         if (len > MPU_MEM_BANK_SIZE)
 304                 return -EINVAL;
 305         addr = inv_dmp_get_address(key);
 306         if (addr > MPU6XXX_MAX_MPU_MEM)
 307                 return -EINVAL;
 308         start = (addr >> 8);
 309         end   = ((addr + len - 1) >> 8);
 310         if (start == end) {
 311                 result = mpu_memory_write(st, st->i2c_addr, addr, len, d);
 312         } else {
 313                 end <<= 8;
 314                 len1 = end - addr;
 315                 len2 = len - len1;
 316                 result = mpu_memory_write(st, st->i2c_addr, addr, len1, d);
 317                 result |= mpu_memory_write(st, st->i2c_addr, end, len2,
 318                                                                 d + len1);
 319         }
 320
 321         return result;
 322 }
 323
 324 /**
 325  *  index_of_key()- Inverse lookup of the index of an MPL product key .
 326  *  @key: the MPL product indentifier also referred to as 'key'.
 327  */
 328 static short index_of_key(u16 key)
 329 {
 330         int i;
 331         for (i = 0; i < NUM_OF_PROD_REVS; i++)
 332                 if (prod_rev_map[i].mpl_product_key == key)
 333                         return (short)i;
 334         return -EINVAL;
 335 }
 336
 337 int inv_get_silicon_rev_mpu6500(struct inv_mpu_iio_s *st)
 338 {
 339         struct inv_chip_info_s *chip_info = &st->chip_info;
 340         int result;
 341         u8 whoami, sw_rev;
 342
 343         result = inv_plat_read(st, REG_WHOAMI, 1, &whoami);
 344         if (result)
 345                 return result;
 346         if (whoami != MPU6500_ID && whoami != MPU9250_ID &&
 347                         whoami != MPU6515_ID)
 348                 return -EINVAL;
 349
 350         /*memory read need more time after power up */
 351         msleep(POWER_UP_TIME);
 352         result = mpu_memory_read(st, st->i2c_addr,
 353                         MPU6500_MEM_REV_ADDR, 1, &sw_rev);
 354         sw_rev &= INV_MPU_REV_MASK;
 355         if (result)
 356                 return result;
 357         if (sw_rev != 0)
 358                 return -EINVAL;
 359
 360         /* these values are place holders and not real values */
 361         chip_info->product_id = MPU6500_PRODUCT_REVISION;
 362         chip_info->product_revision = MPU6500_PRODUCT_REVISION;
 363         chip_info->silicon_revision = MPU6500_PRODUCT_REVISION;
 364         chip_info->software_revision = sw_rev;
 365         chip_info->gyro_sens_trim = DEFAULT_GYRO_TRIM;
 366         chip_info->accl_sens_trim = DEFAULT_ACCL_TRIM;
 367         chip_info->multi = 1;
 368
 369         return 0;
 370 }
 371
 372 int inv_get_silicon_rev_mpu6050(struct inv_mpu_iio_s *st)
 373 {
 374         int result;
 375         struct inv_reg_map_s *reg;
 376         u8 prod_ver = 0x00, prod_rev = 0x00;
 377         struct prod_rev_map_t *p_rev;
 378         u8 bank =
 379             (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
 380         u16 mem_addr = ((bank << 8) | MEM_ADDR_PROD_REV);
 381         u16 key;
 382         u8 regs[5];
 383         u16 sw_rev;
 384         short index;
 385         struct inv_chip_info_s *chip_info = &st->chip_info;
 386         reg = &st->reg;
 387
 388         result = inv_plat_read(st, REG_PRODUCT_ID, 1, &prod_ver);
 389         if (result)
 390                 return result;
 391         prod_ver &= 0xf;
 392         /*memory read need more time after power up */
 393         msleep(POWER_UP_TIME);
 394         result = mpu_memory_read(st, st->i2c_addr, mem_addr,
 395                         1, &prod_rev);
 396         if (result)
 397                 return result;
 398         prod_rev >>= 2;
 399         /* clean the prefetch and cfg user bank bits */
 400         result = inv_plat_single_write(st, reg->bank_sel, 0);
 401         if (result)
 402                 return result;
 403         /* get the software-product version, read from XA_OFFS_L */
 404         result = inv_plat_read(st, REG_XA_OFFS_L_TC,
 405                                 SOFT_PROD_VER_BYTES, regs);
 406         if (result)
 407                 return result;
 408
 409         sw_rev = (regs[4] & 0x01) << 2 |        /* 0x0b, bit 0 */
 410                  (regs[2] & 0x01) << 1 |        /* 0x09, bit 0 */
 411                  (regs[0] & 0x01);              /* 0x07, bit 0 */
 412         /* if 0, use the product key to determine the type of part */
 413         if (sw_rev == 0) {
 414                 key = MPL_PROD_KEY(prod_ver, prod_rev);
 415                 if (key == 0)
 416                         return -EINVAL;
 417                 index = index_of_key(key);
 418                 if (index < 0 || index >= NUM_OF_PROD_REVS)
 419                         return -EINVAL;
 420                 /* check MPL is compiled for this device */
 421                 if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1)
 422                         return -EINVAL;
 423                 p_rev = (struct prod_rev_map_t *)&prod_rev_map[index];
 424         /* if valid, use the software product key */
 425         } else if (sw_rev < ARRAY_SIZE(sw_rev_map)) {
 426                 p_rev = (struct prod_rev_map_t *)&sw_rev_map[sw_rev];
 427         } else {
 428                 return -EINVAL;
 429         }
 430         chip_info->product_id = prod_ver;
 431         chip_info->product_revision = prod_rev;
 432         chip_info->silicon_revision = p_rev->silicon_rev;
 433         chip_info->software_revision = sw_rev;
 434         chip_info->gyro_sens_trim = p_rev->gyro_trim;
 435         chip_info->accl_sens_trim = p_rev->accel_trim;
 436         if (chip_info->accl_sens_trim == 0)
 437                 chip_info->accl_sens_trim = DEFAULT_ACCL_TRIM;
 438         chip_info->multi = DEFAULT_ACCL_TRIM / chip_info->accl_sens_trim;
 439         if (chip_info->multi != 1)
 440                 pr_info("multi is %d\n", chip_info->multi);
 441         return result;
 442 }
 443
 444 /**
 445  *  read_accel_hw_self_test_prod_shift()- read the accelerometer hardware
 446  *                                         self-test bias shift calculated
 447  *                                         during final production test and
 448  *                                         stored in chip non-volatile memory.
 449  *  @st:  main data structure.
 450  *  @st_prod:   A pointer to an array of 3 elements to hold the values
 451  *              for production hardware self-test bias shifts returned to the
 452  *              user.
 453  *  @accl_sens: accel sensitivity.
 454  */
 455 static int read_accel_hw_self_test_prod_shift(struct inv_mpu_iio_s *st,
 456                                         int *st_prod, int *accl_sens)
 457 {
 458         u8 regs[4];
 459         u8 shift_code[3];
 460         int result, i;
 461
 462         for (i = 0; i < 3; i++)
 463                 st_prod[i] = 0;
 464
 465         result = inv_plat_read(st, REG_ST_GCT_X, ARRAY_SIZE(regs), regs);
 466
 467         if (result)
 468                 return result;
 469         if ((0 == regs[0])  && (0 == regs[1]) &&
 470             (0 == regs[2]) && (0 == regs[3]))
 471                 return -EINVAL;
 472         shift_code[X] = ((regs[0] & 0xE0) >> 3) | ((regs[3] & 0x30) >> 4);
 473         shift_code[Y] = ((regs[1] & 0xE0) >> 3) | ((regs[3] & 0x0C) >> 2);
 474         shift_code[Z] = ((regs[2] & 0xE0) >> 3) |  (regs[3] & 0x03);
 475         for (i = 0; i < 3; i++) {
 476                 if (shift_code[i] != 0)
 477                         st_prod[i] = accl_sens[i] *
 478                                 accl_st_tb[shift_code[i] - 1];
 479         }
 480
 481         return 0;
 482 }
 483
 484 /**
 485 * inv_check_accl_self_test()- check accel self test. this function returns
 486 *                              zero as success. A non-zero return value
 487 *                              indicates failure in self test.
 488 *  @*st: main data structure.
 489 *  @*reg_avg: average value of normal test.
 490 *  @*st_avg:  average value of self test
 491 */
 492 static int inv_check_accl_self_test(struct inv_mpu_iio_s *st,
 493         int *reg_avg, int *st_avg){
 494         int gravity, reg_z_avg, g_z_sign, j, ret_val;
 495         int tmp;
 496         int st_shift_prod[THREE_AXIS], st_shift_cust[THREE_AXIS];
 497         int st_shift_ratio[THREE_AXIS];
 498         int accel_sens[THREE_AXIS];
 499
 500         if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
 501             st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
 502                 return 0;
 503         g_z_sign = 1;
 504         ret_val = 0;
 505         tmp = DEF_ST_SCALE * DEF_ST_PRECISION / DEF_ST_ACCEL_FS_MG;
 506         for (j = 0; j < 3; j++)
 507                 accel_sens[j] = tmp;
 508
 509         if (MPL_PROD_KEY(st->chip_info.product_id,
 510                          st->chip_info.product_revision) ==
 511             MPU_PRODUCT_KEY_B1_E1_5) {
 512                 /* half sensitivity Z accelerometer parts */
 513                 accel_sens[Z] /= 2;
 514         } else {
 515                 /* half sensitivity X, Y, Z accelerometer parts */
 516                 accel_sens[X] /= st->chip_info.multi;
 517                 accel_sens[Y] /= st->chip_info.multi;
 518                 accel_sens[Z] /= st->chip_info.multi;
 519         }
 520         gravity = accel_sens[Z];
 521         reg_z_avg = reg_avg[Z] - g_z_sign * gravity * DEF_ST_PRECISION;
 522         ret_val = read_accel_hw_self_test_prod_shift(st, st_shift_prod,
 523                                                         accel_sens);
 524         if (ret_val)
 525                 return ret_val;
 526
 527         for (j = 0; j < 3; j++) {
 528                 st_shift_cust[j] = abs(reg_avg[j] - st_avg[j]);
 529                 if (st_shift_prod[j]) {
 530                         tmp = st_shift_prod[j] / DEF_ST_PRECISION;
 531                         st_shift_ratio[j] = abs(st_shift_cust[j] / tmp
 532                                 - DEF_ST_PRECISION);
 533                         if (st_shift_ratio[j] > DEF_ACCEL_ST_SHIFT_DELTA)
 534                                 ret_val = 1;
 535                 } else {
 536                         if (st_shift_cust[j] <
 537                                 DEF_ACCEL_ST_SHIFT_MIN * gravity)
 538                                 ret_val = 1;
 539                         if (st_shift_cust[j] >
 540                                 DEF_ACCEL_ST_SHIFT_MAX * gravity)
 541                                 ret_val = 1;
 542                 }
 543         }
 544
 545         return ret_val;
 546 }
 547
 548 /**
 549 * inv_check_3500_gyro_self_test() check gyro self test. this function returns
 550 *                                 zero as success. A non-zero return value
 551 *                                 indicates failure in self test.
 552 *  @*st: main data structure.
 553 *  @*reg_avg: average value of normal test.
 554 *  @*st_avg:  average value of self test
 555 */
 556
 557 static int inv_check_3500_gyro_self_test(struct inv_mpu_iio_s *st,
 558         int *reg_avg, int *st_avg){
 559         int result;
 560         int gst[3], ret_val;
 561         int gst_otp[3], i;
 562         u8 st_code[THREE_AXIS];
 563         ret_val = 0;
 564
 565         for (i = 0; i < 3; i++)
 566                 gst[i] = st_avg[i] - reg_avg[i];
 567         result = inv_plat_read(st, REG_3500_OTP, THREE_AXIS, st_code);
 568         if (result)
 569                 return result;
 570         gst_otp[0] = 0;
 571         gst_otp[1] = 0;
 572         gst_otp[2] = 0;
 573         for (i = 0; i < 3; i++) {
 574                 if (st_code[i] != 0)
 575                         gst_otp[i] = gyro_3500_st_tb[st_code[i] - 1];
 576         }
 577         /* check self test value passing criterion. Using the DEF_ST_TOR
 578          * for certain degree of tolerance */
 579         for (i = 0; i < 3; i++) {
 580                 if (gst_otp[i] == 0) {
 581                         if (abs(gst[i]) * DEF_ST_TOR < DEF_ST_OTP0_THRESH *
 582                                                         DEF_ST_PRECISION *
 583                                                         DEF_GYRO_SCALE)
 584                                 ret_val |= (1 << i);
 585                 } else {
 586                         if (abs(gst[i]/gst_otp[i] - DEF_ST_PRECISION) >
 587                                         DEF_GYRO_CT_SHIFT_DELTA)
 588                                 ret_val |= (1 << i);
 589                 }
 590         }
 591         /* check for absolute value passing criterion. Using DEF_ST_TOR
 592          * for certain degree of tolerance */
 593         for (i = 0; i < 3; i++) {
 594                 if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH *
 595                     DEF_ST_PRECISION * DEF_GYRO_SCALE)
 596                         ret_val |= (1 << i);
 597         }
 598
 599         return ret_val;
 600 }
 601
 602 /**
 603 * inv_check_6050_gyro_self_test() - check 6050 gyro self test. this function
 604 *                                   returns zero as success. A non-zero return
 605 *                                   value indicates failure in self test.
 606 *  @*st: main data structure.
 607 *  @*reg_avg: average value of normal test.
 608 *  @*st_avg:  average value of self test
 609 */
 610 static int inv_check_6050_gyro_self_test(struct inv_mpu_iio_s *st,
 611         int *reg_avg, int *st_avg){
 612         int result;
 613         int ret_val;
 614         int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
 615         u8 regs[3];
 616
 617         if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
 618             st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
 619                 return 0;
 620
 621         ret_val = 0;
 622         result = inv_plat_read(st, REG_ST_GCT_X, 3, regs);
 623         if (result)
 624                 return result;
 625         regs[X] &= 0x1f;
 626         regs[Y] &= 0x1f;
 627         regs[Z] &= 0x1f;
 628         for (i = 0; i < 3; i++) {
 629                 if (regs[i] != 0)
 630                         st_shift_prod[i] = gyro_6050_st_tb[regs[i] - 1];
 631                 else
 632                         st_shift_prod[i] = 0;
 633         }
 634         st_shift_prod[1] = -st_shift_prod[1];
 635
 636         for (i = 0; i < 3; i++) {
 637                 st_shift_cust[i] =  st_avg[i] - reg_avg[i];
 638                 if (st_shift_prod[i]) {
 639                         st_shift_ratio[i] = abs(st_shift_cust[i] /
 640                                 st_shift_prod[i] - DEF_ST_PRECISION);
 641                         if (st_shift_ratio[i] > DEF_GYRO_CT_SHIFT_DELTA)
 642                                 ret_val = 1;
 643                 } else {
 644                         if (st_shift_cust[i] < DEF_ST_PRECISION *
 645                                 DEF_GYRO_CT_SHIFT_MIN * DEF_SELFTEST_GYRO_SENS)
 646                                 ret_val = 1;
 647                         if (st_shift_cust[i] > DEF_ST_PRECISION *
 648                                 DEF_GYRO_CT_SHIFT_MAX * DEF_SELFTEST_GYRO_SENS)
 649                                 ret_val = 1;
 650                 }
 651         }
 652         /* check for absolute value passing criterion. Using DEF_ST_TOR
 653          * for certain degree of tolerance */
 654         for (i = 0; i < 3; i++)
 655                 if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH *
 656                     DEF_ST_PRECISION * DEF_GYRO_SCALE)
 657                         ret_val = 1;
 658
 659         return ret_val;
 660 }
 661
 662 /**
 663 * inv_check_6500_gyro_self_test() - check 6500 gyro self test. this function
 664 *                                   returns zero as success. A non-zero return
 665 *                                   value indicates failure in self test.
 666 *  @*st: main data structure.
 667 *  @*reg_avg: average value of normal test.
 668 *  @*st_avg:  average value of self test
 669 */
 670 static int inv_check_6500_gyro_self_test(struct inv_mpu_iio_s *st,
 671         int *reg_avg, int *st_avg)
 672 {
 673         u8 regs[3];
 674         int ret_val, result;
 675         int otp_value_zero = 0;
 676         int st_shift_prod[3], st_shift_cust[3], i;
 677
 678         ret_val = 0;
 679         result = inv_plat_read(st, REG_6500_XG_ST_DATA, 3, regs);
 680         if (result)
 681                 return result;
 682         pr_debug("%s self_test gyro shift_code - %02x %02x %02x\n",
 683                  st->hw->name, regs[0], regs[1], regs[2]);
 684
 685         for (i = 0; i < 3; i++) {
 686                 if (regs[i] != 0) {
 687                         st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
 688                 } else {
 689                         st_shift_prod[i] = 0;
 690                         otp_value_zero = 1;
 691                 }
 692         }
 693         pr_debug("%s self_test gyro st_shift_prod - %+d %+d %+d\n",
 694                  st->hw->name, st_shift_prod[0], st_shift_prod[1],
 695                  st_shift_prod[2]);
 696
 697         for (i = 0; i < 3; i++) {
 698                 st_shift_cust[i] = st_avg[i] - reg_avg[i];
 699                 if (!otp_value_zero) {
 700                         /* Self Test Pass/Fail Criteria A */
 701                         if (st_shift_cust[i] < DEF_6500_GYRO_CT_SHIFT_DELTA
 702                                                 * st_shift_prod[i])
 703                                         ret_val = 1;
 704                 } else {
 705                         /* Self Test Pass/Fail Criteria B */
 706                         if (st_shift_cust[i] < DEF_GYRO_ST_AL *
 707                                                 DEF_SELFTEST_GYRO_SENS *
 708                                                 DEF_ST_PRECISION)
 709                                 ret_val = 1;
 710                 }
 711         }
 712         pr_debug("%s self_test gyro st_shift_cust - %+d %+d %+d\n",
 713                  st->hw->name, st_shift_cust[0], st_shift_cust[1],
 714                  st_shift_cust[2]);
 715
 716         if (ret_val == 0) {
 717                 /* Self Test Pass/Fail Criteria C */
 718                 for (i = 0; i < 3; i++)
 719                         if (abs(reg_avg[i]) > DEF_GYRO_OFFSET_MAX *
 720                                                 DEF_SELFTEST_GYRO_SENS *
 721                                                 DEF_ST_PRECISION)
 722                                 ret_val = 1;
 723         }
 724
 725         return ret_val;
 726 }
 727
 728 /**
 729 * inv_check_6500_accel_self_test() - check 6500 accel self test. this function
 730 *                                   returns zero as success. A non-zero return
 731 *                                   value indicates failure in self test.
 732 *  @*st: main data structure.
 733 *  @*reg_avg: average value of normal test.
 734 *  @*st_avg:  average value of self test
 735 */
 736 static int inv_check_6500_accel_self_test(struct inv_mpu_iio_s *st,
 737                                                 int *reg_avg, int *st_avg) {
 738         int ret_val, result;
 739         int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
 740         u8 regs[3];
 741         int otp_value_zero = 0;
 742
 743 #define ACCEL_ST_AL_MIN ((DEF_ACCEL_ST_AL_MIN * DEF_ST_SCALE \
 744                                  / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
 745 #define ACCEL_ST_AL_MAX ((DEF_ACCEL_ST_AL_MAX * DEF_ST_SCALE \
 746                                  / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
 747
 748         ret_val = 0;
 749         result = inv_plat_read(st, REG_6500_XA_ST_DATA, 3, regs);
 750         if (result)
 751                 return result;
 752         pr_debug("%s self_test accel shift_code - %02x %02x %02x\n",
 753                  st->hw->name, regs[0], regs[1], regs[2]);
 754
 755         for (i = 0; i < 3; i++) {
 756                 if (regs[i] != 0) {
 757                         st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
 758                 } else {
 759                         st_shift_prod[i] = 0;
 760                         otp_value_zero = 1;
 761                 }
 762         }
 763         pr_debug("%s self_test accel st_shift_prod - %+d %+d %+d\n",
 764                  st->hw->name, st_shift_prod[0], st_shift_prod[1],
 765                  st_shift_prod[2]);
 766
 767         if (!otp_value_zero) {
 768                 /* Self Test Pass/Fail Criteria A */
 769                 for (i = 0; i < 3; i++) {
 770                         st_shift_cust[i] = st_avg[i] - reg_avg[i];
 771                         st_shift_ratio[i] = abs(st_shift_cust[i] /
 772                                         st_shift_prod[i] - DEF_ST_PRECISION);
 773                         if (st_shift_ratio[i] > DEF_6500_ACCEL_ST_SHIFT_DELTA)
 774                                 ret_val = 1;
 775                 }
 776         } else {
 777                 /* Self Test Pass/Fail Criteria B */
 778                 for (i = 0; i < 3; i++) {
 779                         st_shift_cust[i] = abs(st_avg[i] - reg_avg[i]);
 780                         if (st_shift_cust[i] < ACCEL_ST_AL_MIN ||
 781                                         st_shift_cust[i] > ACCEL_ST_AL_MAX)
 782                                 ret_val = 1;
 783                 }
 784         }
 785         pr_debug("%s self_test accel st_shift_cust - %+d %+d %+d\n",
 786                  st->hw->name, st_shift_cust[0], st_shift_cust[1],
 787                  st_shift_cust[2]);
 788
 789         return ret_val;
 790 }
 791
 792 /*
 793  *  inv_do_test() - do the actual test of self testing
 794  */
 795 int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
 796                 int *gyro_result, int *accl_result)
 797 {
 798         struct inv_reg_map_s *reg;
 799         int result, i, j, packet_size;
 800         u8 data[BYTES_PER_SENSOR * 2], d;
 801         bool has_accl;
 802         int fifo_count, packet_count, ind, s;
 803
 804         reg = &st->reg;
 805         has_accl = (st->chip_type != INV_ITG3500);
 806         if (has_accl)
 807                 packet_size = BYTES_PER_SENSOR * 2;
 808         else
 809                 packet_size = BYTES_PER_SENSOR;
 810
 811         result = inv_plat_single_write(st, reg->int_enable, 0);
 812         if (result)
 813                 return result;
 814         /* disable the sensor output to FIFO */
 815         result = inv_plat_single_write(st, reg->fifo_en, 0);
 816         if (result)
 817                 return result;
 818         /* disable fifo reading */
 819         result = inv_plat_single_write(st, reg->user_ctrl, st->i2c_dis);
 820         if (result)
 821                 return result;
 822         /* clear FIFO */
 823         result = inv_plat_single_write(st, reg->user_ctrl, BIT_FIFO_RST | st->i2c_dis);
 824         if (result)
 825                 return result;
 826         /* setup parameters */
 827         result = inv_plat_single_write(st, reg->lpf, INV_FILTER_98HZ);
 828         if (result)
 829                 return result;
 830
 831         if (INV_MPU6500 == st->chip_type) {
 832                 /* config accel LPF register for MPU6500 */
 833                 result = inv_plat_single_write(st, REG_6500_ACCEL_CONFIG2,
 834                                                 DEF_ST_MPU6500_ACCEL_LPF |
 835                                                 BIT_FIFO_SIZE_1K);
 836                 if (result)
 837                         return result;
 838         }
 839
 840         result = inv_plat_single_write(st, reg->sample_rate_div,
 841                         DEF_SELFTEST_SAMPLE_RATE);
 842         if (result)
 843                 return result;
 844         /* wait for the sampling rate change to stabilize */
 845         mdelay(INV_MPU_SAMPLE_RATE_CHANGE_STABLE);
 846         result = inv_plat_single_write(st, reg->gyro_config,
 847                 self_test_flag | DEF_SELFTEST_GYRO_FS);
 848         if (result)
 849                 return result;
 850         if (has_accl) {
 851                 if (INV_MPU6500 == st->chip_type)
 852                         d = DEF_SELFTEST_6500_ACCEL_FS;
 853                 else
 854                         d = DEF_SELFTEST_ACCEL_FS;
 855                 d |= self_test_flag;
 856                 result = inv_plat_single_write(st, reg->accl_config, d);
 857                 if (result)
 858                         return result;
 859         }
 860         /* wait for the output to get stable */
 861         if (self_test_flag) {
 862                 if (INV_MPU6500 == st->chip_type)
 863                         msleep(DEF_ST_6500_STABLE_TIME);
 864                 else
 865                         msleep(DEF_ST_STABLE_TIME);
 866         }
 867
 868         /* enable FIFO reading */
 869         result = inv_plat_single_write(st, reg->user_ctrl, BIT_FIFO_EN | st->i2c_dis);
 870         if (result)
 871                 return result;
 872         /* enable sensor output to FIFO */
 873         if (has_accl)
 874                 d = BITS_GYRO_OUT | BIT_ACCEL_OUT;
 875         else
 876                 d = BITS_GYRO_OUT;
 877         for (i = 0; i < THREE_AXIS; i++) {
 878                 gyro_result[i] = 0;
 879                 accl_result[i] = 0;
 880         }
 881         s = 0;
 882         while (s < st->self_test.samples) {
 883                 result = inv_plat_single_write(st, reg->fifo_en, d);
 884                 if (result)
 885                         return result;
 886                 mdelay(DEF_GYRO_WAIT_TIME);
 887                 result = inv_plat_single_write(st, reg->fifo_en, 0);
 888                 if (result)
 889                         return result;
 890
 891                 result = inv_plat_read(st, reg->fifo_count_h,
 892                                         FIFO_COUNT_BYTE, data);
 893                 if (result)
 894                         return result;
 895                 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
 896                 pr_debug("%s self_test fifo_count - %d\n",
 897                          st->hw->name, fifo_count);
 898                 packet_count = fifo_count / packet_size;
 899                 i = 0;
 900                 while ((i < packet_count) && (s < st->self_test.samples)) {
 901                         short vals[3];
 902                         result = inv_plat_read(st, reg->fifo_r_w,
 903                                 packet_size, data);
 904                         if (result)
 905                                 return result;
 906                         ind = 0;
 907                         if (has_accl) {
 908                                 for (j = 0; j < THREE_AXIS; j++) {
 909                                         vals[j] = (short)be16_to_cpup(
 910                                             (__be16 *)(&data[ind + 2 * j]));
 911                                         accl_result[j] += vals[j];
 912                                 }
 913                                 ind += BYTES_PER_SENSOR;
 914                                 pr_debug(
 915                                     "%s self_test accel data - %d %+d %+d %+d",
 916                                     st->hw->name, s, vals[0], vals[1], vals[2]);
 917                         }
 918
 919                         for (j = 0; j < THREE_AXIS; j++) {
 920                                 vals[j] = (short)be16_to_cpup(
 921                                         (__be16 *)(&data[ind + 2 * j]));
 922                                 gyro_result[j] += vals[j];
 923                         }
 924                         pr_debug("%s self_test gyro data - %d %+d %+d %+d",
 925                                  st->hw->name, s, vals[0], vals[1], vals[2]);
 926
 927                         s++;
 928                         i++;
 929                 }
 930         }
 931
 932         if (has_accl) {
 933                 for (j = 0; j < THREE_AXIS; j++) {
 934                         accl_result[j] = accl_result[j] / s;
 935                         accl_result[j] *= DEF_ST_PRECISION;
 936                 }
 937         }
 938         for (j = 0; j < THREE_AXIS; j++) {
 939                 gyro_result[j] = gyro_result[j] / s;
 940                 gyro_result[j] *= DEF_ST_PRECISION;
 941         }
 942
 943         return 0;
 944 }
 945
 946 /*
 947  *  inv_recover_setting() recover the old settings after everything is done
 948  */
 949 void inv_recover_setting(struct inv_mpu_iio_s *st)
 950 {
 951         struct inv_reg_map_s *reg;
 952         int data;
 953
 954         reg = &st->reg;
 955         inv_plat_single_write(st, reg->gyro_config,
 956                              st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT);
 957         inv_plat_single_write(st, reg->lpf, st->chip_config.lpf);
 958         data = ONE_K_HZ/st->chip_config.new_fifo_rate - 1;
 959         inv_plat_single_write(st, reg->sample_rate_div, data);
 960         if (INV_ITG3500 != st->chip_type) {
 961                 inv_plat_single_write(st, reg->accl_config,
 962                                      (st->chip_config.accl_fs <<
 963                                      ACCL_CONFIG_FSR_SHIFT));
 964         }
 965         st->switch_gyro_engine(st, false);
 966         st->switch_accl_engine(st, false);
 967         st->set_power_state(st, false);
 968 }
 969
 970 static int inv_check_compass_self_test(struct inv_mpu_iio_s *st)
 971 {
 972         int result;
 973         u8 data[6];
 974         u8 counter, cntl;
 975         short x, y, z;
 976         u8 *sens;
 977         sens = st->chip_info.compass_sens;
 978
 979         /* set to bypass mode */
 980         result = inv_plat_single_write(st, REG_INT_PIN_CFG,
 981                                 st->plat_data.int_config | BIT_BYPASS_EN);
 982         if (result) {
 983                 result = inv_plat_single_write(st, REG_INT_PIN_CFG,
 984                                 st->plat_data.int_config);
 985                 return result;
 986         }
 987         /* set to power down mode */
 988         result = inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_PD);
 989         if (result)
 990                 goto AKM_fail;
 991
 992         /* write 1 to ASTC register */
 993         result = inv_secondary_write(st, REG_AKM_ST_CTRL, DATA_AKM_SELF_TEST);
 994         if (result)
 995                 goto AKM_fail;
 996         /* set self test mode */
 997         result = inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_ST);
 998         if (result)
 999                 goto AKM_fail;
1000         counter = DEF_ST_COMPASS_TRY_TIMES;
1001         while (counter > 0) {
1002                 usleep_range(DEF_ST_COMPASS_WAIT_MIN, DEF_ST_COMPASS_WAIT_MAX);
1003                 result = inv_secondary_read(st, REG_AKM_STATUS, 1, data);
1004                 if (result)
1005                         goto AKM_fail;
1006                 if ((data[0] & DATA_AKM_DRDY) == 0)
1007                         counter--;
1008                 else
1009                         counter = 0;
1010         }
1011         if ((data[0] & DATA_AKM_DRDY) == 0) {
1012                 result = -EINVAL;
1013                 goto AKM_fail;
1014         }
1015         result = inv_secondary_read(st, REG_AKM_MEASURE_DATA,
1016                                         BYTES_PER_SENSOR, data);
1017         if (result)
1018                 goto AKM_fail;
1019
1020         x = le16_to_cpup((__le16 *)(&data[0]));
1021         y = le16_to_cpup((__le16 *)(&data[2]));
1022         z = le16_to_cpup((__le16 *)(&data[4]));
1023         x = ((x * (sens[0] + 128)) >> 8);
1024         y = ((y * (sens[1] + 128)) >> 8);
1025         z = ((z * (sens[2] + 128)) >> 8);
1026         if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) {
1027                 result = inv_secondary_read(st, REG_AKM8963_CNTL1, 1, &cntl);
1028                 if (result)
1029                         goto AKM_fail;
1030                 if (0 == (cntl & DATA_AKM8963_BIT)) {
1031                         x <<= DEF_ST_COMPASS_8963_SHIFT;
1032                         y <<= DEF_ST_COMPASS_8963_SHIFT;
1033                         z <<= DEF_ST_COMPASS_8963_SHIFT;
1034                 }
1035         }
1036         result = -EINVAL;
1037         if (x > st->compass_st_upper[X] || x < st->compass_st_lower[X])
1038                 goto AKM_fail;
1039         if (y > st->compass_st_upper[Y] || y < st->compass_st_lower[Y])
1040                 goto AKM_fail;
1041         if (z > st->compass_st_upper[Z] || z < st->compass_st_lower[Z])
1042                 goto AKM_fail;
1043         result = 0;
1044 AKM_fail:
1045         /*write 0 to ASTC register */
1046         result |= inv_secondary_write(st, REG_AKM_ST_CTRL, 0);
1047         /*set to power down mode */
1048         result |= inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_PD);
1049         /*restore to non-bypass mode */
1050         result |= inv_plat_single_write(st, REG_INT_PIN_CFG,
1051                         st->plat_data.int_config);
1052         return result;
1053 }
1054
1055 int inv_power_up_self_test(struct inv_mpu_iio_s *st)
1056 {
1057         int result;
1058
1059         result = st->set_power_state(st, true);
1060         if (result)
1061                 return result;
1062         result = st->switch_accl_engine(st, true);
1063         if (result)
1064                 return result;
1065         result = st->switch_gyro_engine(st, true);
1066         if (result)
1067                 return result;
1068
1069         return 0;
1070 }
1071
1072 /*
1073  *  inv_hw_self_test() - main function to do hardware self test
1074  */
1075 int inv_hw_self_test(struct inv_mpu_iio_s *st)
1076 {
1077         int result;
1078         int gyro_bias_st[THREE_AXIS], gyro_bias_regular[THREE_AXIS];
1079         int accl_bias_st[THREE_AXIS], accl_bias_regular[THREE_AXIS];
1080         int test_times;
1081         char compass_result, accel_result, gyro_result;
1082
1083         result = inv_power_up_self_test(st);
1084         if (result)
1085                 return result;
1086         compass_result = 0;
1087         accel_result   = 0;
1088         gyro_result    = 0;
1089         test_times = DEF_ST_TRY_TIMES;
1090         while (test_times > 0) {
1091                 result = inv_do_test(st, 0, gyro_bias_regular,
1092                         accl_bias_regular);
1093                 if (result == -EAGAIN)
1094                         test_times--;
1095                 else
1096                         test_times = 0;
1097         }
1098         if (result)
1099                 goto test_fail;
1100
1101         test_times = DEF_ST_TRY_TIMES;
1102         while (test_times > 0) {
1103                 result = inv_do_test(st, BITS_SELF_TEST_EN, gyro_bias_st,
1104                                         accl_bias_st);
1105                 if (result == -EAGAIN)
1106                         test_times--;
1107                 else
1108                         break;
1109         }
1110         if (result)
1111                 goto test_fail;
1112         if (st->chip_type == INV_ITG3500) {
1113                 gyro_result = !inv_check_3500_gyro_self_test(st,
1114                         gyro_bias_regular, gyro_bias_st);
1115         } else {
1116                 if (st->chip_config.has_compass)
1117                         compass_result = !inv_check_compass_self_test(st);
1118
1119                  if (INV_MPU6050 == st->chip_type) {
1120                         accel_result = !inv_check_accl_self_test(st,
1121                                 accl_bias_regular, accl_bias_st);
1122                         gyro_result = !inv_check_6050_gyro_self_test(st,
1123                                 gyro_bias_regular, gyro_bias_st);
1124                 } else if (INV_MPU6500 == st->chip_type) {
1125                         accel_result = !inv_check_6500_accel_self_test(st,
1126                                 accl_bias_regular, accl_bias_st);
1127                         gyro_result = !inv_check_6500_gyro_self_test(st,
1128                                 gyro_bias_regular, gyro_bias_st);
1129                 }
1130         }
1131 test_fail:
1132         inv_recover_setting(st);
1133
1134         return (compass_result << DEF_ST_COMPASS_RESULT_SHIFT) |
1135                 (accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result;
1136 }
1137
1138 static int inv_load_firmware(struct inv_mpu_iio_s *st,
1139         u8 *data, int size)
1140 {
1141         int bank, write_size;
1142         int result;
1143         u16 memaddr;
1144
1145         /* Write and verify memory */
1146         for (bank = 0; size > 0; bank++,
1147                 size -= write_size,
1148                 data += write_size) {
1149                 if (size > MPU_MEM_BANK_SIZE)
1150                         write_size = MPU_MEM_BANK_SIZE;
1151                 else
1152                         write_size = size;
1153
1154                 memaddr = ((bank << 8) | 0x00);
1155
1156                 result = mem_w(memaddr, write_size, data);
1157                 if (result)
1158                         return result;
1159         }
1160         return 0;
1161 }
1162
1163 static int inv_verify_firmware(struct inv_mpu_iio_s *st,
1164         u8 *data, int size)
1165 {
1166         int bank, write_size;
1167         int result;
1168         u16 memaddr;
1169         u8 firmware[MPU_MEM_BANK_SIZE];
1170
1171         /* Write and verify memory */
1172         for (bank = 0; size > 0; bank++,
1173                 size -= write_size,
1174                 data += write_size) {
1175                 if (size > MPU_MEM_BANK_SIZE)
1176                         write_size = MPU_MEM_BANK_SIZE;
1177                 else
1178                         write_size = size;
1179
1180                 memaddr = ((bank << 8) | 0x00);
1181                 result = mpu_memory_read(st,
1182                         st->i2c_addr, memaddr, write_size, firmware);
1183                 if (result)
1184                         return result;
1185                 if (0 != memcmp(firmware, data, write_size))
1186                         return -EINVAL;
1187         }
1188         return 0;
1189 }
1190
1191 static int inv_set_fifo_div(struct inv_mpu_iio_s *st,
1192                 u16 fifoRate)
1193 {
1194         u8 regs[2];
1195         int result = 0;
1196         /*For some reason DINAC4 is defined as 0xb8, but DINBC4 is not*/
1197         const u8 regs_end[] = {DINAFE, DINAF2, DINAAB, 0xc4,
1198                                         DINAAA, DINAF1, DINADF, DINADF,
1199                                         0xbb, 0xaf, DINADF, DINADF};
1200
1201         regs[0] = (u8)((fifoRate >> 8) & 0xff);
1202         regs[1] = (u8)(fifoRate & 0xff);
1203         result = mem_w_key(KEY_D_0_22, ARRAY_SIZE(regs), regs);
1204         if (result)
1205                 return result;
1206
1207         /*Modify the FIFO handler to reset the tap/orient interrupt flags*/
1208         /* each time the FIFO handler runs*/
1209         result = mem_w_key(KEY_CFG_6, ARRAY_SIZE(regs_end), regs_end);
1210
1211         return result;
1212 }
1213
1214 int inv_send_quaternion(struct inv_mpu_iio_s *st, bool on)
1215 {
1216         const u8 regs_on[] = {DINBC0, DINBC2,
1217                                          DINBC4, DINBC6};
1218         const u8 regs_off[] = {DINA80, DINA80,
1219                                           DINA80, DINA80};
1220         const u8 *regs;
1221         u8 result;
1222         if (on)
1223                 regs = regs_on;
1224         else
1225                 regs = regs_off;
1226         result = mem_w_key(KEY_CFG_LP_QUAT, ARRAY_SIZE(regs_on), regs);
1227
1228         return result;
1229 }
1230
1231 int inv_set_display_orient_interrupt_dmp(struct inv_mpu_iio_s *st,
1232                                                 bool on)
1233 {
1234         /*Turn on the display orientation interrupt in the DMP*/
1235         int result;
1236         u8  regs[] = {0xd8};
1237
1238         if (on)
1239                 regs[0] = 0xd9;
1240         result = mem_w_key(KEY_CFG_DISPLAY_ORIENT_INT, 1, regs);
1241         return result;
1242 }
1243
1244 int inv_set_fifo_rate(struct inv_mpu_iio_s *st, u16 fifo_rate)
1245 {
1246         u8 divider;
1247         int result;
1248
1249         divider = (u8)(ONE_K_HZ / fifo_rate) - 1;
1250         if (divider > DMP_MAX_DIVIDER) {
1251                 st->sample_divider = DMP_MAX_DIVIDER;
1252                 st->fifo_divider =
1253                         (u8)(DMP_DEFAULT_FIFO_RATE / fifo_rate) - 1;
1254         } else {
1255                 st->sample_divider = divider;
1256                 st->fifo_divider = 0;
1257         }
1258
1259         result = inv_set_fifo_div(st, st->fifo_divider);
1260         return result;
1261 }
1262
1263 static int inv_set_tap_interrupt_dmp(struct inv_mpu_iio_s *st,
1264         u8 on)
1265 {
1266         int result;
1267         u8  regs[] = {0};
1268
1269         if (on)
1270                 regs[0] = 0xf8;
1271         else
1272                 regs[0] = DINAD8;
1273         result = mem_w_key(KEY_CFG_20, ARRAY_SIZE(regs), regs);
1274         if (result)
1275                 return result;
1276         return result;
1277 }
1278
1279 int inv_set_tap_threshold_dmp(struct inv_mpu_iio_s *st,
1280                                 u32 axis, u16 threshold)
1281 {
1282         /* Sets the tap threshold in the dmp
1283         Simultaneously sets secondary tap threshold to help correct the tap
1284         direction for soft taps */
1285         int result;
1286         /* DMP Algorithm */
1287         u8 data[2];
1288         int sampleDivider;
1289         int scaledThreshold;
1290         u32 dmpThreshold;
1291         u8 sample_div;
1292         const u32  accel_sens = (0x20000000 / 0x00010000);
1293
1294         if ((axis & ~(INV_TAP_AXIS_ALL)) || (threshold > (1 << 15)))
1295                 return -EINVAL;
1296         sample_div = st->sample_divider;
1297
1298         sampleDivider = (1 + sample_div);
1299         /* Scale factor corresponds linearly using
1300         * 0  : 0
1301         * 25 : 0.0250  g/ms
1302         * 50 : 0.0500  g/ms
1303         * 100: 1.0000  g/ms
1304         * 200: 2.0000  g/ms
1305         * 400: 4.0000  g/ms
1306         * 800: 8.0000  g/ms
1307         */
1308         /*multiply by 1000 to avoid floating point 1000/1000*/
1309         scaledThreshold = threshold;
1310         /* Convert to per sample */
1311         scaledThreshold *= sampleDivider;
1312
1313         /* Scale to DMP 16 bit value */
1314         if (accel_sens != 0)
1315                 dmpThreshold = (u32)(scaledThreshold * accel_sens);
1316         else
1317                 return -EINVAL;
1318         dmpThreshold = dmpThreshold / DMP_PRECISION;
1319
1320         data[0] = dmpThreshold >> 8;
1321         data[1] = dmpThreshold & 0xFF;
1322
1323         /* MPL algorithm */
1324         if (axis & INV_TAP_AXIS_X) {
1325                 result = mem_w_key(KEY_DMP_TAP_THR_X, ARRAY_SIZE(data), data);
1326                 if (result)
1327                         return result;
1328
1329                 /*Also set additional threshold for correcting the direction
1330                 of taps that were very near the threshold. */
1331                 data[0] = (dmpThreshold * 3 / 4) >> 8;
1332                 data[1] = (dmpThreshold * 3 / 4) & 0xFF;
1333                 result = mem_w_key(KEY_D_1_36, ARRAY_SIZE(data), data);
1334                 if (result)
1335                         return result;
1336         }
1337         if (axis & INV_TAP_AXIS_Y) {
1338                 result = mem_w_key(KEY_DMP_TAP_THR_Y, 2, data);
1339                 if (result)
1340                         return result;
1341                 data[0] = (dmpThreshold * 3 / 4) >> 8;
1342                 data[1] = (dmpThreshold * 3 / 4) & 0xFF;
1343
1344                 result = mem_w_key(KEY_D_1_40, ARRAY_SIZE(data), data);
1345                 if (result)
1346                         return result;
1347         }
1348         if (axis & INV_TAP_AXIS_Z) {
1349                 result = mem_w_key(KEY_DMP_TAP_THR_Z, ARRAY_SIZE(data), data);
1350                 if (result)
1351                         return result;
1352                 data[0] = (dmpThreshold * 3 / 4) >> 8;
1353                 data[1] = (dmpThreshold * 3 / 4) & 0xFF;
1354
1355                 result = mem_w_key(KEY_D_1_44, ARRAY_SIZE(data), data);
1356                 if (result)
1357                         return result;
1358         }
1359         return 0;
1360 }
1361
1362 static int inv_set_tap_axes_dmp(struct inv_mpu_iio_s *st,
1363                                 u32 axes)
1364 {
1365         /* Sets a mask in the DMP that indicates what tap events
1366         should result in an interrupt */
1367         u8 regs[4];
1368         u8 result;
1369
1370         /* check if any spurious bit other the ones expected are set */
1371         if (axes & (~(INV_TAP_ALL_DIRECTIONS)))
1372                 return -EINVAL;
1373
1374         regs[0] = (u8)axes;
1375         result = mem_w_key(KEY_D_1_72, 1, regs);
1376
1377         return result;
1378 }
1379
1380 int inv_set_min_taps_dmp(struct inv_mpu_iio_s *st,
1381                                 u16 min_taps) {
1382         /*Indicates the minimum number of consecutive taps required
1383                 before the DMP will generate an interrupt */
1384         u8 regs[1];
1385         u8 result;
1386         /* check if any spurious bit other the ones expected are set */
1387         if ((min_taps > DMP_MAX_MIN_TAPS) || (min_taps < 1))
1388                 return -EINVAL;
1389         regs[0] = (u8)(min_taps-1);
1390         result = mem_w_key(KEY_D_1_79, ARRAY_SIZE(regs), regs);
1391
1392         return result;
1393 }
1394
1395 int  inv_set_tap_time_dmp(struct inv_mpu_iio_s *st, u16 time)
1396 {
1397         /* Determines how long after a tap the DMP requires before
1398           another tap can be registered*/
1399         int result;
1400         /* DMP Algorithm */
1401         u16 dmpTime;
1402         u8 data[2];
1403         u8 sampleDivider;
1404
1405         sampleDivider = st->sample_divider;
1406         sampleDivider++;
1407
1408         /* 60 ms minimum time added */
1409         dmpTime = ((time) / sampleDivider);
1410         data[0] = dmpTime >> 8;
1411         data[1] = dmpTime & 0xFF;
1412
1413         result = mem_w_key(KEY_DMP_TAPW_MIN, ARRAY_SIZE(data), data);
1414
1415         return result;
1416 }
1417
1418 static int inv_set_multiple_tap_time_dmp(struct inv_mpu_iio_s *st,
1419                                         u32 time)
1420 {
1421         /*Determines how close together consecutive taps must occur
1422         to be considered double/triple taps*/
1423         int result;
1424         /* DMP Algorithm */
1425         u16 dmpTime;
1426         u8 data[2];
1427         u8 sampleDivider;
1428
1429         sampleDivider = st->sample_divider;
1430         sampleDivider++;
1431
1432         /* 60 ms minimum time added */
1433         dmpTime = ((time) / sampleDivider);
1434         data[0] = dmpTime >> 8;
1435         data[1] = dmpTime & 0xFF;
1436         result = mem_w_key(KEY_D_1_218, ARRAY_SIZE(data), data);
1437
1438         return result;
1439 }
1440
1441 int inv_q30_mult(int a, int b)
1442 {
1443         u64 temp;
1444         int result;
1445
1446         temp = (u64)a * b;
1447         result = (int)(temp >> DMP_MULTI_SHIFT);
1448
1449         return result;
1450 }
1451
1452 static u16 inv_row_2_scale(const s8 *row)
1453 {
1454         u16 b;
1455
1456         if (row[0] > 0)
1457                 b = 0;
1458         else if (row[0] < 0)
1459                 b = 4;
1460         else if (row[1] > 0)
1461                 b = 1;
1462         else if (row[1] < 0)
1463                 b = 5;
1464         else if (row[2] > 0)
1465                 b = 2;
1466         else if (row[2] < 0)
1467                 b = 6;
1468         else
1469                 b = 7;
1470
1471         return b;
1472 }
1473
1474 /** Converts an orientation matrix made up of 0,+1,and -1 to a scalar
1475 *       representation.
1476 * @param[in] mtx Orientation matrix to convert to a scalar.
1477 * @return Description of orientation matrix. The lowest 2 bits (0 and 1)
1478 * represent the column the one is on for the
1479 * first row, with the bit number 2 being the sign. The next 2 bits
1480 * (3 and 4) represent
1481 * the column the one is on for the second row with bit number 5 being
1482 * the sign.
1483 * The next 2 bits (6 and 7) represent the column the one is on for the
1484 * third row with
1485 * bit number 8 being the sign. In binary the identity matrix would therefor
1486 * be: 010_001_000 or 0x88 in hex.
1487 */
1488 static u16 inv_orientation_matrix_to_scaler(const signed char *mtx)
1489 {
1490
1491         u16 scalar;
1492         scalar = inv_row_2_scale(mtx);
1493         scalar |= inv_row_2_scale(mtx + 3) << 3;
1494         scalar |= inv_row_2_scale(mtx + 6) << 6;
1495
1496         return scalar;
1497 }
1498 #if 0
1499 static int inv_disable_gyro_cal(struct inv_mpu_iio_s *st)
1500 {
1501         const u8 regs[] = {
1502                 0xb8, 0xaa, 0xaa, 0xaa,
1503                 0xb0, 0x88, 0xc3, 0xc5,
1504                 0xc7
1505         };
1506         return mem_w_key(KEY_CFG_MOTION_BIAS, ARRAY_SIZE(regs), regs);
1507 }
1508 #endif
1509
1510 static int inv_gyro_dmp_cal(struct inv_mpu_iio_s *st)
1511 {
1512         int inv_gyro_orient;
1513         u8 regs[3];
1514         int result;
1515
1516         u8 tmpD = DINA4C;
1517         u8 tmpE = DINACD;
1518         u8 tmpF = DINA6C;
1519
1520         inv_gyro_orient =
1521                 inv_orientation_matrix_to_scaler(st->plat_data.orientation);
1522
1523         if ((inv_gyro_orient & 3) == 0)
1524                 regs[0] = tmpD;
1525         else if ((inv_gyro_orient & 3) == 1)
1526                 regs[0] = tmpE;
1527         else if ((inv_gyro_orient & 3) == 2)
1528                 regs[0] = tmpF;
1529         if ((inv_gyro_orient & 0x18) == 0)
1530                 regs[1] = tmpD;
1531         else if ((inv_gyro_orient & 0x18) == 0x8)
1532                 regs[1] = tmpE;
1533         else if ((inv_gyro_orient & 0x18) == 0x10)
1534                 regs[1] = tmpF;
1535         if ((inv_gyro_orient & 0xc0) == 0)
1536                 regs[2] = tmpD;
1537         else if ((inv_gyro_orient & 0xc0) == 0x40)
1538                 regs[2] = tmpE;
1539         else if ((inv_gyro_orient & 0xc0) == 0x80)
1540                 regs[2] = tmpF;
1541
1542         result = mem_w_key(KEY_FCFG_1, ARRAY_SIZE(regs), regs);
1543         if (result)
1544                 return result;
1545
1546         if (inv_gyro_orient & 4)
1547                 regs[0] = DINA36 | 1;
1548         else
1549                 regs[0] = DINA36;
1550         if (inv_gyro_orient & 0x20)
1551                 regs[1] = DINA56 | 1;
1552         else
1553                 regs[1] = DINA56;
1554         if (inv_gyro_orient & 0x100)
1555                 regs[2] = DINA76 | 1;
1556         else
1557                 regs[2] = DINA76;
1558
1559         result = mem_w_key(KEY_FCFG_3, ARRAY_SIZE(regs), regs);
1560
1561         return result;
1562 }
1563
1564 static int inv_accel_dmp_cal(struct inv_mpu_iio_s *st)
1565 {
1566         int inv_accel_orient;
1567         int result;
1568         u8 regs[3];
1569         const u8 tmp[3] = { DINA0C, DINAC9, DINA2C };
1570         inv_accel_orient =
1571                 inv_orientation_matrix_to_scaler(st->plat_data.orientation);
1572
1573         regs[0] = tmp[inv_accel_orient & 3];
1574         regs[1] = tmp[(inv_accel_orient >> 3) & 3];
1575         regs[2] = tmp[(inv_accel_orient >> 6) & 3];
1576         result = mem_w_key(KEY_FCFG_2, ARRAY_SIZE(regs), regs);
1577         if (result)
1578                 return result;
1579
1580         regs[0] = DINA26;
1581         regs[1] = DINA46;
1582         regs[2] = DINA66;
1583         if (inv_accel_orient & 4)
1584                 regs[0] |= 1;
1585         if (inv_accel_orient & 0x20)
1586                 regs[1] |= 1;
1587         if (inv_accel_orient & 0x100)
1588                 regs[2] |= 1;
1589         result = mem_w_key(KEY_FCFG_7, ARRAY_SIZE(regs), regs);
1590
1591         return result;
1592 }
1593
1594 static u16 inv_orientation_matrix_to_scalar(const s8 *mtx)
1595 {
1596
1597         u16 scalar;
1598
1599         /*
1600        XYZ  010_001_000 Identity Matrix
1601        XZY  001_010_000
1602        YXZ  010_000_001
1603        YZX  000_010_001
1604        ZXY  001_000_010
1605        ZYX  000_001_010
1606         */
1607
1608         scalar = inv_row_2_scale(mtx);
1609         scalar |= inv_row_2_scale(mtx + 3) << 3;
1610         scalar |= inv_row_2_scale(mtx + 6) << 6;
1611
1612         return scalar;
1613 }
1614
1615 int inv_set_accel_bias_dmp(struct inv_mpu_iio_s *st)
1616 {
1617         int inv_accel_orient, result, i, accel_bias_body[3], out[3];
1618         int tmp[] = {1, 1, 1};
1619         int mask[] = {4, 0x20, 0x100};
1620         int accel_sf = 0x20000000;/* 536870912 */
1621         u8 *regs;
1622
1623         inv_accel_orient =
1624                 inv_orientation_matrix_to_scalar(st->plat_data.orientation);
1625
1626         for (i = 0; i < 3; i++)
1627                 if (inv_accel_orient & mask[i])
1628                         tmp[i] = -1;
1629
1630         for (i = 0; i < 3; i++)
1631                 accel_bias_body[i] = st->input_accel_bias[(inv_accel_orient >>
1632                                         (i * 3)) & 3] * tmp[i];
1633         for (i = 0; i < 3; i++)
1634                 accel_bias_body[i] = inv_q30_mult(accel_sf,
1635                                         accel_bias_body[i]);
1636         for (i = 0; i < 3; i++)
1637                 out[i] = cpu_to_be32p(&accel_bias_body[i]);
1638         regs = (u8 *)out;
1639         result = mem_w_key(KEY_D_ACCEL_BIAS, sizeof(out), regs);
1640
1641         return result;
1642 }
1643
1644 static int inv_set_gyro_sf_dmp(struct inv_mpu_iio_s *st)
1645 {
1646         /*The gyro threshold, in dps, above which taps will be rejected*/
1647         int result;
1648         /* DMP Algorithm */
1649         u8 sampleDivider;
1650         u32 gyro_sf;
1651         const u32 gyro_sens = 0x03e80000;
1652
1653         sampleDivider = st->sample_divider;
1654         gyro_sf = inv_q30_mult(gyro_sens,
1655                         (int)(DMP_TAP_SCALE * (sampleDivider + 1)));
1656         result = write_be32_key_to_mem(st, gyro_sf, KEY_D_0_104);
1657
1658         return result;
1659 }
1660
1661 static int inv_set_shake_reject_thresh_dmp(struct inv_mpu_iio_s *st,
1662                                                 int thresh)
1663 {       /*THIS FUNCTION FAILS MEM_W*/
1664         /*The gyro threshold, in dps, above which taps will be rejected */
1665         int result;
1666         /* DMP Algorithm */
1667         u8 sampleDivider;
1668         int thresh_scaled;
1669         u32 gyro_sf;
1670         const u32 gyro_sens = 0x03e80000;
1671         sampleDivider = st->sample_divider;
1672         gyro_sf = inv_q30_mult(gyro_sens, (int)(DMP_TAP_SCALE *
1673                         (sampleDivider + 1)));
1674         /* We're in units of DPS, convert it back to chip units*/
1675         /*split the operation to aviod overflow of integer*/
1676         thresh_scaled = gyro_sens / (1L << 16);
1677         thresh_scaled = thresh_scaled / thresh;
1678         thresh_scaled = gyro_sf / thresh_scaled;
1679         result = write_be32_key_to_mem(st, thresh_scaled, KEY_D_1_92);
1680
1681         return result;
1682 }
1683
1684 static int inv_set_shake_reject_time_dmp(struct inv_mpu_iio_s *st,
1685                                                 u32 time)
1686 {
1687         /* How long a gyro axis must remain above its threshold
1688         before taps are rejected */
1689         int result;
1690         /* DMP Algorithm */
1691         u16 dmpTime;
1692         u8 data[2];
1693         u8 sampleDivider;
1694
1695         sampleDivider = st->sample_divider;
1696         sampleDivider++;
1697
1698         /* 60 ms minimum time added */
1699         dmpTime = ((time) / sampleDivider);
1700         data[0] = dmpTime >> 8;
1701         data[1] = dmpTime & 0xFF;
1702
1703         result = mem_w_key(KEY_D_1_88, ARRAY_SIZE(data), data);
1704         return result;
1705 }
1706
1707 static int inv_set_shake_reject_timeout_dmp(struct inv_mpu_iio_s *st,
1708                                                 u32 time)
1709 {
1710         /*How long the gyros must remain below their threshold,
1711         after taps have been rejected, before taps can be detected again*/
1712         int result;
1713         /* DMP Algorithm */
1714         u16 dmpTime;
1715         u8 data[2];
1716         u8 sampleDivider;
1717
1718         sampleDivider = st->sample_divider;
1719         sampleDivider++;
1720
1721         /* 60 ms minimum time added */
1722         dmpTime = ((time) / sampleDivider);
1723         data[0] = dmpTime >> 8;
1724         data[1] = dmpTime & 0xFF;
1725
1726         result = mem_w_key(KEY_D_1_90, ARRAY_SIZE(data), data);
1727         return result;
1728 }
1729
1730 int inv_set_interrupt_on_gesture_event(struct inv_mpu_iio_s *st, bool on)
1731 {
1732         u8 result;
1733         const u8 regs_on[] = {DINADA, DINAB1, DINAB9,
1734                                          DINAF3, DINA8B, DINAA3, DINA91,
1735                                          DINAB6, DINADA, DINAB4, DINADA};
1736         const u8 regs_off[] = {0xd8, 0xb1, 0xb9, 0xf3, 0x8b,
1737                                           0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
1738         /*For some reason DINAC4 is defined as 0xb8,
1739         but DINBC4 is not defined.*/
1740         const u8 regs_end[] = {DINAFE, DINAF2, DINAAB, 0xc4,
1741                                         DINAAA, DINAF1, DINADF, DINADF,
1742                                         0xbb, 0xaf, DINADF, DINADF};
1743         const u8 regs[] = {0, 0};
1744         /* reset fifo count to zero */
1745         result = mem_w_key(KEY_D_1_178, ARRAY_SIZE(regs), regs);
1746         if (result)
1747                 return result;
1748
1749         if (on)
1750                 /*Sets the DMP to send an interrupt and put a FIFO packet
1751                 in the FIFO if and only if a tap/orientation event
1752                 just occurred*/
1753                 result = mem_w_key(KEY_CFG_FIFO_ON_EVENT, ARRAY_SIZE(regs_on),
1754                                         regs_on);
1755         else
1756                 /*Sets the DMP to send an interrupt and put a FIFO packet
1757                 in the FIFO at the rate specified by the FIFO div.
1758                 see inv_set_fifo_div in hw_setup.c to set the FIFO div.*/
1759                 result = mem_w_key(KEY_CFG_FIFO_ON_EVENT, ARRAY_SIZE(regs_off),
1760                                         regs_off);
1761         if (result)
1762                 return result;
1763
1764         result = mem_w_key(KEY_CFG_6, ARRAY_SIZE(regs_end), regs_end);
1765
1766         return result;
1767 }
1768
1769 /**
1770  * inv_enable_tap_dmp() -  calling this function will enable/disable tap function.
1771  */
1772 int inv_enable_tap_dmp(struct inv_mpu_iio_s *st, bool on)
1773 {
1774         int result;
1775
1776         result = inv_set_tap_interrupt_dmp(st, on);
1777         if (result)
1778                 return result;
1779         if (on) {
1780                 result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_X,
1781                                                    st->tap.thresh);
1782                 if (result)
1783                         return result;
1784                 result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_Y,
1785                                                    st->tap.thresh);
1786                 if (result)
1787                         return result;
1788                 result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_Z,
1789                                                    st->tap.thresh);
1790                 if (result)
1791                         return result;
1792         }
1793
1794         result = inv_set_tap_axes_dmp(st, INV_TAP_ALL_DIRECTIONS);
1795         if (result)
1796                 return result;
1797         result = inv_set_min_taps_dmp(st, st->tap.min_count);
1798         if (result)
1799                 return result;
1800
1801         result = inv_set_tap_time_dmp(st, st->tap.time);
1802         if (result)
1803                 return result;
1804
1805         result = inv_set_multiple_tap_time_dmp(st, DMP_MULTI_TAP_TIME);
1806         if (result)
1807                 return result;
1808
1809         result = inv_set_gyro_sf_dmp(st);
1810         if (result)
1811                 return result;
1812
1813         result = inv_set_shake_reject_thresh_dmp(st, DMP_SHAKE_REJECT_THRESH);
1814         if (result)
1815                 return result;
1816
1817         result = inv_set_shake_reject_time_dmp(st, DMP_SHAKE_REJECT_TIME);
1818         if (result)
1819                 return result;
1820
1821         result = inv_set_shake_reject_timeout_dmp(st,
1822                                                   DMP_SHAKE_REJECT_TIMEOUT);
1823         return result;
1824 }
1825
1826 int inv_send_sensor_data(struct inv_mpu_iio_s *st, u16 elements)
1827 {
1828         int result;
1829         u8 regs[] = {DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1830                                 DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1831                                 DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1832                                 DINAA0 + 3};
1833
1834         if (elements & INV_ELEMENT_1)
1835                 regs[0] = DINACA;
1836         if (elements & INV_ELEMENT_2)
1837                 regs[4] = DINBC4;
1838         if (elements & INV_ELEMENT_3)
1839                 regs[5] = DINACC;
1840         if (elements & INV_ELEMENT_4)
1841                 regs[6] = DINBC6;
1842         if ((elements & INV_ELEMENT_5) || (elements & INV_ELEMENT_6) ||
1843             (elements & INV_ELEMENT_7)) {
1844                 regs[1] = DINBC0;
1845                 regs[2] = DINAC8;
1846                 regs[3] = DINBC2;
1847         }
1848         result = mem_w_key(KEY_CFG_15, ARRAY_SIZE(regs), regs);
1849         return result;
1850 }
1851
1852 int inv_send_interrupt_word(struct inv_mpu_iio_s *st, bool on)
1853 {
1854         const u8 regs_on[] = { DINA20 };
1855         const u8 regs_off[] = { DINAA3 };
1856         u8 result;
1857
1858         if (on)
1859                 result = mem_w_key(KEY_CFG_27, ARRAY_SIZE(regs_on), regs_on);
1860         else
1861                 result = mem_w_key(KEY_CFG_27, ARRAY_SIZE(regs_off), regs_off);
1862
1863         return result;
1864 }
1865
1866 /**
1867  * inv_dmp_firmware_write() -  calling this function will load the firmware.
1868  *                        This is the write function of file "dmp_firmware".
1869  */
1870 ssize_t inv_dmp_firmware_write(struct file *fp, struct kobject *kobj,
1871         struct bin_attribute *attr,
1872         char *buf, loff_t pos, size_t size)
1873 {
1874         u8 *firmware;
1875         int result;
1876         struct inv_reg_map_s *reg;
1877         struct iio_dev *indio_dev;
1878         struct inv_mpu_iio_s *st;
1879
1880         indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj));
1881         st = iio_priv(indio_dev);
1882
1883         if (st->chip_config.firmware_loaded)
1884                 return -EINVAL;
1885         if (st->chip_config.enable)
1886                 return -EBUSY;
1887
1888         reg = &st->reg;
1889         if (DMP_IMAGE_SIZE != size) {
1890                 pr_err("wrong DMP image size\n");
1891                 return -EINVAL;
1892         }
1893
1894         firmware = kmalloc(size, GFP_KERNEL);
1895         if (!firmware)
1896                 return -ENOMEM;
1897
1898         mutex_lock(&indio_dev->mlock);
1899
1900         memcpy(firmware, buf, size);
1901         result = crc32(CRC_FIRMWARE_SEED, firmware, size);
1902         if (DMP_IMAGE_CRC_VALUE != result) {
1903                 pr_err("firmware CRC error - 0x%08x vs 0x%08x\n",
1904                         result, DMP_IMAGE_CRC_VALUE);
1905                 result = -EINVAL;
1906                 goto firmware_write_fail;
1907         }
1908
1909         result = st->set_power_state(st, true);
1910         if (result)
1911                 goto firmware_write_fail;
1912
1913         result = inv_load_firmware(st, firmware, size);
1914         if (result)
1915                 goto firmware_write_fail;
1916
1917         result = inv_verify_firmware(st, firmware, size);
1918         if (result)
1919                 goto firmware_write_fail;
1920
1921         result = inv_plat_single_write(st, reg->prgm_strt_addrh,
1922         st->chip_config.prog_start_addr >> 8);
1923         if (result)
1924                 goto firmware_write_fail;
1925         result = inv_plat_single_write(st, reg->prgm_strt_addrh + 1,
1926         st->chip_config.prog_start_addr & 0xff);
1927         if (result)
1928                 goto firmware_write_fail;
1929
1930         result = inv_set_fifo_rate(st, DMP_DEFAULT_FIFO_RATE);
1931         if (result)
1932                 goto firmware_write_fail;
1933         result = inv_gyro_dmp_cal(st);
1934         if (result)
1935                 goto firmware_write_fail;
1936         result = inv_accel_dmp_cal(st);
1937         if (result)
1938                 goto firmware_write_fail;
1939         /* result = inv_disable_gyro_cal(st);*/
1940         if (result)
1941                 goto firmware_write_fail;
1942
1943         st->chip_config.firmware_loaded = 1;
1944
1945 firmware_write_fail:
1946         result |= st->set_power_state(st, false);
1947         mutex_unlock(&indio_dev->mlock);
1948         kfree(firmware);
1949         if (result)
1950                 return result;
1951         return size;
1952 }
1953
1954 ssize_t inv_dmp_firmware_read(struct file *filp,
1955                                 struct kobject *kobj,
1956                                 struct bin_attribute *bin_attr,
1957                                 char *buf, loff_t off, size_t count)
1958 {
1959         int bank, write_size, size, data, result;
1960         u16 memaddr;
1961         struct iio_dev *indio_dev;
1962         struct inv_mpu_iio_s *st;
1963
1964         size = count;
1965         indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj));
1966         st = iio_priv(indio_dev);
1967
1968         data = 0;
1969         mutex_lock(&indio_dev->mlock);
1970         if (!st->chip_config.enable) {
1971                 result = st->set_power_state(st, true);
1972                 if (result) {
1973                         mutex_unlock(&indio_dev->mlock);
1974                         return result;
1975                 }
1976         }
1977         for (bank = 0; size > 0; bank++, size -= write_size,
1978                                         data += write_size) {
1979                 if (size > MPU_MEM_BANK_SIZE)
1980                         write_size = MPU_MEM_BANK_SIZE;
1981                 else
1982                         write_size = size;
1983
1984                 memaddr = (bank << 8);
1985                 result = mpu_memory_read(st,
1986                         st->i2c_addr, memaddr, write_size, &buf[data]);
1987                 if (result) {
1988                         mutex_unlock(&indio_dev->mlock);
1989                         return result;
1990                 }
1991         }
1992         if (!st->chip_config.enable)
1993                 result = st->set_power_state(st, false);
1994         mutex_unlock(&indio_dev->mlock);
1995         if (result)
1996                 return result;
1997
1998         return count;
1999 }
2000 /**
2001  *  @}
2002  */