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(struct inv_mpu_iio_s *st, u8 mpu_addr, u16 mem_addr,
 297                      u32 len, u8 const *data)
 298 {
 299         u8 bank[2];
 300         u8 addr[2];
 301         u8 buf[513];
 302
 303         struct i2c_msg msg;
 304         int res;
 305
 306         if (!data || !st)
 307                 return -EINVAL;
 308
 309         if (len >= (sizeof(buf) - 1))
 310                 return -ENOMEM;
 311
 312         bank[0] = REG_BANK_SEL;
 313         bank[1] = mem_addr >> 8;
 314
 315         addr[0] = REG_MEM_START_ADDR;
 316         addr[1] = mem_addr & 0xFF;
 317
 318         buf[0] = REG_MEM_RW;
 319         memcpy(buf + 1, data, len);
 320
 321         /* write message */
 322         msg.addr = mpu_addr;
 323         msg.flags = 0;
 324         msg.buf = bank;
 325         msg.len = sizeof(bank);
 326         /* msg.scl_rate = 200*1000; */
 327         INV_I2C_INC_MPUWRITE(3);
 328         res = i2c_transfer(st->sl_handle, &msg, 1);
 329         if (res < 1) {
 330                 if (res >= 0)
 331                         res = -EIO;
 332                 return res;
 333         }
 334
 335         msg.addr = mpu_addr;
 336         msg.flags = 0;
 337         msg.buf = addr;
 338         msg.len = sizeof(addr);
 339         /* msg.scl_rate = 200*1000; */
 340         INV_I2C_INC_MPUWRITE(3);
 341         res = i2c_transfer(st->sl_handle, &msg, 1);
 342         if (res < 1) {
 343                 if (res >= 0)
 344                         res = -EIO;
 345                 return res;
 346         }
 347
 348         msg.addr = mpu_addr;
 349         msg.flags = 0;
 350         msg.buf = (u8 *)buf;
 351         msg.len = len + 1;
 352         /* msg.scl_rate = 200*1000; */
 353         INV_I2C_INC_MPUWRITE(2+len);
 354         res = i2c_transfer(st->sl_handle, &msg, 1);
 355         if (res < 1) {
 356                 if (res >= 0)
 357                         res = -EIO;
 358                 return res;
 359         }
 360
 361         {
 362                 char *write = 0;
 363                 pr_debug("%s WM%02X%02X%02X%s%s - %d\n", st->hw->name,
 364                          mpu_addr, bank[1], addr[1],
 365                          wr_pr_debug_begin(data, len, write),
 366                          wr_pr_debug_end(write),
 367                          len);
 368         }
 369         return 0;
 370 }
 371
 372 int mpu_memory_read(struct inv_mpu_iio_s *st, u8 mpu_addr, u16 mem_addr,
 373                     u32 len, u8 *data)
 374 {
 375         u8 bank[2];
 376         u8 addr[2];
 377         u8 buf;
 378
 379         struct i2c_msg msg;
 380         int res;
 381
 382         if (!data || !st)
 383                 return -EINVAL;
 384
 385         bank[0] = REG_BANK_SEL;
 386         bank[1] = mem_addr >> 8;
 387
 388         addr[0] = REG_MEM_START_ADDR;
 389         addr[1] = mem_addr & 0xFF;
 390
 391         buf = REG_MEM_RW;
 392
 393         /* write message */
 394         msg.addr = mpu_addr;
 395         msg.flags = 0;
 396         msg.buf = bank;
 397         msg.len = sizeof(bank);
 398         /* msg.scl_rate = 200*1000; */
 399         INV_I2C_INC_MPUWRITE(3);
 400         res = i2c_transfer(st->sl_handle, &msg, 1);
 401         if (res < 1) {
 402                 if (res >= 0)
 403                         res = -EIO;
 404                 return res;
 405         }
 406
 407         msg.addr = mpu_addr;
 408         msg.flags = 0;
 409         msg.buf = addr;
 410         msg.len = sizeof(addr);
 411         /* msg.scl_rate = 200*1000; */
 412         INV_I2C_INC_MPUWRITE(3);
 413         res = i2c_transfer(st->sl_handle, &msg, 1);
 414         if (res < 1) {
 415                 if (res >= 0)
 416                         res = -EIO;
 417                 return res;
 418         }
 419
 420         msg.addr = mpu_addr;
 421         msg.flags = 0;
 422         msg.buf = &buf;
 423         msg.len = 1;
 424         /* msg.scl_rate = 200*1000; */
 425         INV_I2C_INC_MPUWRITE(3);
 426         res = i2c_transfer(st->sl_handle, &msg, 1);
 427         if (res < 1) {
 428                 if (res >= 0)
 429                         res = -EIO;
 430                 return res;
 431         }
 432
 433         msg.addr = mpu_addr;
 434         msg.flags = I2C_M_RD;
 435         msg.buf = data;
 436         msg.len = len;
 437         /* msg.scl_rate = 200*1000; */
 438         INV_I2C_INC_MPUREAD(len);
 439         res = i2c_transfer(st->sl_handle, &msg, 1);
 440         if (res < 1) {
 441                 if (res >= 0)
 442                         res = -EIO;
 443                 return res;
 444         }
 445         {
 446                 char *read = 0;
 447                 pr_debug("%s RM%02X%02X%02X%02X - %s%s\n", st->hw->name,
 448                          mpu_addr, bank[1], addr[1], len,
 449                          wr_pr_debug_begin(data, len, read),
 450                          wr_pr_debug_end(read));
 451         }
 452
 453         return 0;
 454 }
 455
 456 int mpu_memory_write_unaligned(struct inv_mpu_iio_s *st, u16 key, int len,
 457                                                                 u8 const *d)
 458 {
 459         u32 addr;
 460         int start, end;
 461         int len1, len2;
 462         int result = 0;
 463         if (len > MPU_MEM_BANK_SIZE)
 464                 return -EINVAL;
 465         addr = inv_dmp_get_address(key);
 466         if (addr > MPU6XXX_MAX_MPU_MEM)
 467                 return -EINVAL;
 468         start = (addr >> 8);
 469         end   = ((addr + len - 1) >> 8);
 470         if (start == end) {
 471                 result = mpu_memory_write(st, st->i2c_addr, addr, len, d);
 472         } else {
 473                 end <<= 8;
 474                 len1 = end - addr;
 475                 len2 = len - len1;
 476                 result = mpu_memory_write(st, st->i2c_addr, addr, len1, d);
 477                 result |= mpu_memory_write(st, st->i2c_addr, end, len2,
 478                                                                 d + len1);
 479         }
 480
 481         return result;
 482 }
 483
 484 /**
 485  *  index_of_key()- Inverse lookup of the index of an MPL product key .
 486  *  @key: the MPL product indentifier also referred to as 'key'.
 487  */
 488 static short index_of_key(u16 key)
 489 {
 490         int i;
 491         for (i = 0; i < NUM_OF_PROD_REVS; i++)
 492                 if (prod_rev_map[i].mpl_product_key == key)
 493                         return (short)i;
 494         return -EINVAL;
 495 }
 496
 497 int inv_get_silicon_rev_mpu6500(struct inv_mpu_iio_s *st)
 498 {
 499         struct inv_chip_info_s *chip_info = &st->chip_info;
 500         int result;
 501         u8 whoami, sw_rev;
 502
 503         result = inv_i2c_read(st, REG_WHOAMI, 1, &whoami);
 504         if (result)
 505                 return result;
 506         if (whoami != MPU6500_ID && whoami != MPU9250_ID &&
 507                         whoami != MPU6515_ID)
 508                 return -EINVAL;
 509
 510         /*memory read need more time after power up */
 511         msleep(POWER_UP_TIME);
 512         result = mpu_memory_read(st, st->i2c_addr,
 513                         MPU6500_MEM_REV_ADDR, 1, &sw_rev);
 514         sw_rev &= INV_MPU_REV_MASK;
 515         if (result)
 516                 return result;
 517         if (sw_rev != 0)
 518                 return -EINVAL;
 519
 520         /* these values are place holders and not real values */
 521         chip_info->product_id = MPU6500_PRODUCT_REVISION;
 522         chip_info->product_revision = MPU6500_PRODUCT_REVISION;
 523         chip_info->silicon_revision = MPU6500_PRODUCT_REVISION;
 524         chip_info->software_revision = sw_rev;
 525         chip_info->gyro_sens_trim = DEFAULT_GYRO_TRIM;
 526         chip_info->accl_sens_trim = DEFAULT_ACCL_TRIM;
 527         chip_info->multi = 1;
 528
 529         return 0;
 530 }
 531
 532 int inv_get_silicon_rev_mpu6050(struct inv_mpu_iio_s *st)
 533 {
 534         int result;
 535         struct inv_reg_map_s *reg;
 536         u8 prod_ver = 0x00, prod_rev = 0x00;
 537         struct prod_rev_map_t *p_rev;
 538         u8 bank =
 539             (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
 540         u16 mem_addr = ((bank << 8) | MEM_ADDR_PROD_REV);
 541         u16 key;
 542         u8 regs[5];
 543         u16 sw_rev;
 544         short index;
 545         struct inv_chip_info_s *chip_info = &st->chip_info;
 546         reg = &st->reg;
 547
 548         result = inv_i2c_read(st, REG_PRODUCT_ID, 1, &prod_ver);
 549         if (result)
 550                 return result;
 551         prod_ver &= 0xf;
 552         /*memory read need more time after power up */
 553         msleep(POWER_UP_TIME);
 554         result = mpu_memory_read(st, st->i2c_addr, mem_addr,
 555                         1, &prod_rev);
 556         if (result)
 557                 return result;
 558         prod_rev >>= 2;
 559         /* clean the prefetch and cfg user bank bits */
 560         result = inv_i2c_single_write(st, reg->bank_sel, 0);
 561         if (result)
 562                 return result;
 563         /* get the software-product version, read from XA_OFFS_L */
 564         result = inv_i2c_read(st, REG_XA_OFFS_L_TC,
 565                                 SOFT_PROD_VER_BYTES, regs);
 566         if (result)
 567                 return result;
 568
 569         sw_rev = (regs[4] & 0x01) << 2 |        /* 0x0b, bit 0 */
 570                  (regs[2] & 0x01) << 1 |        /* 0x09, bit 0 */
 571                  (regs[0] & 0x01);              /* 0x07, bit 0 */
 572         /* if 0, use the product key to determine the type of part */
 573         if (sw_rev == 0) {
 574                 key = MPL_PROD_KEY(prod_ver, prod_rev);
 575                 if (key == 0)
 576                         return -EINVAL;
 577                 index = index_of_key(key);
 578                 if (index < 0 || index >= NUM_OF_PROD_REVS)
 579                         return -EINVAL;
 580                 /* check MPL is compiled for this device */
 581                 if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1)
 582                         return -EINVAL;
 583                 p_rev = (struct prod_rev_map_t *)&prod_rev_map[index];
 584         /* if valid, use the software product key */
 585         } else if (sw_rev < ARRAY_SIZE(sw_rev_map)) {
 586                 p_rev = (struct prod_rev_map_t *)&sw_rev_map[sw_rev];
 587         } else {
 588                 return -EINVAL;
 589         }
 590         chip_info->product_id = prod_ver;
 591         chip_info->product_revision = prod_rev;
 592         chip_info->silicon_revision = p_rev->silicon_rev;
 593         chip_info->software_revision = sw_rev;
 594         chip_info->gyro_sens_trim = p_rev->gyro_trim;
 595         chip_info->accl_sens_trim = p_rev->accel_trim;
 596         if (chip_info->accl_sens_trim == 0)
 597                 chip_info->accl_sens_trim = DEFAULT_ACCL_TRIM;
 598         chip_info->multi = DEFAULT_ACCL_TRIM / chip_info->accl_sens_trim;
 599         if (chip_info->multi != 1)
 600                 pr_info("multi is %d\n", chip_info->multi);
 601         return result;
 602 }
 603
 604 /**
 605  *  read_accel_hw_self_test_prod_shift()- read the accelerometer hardware
 606  *                                         self-test bias shift calculated
 607  *                                         during final production test and
 608  *                                         stored in chip non-volatile memory.
 609  *  @st:  main data structure.
 610  *  @st_prod:   A pointer to an array of 3 elements to hold the values
 611  *              for production hardware self-test bias shifts returned to the
 612  *              user.
 613  *  @accl_sens: accel sensitivity.
 614  */
 615 static int read_accel_hw_self_test_prod_shift(struct inv_mpu_iio_s *st,
 616                                         int *st_prod, int *accl_sens)
 617 {
 618         u8 regs[4];
 619         u8 shift_code[3];
 620         int result, i;
 621
 622         for (i = 0; i < 3; i++)
 623                 st_prod[i] = 0;
 624
 625         result = inv_i2c_read(st, REG_ST_GCT_X, ARRAY_SIZE(regs), regs);
 626
 627         if (result)
 628                 return result;
 629         if ((0 == regs[0])  && (0 == regs[1]) &&
 630             (0 == regs[2]) && (0 == regs[3]))
 631                 return -EINVAL;
 632         shift_code[X] = ((regs[0] & 0xE0) >> 3) | ((regs[3] & 0x30) >> 4);
 633         shift_code[Y] = ((regs[1] & 0xE0) >> 3) | ((regs[3] & 0x0C) >> 2);
 634         shift_code[Z] = ((regs[2] & 0xE0) >> 3) |  (regs[3] & 0x03);
 635         for (i = 0; i < 3; i++) {
 636                 if (shift_code[i] != 0)
 637                         st_prod[i] = accl_sens[i] *
 638                                 accl_st_tb[shift_code[i] - 1];
 639         }
 640
 641         return 0;
 642 }
 643
 644 /**
 645 * inv_check_accl_self_test()- check accel self test. this function returns
 646 *                              zero as success. A non-zero return value
 647 *                              indicates failure in self test.
 648 *  @*st: main data structure.
 649 *  @*reg_avg: average value of normal test.
 650 *  @*st_avg:  average value of self test
 651 */
 652 static int inv_check_accl_self_test(struct inv_mpu_iio_s *st,
 653         int *reg_avg, int *st_avg){
 654         int gravity, reg_z_avg, g_z_sign, j, ret_val;
 655         int tmp;
 656         int st_shift_prod[THREE_AXIS], st_shift_cust[THREE_AXIS];
 657         int st_shift_ratio[THREE_AXIS];
 658         int accel_sens[THREE_AXIS];
 659
 660         if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
 661             st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
 662                 return 0;
 663         g_z_sign = 1;
 664         ret_val = 0;
 665         tmp = DEF_ST_SCALE * DEF_ST_PRECISION / DEF_ST_ACCEL_FS_MG;
 666         for (j = 0; j < 3; j++)
 667                 accel_sens[j] = tmp;
 668
 669         if (MPL_PROD_KEY(st->chip_info.product_id,
 670                          st->chip_info.product_revision) ==
 671             MPU_PRODUCT_KEY_B1_E1_5) {
 672                 /* half sensitivity Z accelerometer parts */
 673                 accel_sens[Z] /= 2;
 674         } else {
 675                 /* half sensitivity X, Y, Z accelerometer parts */
 676                 accel_sens[X] /= st->chip_info.multi;
 677                 accel_sens[Y] /= st->chip_info.multi;
 678                 accel_sens[Z] /= st->chip_info.multi;
 679         }
 680         gravity = accel_sens[Z];
 681         reg_z_avg = reg_avg[Z] - g_z_sign * gravity * DEF_ST_PRECISION;
 682         ret_val = read_accel_hw_self_test_prod_shift(st, st_shift_prod,
 683                                                         accel_sens);
 684         if (ret_val)
 685                 return ret_val;
 686
 687         for (j = 0; j < 3; j++) {
 688                 st_shift_cust[j] = abs(reg_avg[j] - st_avg[j]);
 689                 if (st_shift_prod[j]) {
 690                         tmp = st_shift_prod[j] / DEF_ST_PRECISION;
 691                         st_shift_ratio[j] = abs(st_shift_cust[j] / tmp
 692                                 - DEF_ST_PRECISION);
 693                         if (st_shift_ratio[j] > DEF_ACCEL_ST_SHIFT_DELTA)
 694                                 ret_val = 1;
 695                 } else {
 696                         if (st_shift_cust[j] <
 697                                 DEF_ACCEL_ST_SHIFT_MIN * gravity)
 698                                 ret_val = 1;
 699                         if (st_shift_cust[j] >
 700                                 DEF_ACCEL_ST_SHIFT_MAX * gravity)
 701                                 ret_val = 1;
 702                 }
 703         }
 704
 705         return ret_val;
 706 }
 707
 708 /**
 709 * inv_check_3500_gyro_self_test() check gyro self test. this function returns
 710 *                                 zero as success. A non-zero return value
 711 *                                 indicates failure in self test.
 712 *  @*st: main data structure.
 713 *  @*reg_avg: average value of normal test.
 714 *  @*st_avg:  average value of self test
 715 */
 716
 717 static int inv_check_3500_gyro_self_test(struct inv_mpu_iio_s *st,
 718         int *reg_avg, int *st_avg){
 719         int result;
 720         int gst[3], ret_val;
 721         int gst_otp[3], i;
 722         u8 st_code[THREE_AXIS];
 723         ret_val = 0;
 724
 725         for (i = 0; i < 3; i++)
 726                 gst[i] = st_avg[i] - reg_avg[i];
 727         result = inv_i2c_read(st, REG_3500_OTP, THREE_AXIS, st_code);
 728         if (result)
 729                 return result;
 730         gst_otp[0] = 0;
 731         gst_otp[1] = 0;
 732         gst_otp[2] = 0;
 733         for (i = 0; i < 3; i++) {
 734                 if (st_code[i] != 0)
 735                         gst_otp[i] = gyro_3500_st_tb[st_code[i] - 1];
 736         }
 737         /* check self test value passing criterion. Using the DEF_ST_TOR
 738          * for certain degree of tolerance */
 739         for (i = 0; i < 3; i++) {
 740                 if (gst_otp[i] == 0) {
 741                         if (abs(gst[i]) * DEF_ST_TOR < DEF_ST_OTP0_THRESH *
 742                                                         DEF_ST_PRECISION *
 743                                                         DEF_GYRO_SCALE)
 744                                 ret_val |= (1 << i);
 745                 } else {
 746                         if (abs(gst[i]/gst_otp[i] - DEF_ST_PRECISION) >
 747                                         DEF_GYRO_CT_SHIFT_DELTA)
 748                                 ret_val |= (1 << i);
 749                 }
 750         }
 751         /* check for absolute value passing criterion. Using DEF_ST_TOR
 752          * for certain degree of tolerance */
 753         for (i = 0; i < 3; i++) {
 754                 if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH *
 755                     DEF_ST_PRECISION * DEF_GYRO_SCALE)
 756                         ret_val |= (1 << i);
 757         }
 758
 759         return ret_val;
 760 }
 761
 762 /**
 763 * inv_check_6050_gyro_self_test() - check 6050 gyro self test. this function
 764 *                                   returns zero as success. A non-zero return
 765 *                                   value indicates failure in self test.
 766 *  @*st: main data structure.
 767 *  @*reg_avg: average value of normal test.
 768 *  @*st_avg:  average value of self test
 769 */
 770 static int inv_check_6050_gyro_self_test(struct inv_mpu_iio_s *st,
 771         int *reg_avg, int *st_avg){
 772         int result;
 773         int ret_val;
 774         int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
 775         u8 regs[3];
 776
 777         if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
 778             st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
 779                 return 0;
 780
 781         ret_val = 0;
 782         result = inv_i2c_read(st, REG_ST_GCT_X, 3, regs);
 783         if (result)
 784                 return result;
 785         regs[X] &= 0x1f;
 786         regs[Y] &= 0x1f;
 787         regs[Z] &= 0x1f;
 788         for (i = 0; i < 3; i++) {
 789                 if (regs[i] != 0)
 790                         st_shift_prod[i] = gyro_6050_st_tb[regs[i] - 1];
 791                 else
 792                         st_shift_prod[i] = 0;
 793         }
 794         st_shift_prod[1] = -st_shift_prod[1];
 795
 796         for (i = 0; i < 3; i++) {
 797                 st_shift_cust[i] =  st_avg[i] - reg_avg[i];
 798                 if (st_shift_prod[i]) {
 799                         st_shift_ratio[i] = abs(st_shift_cust[i] /
 800                                 st_shift_prod[i] - DEF_ST_PRECISION);
 801                         if (st_shift_ratio[i] > DEF_GYRO_CT_SHIFT_DELTA)
 802                                 ret_val = 1;
 803                 } else {
 804                         if (st_shift_cust[i] < DEF_ST_PRECISION *
 805                                 DEF_GYRO_CT_SHIFT_MIN * DEF_SELFTEST_GYRO_SENS)
 806                                 ret_val = 1;
 807                         if (st_shift_cust[i] > DEF_ST_PRECISION *
 808                                 DEF_GYRO_CT_SHIFT_MAX * DEF_SELFTEST_GYRO_SENS)
 809                                 ret_val = 1;
 810                 }
 811         }
 812         /* check for absolute value passing criterion. Using DEF_ST_TOR
 813          * for certain degree of tolerance */
 814         for (i = 0; i < 3; i++)
 815                 if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH *
 816                     DEF_ST_PRECISION * DEF_GYRO_SCALE)
 817                         ret_val = 1;
 818
 819         return ret_val;
 820 }
 821
 822 /**
 823 * inv_check_6500_gyro_self_test() - check 6500 gyro self test. this function
 824 *                                   returns zero as success. A non-zero return
 825 *                                   value indicates failure in self test.
 826 *  @*st: main data structure.
 827 *  @*reg_avg: average value of normal test.
 828 *  @*st_avg:  average value of self test
 829 */
 830 static int inv_check_6500_gyro_self_test(struct inv_mpu_iio_s *st,
 831         int *reg_avg, int *st_avg)
 832 {
 833         u8 regs[3];
 834         int ret_val, result;
 835         int otp_value_zero = 0;
 836         int st_shift_prod[3], st_shift_cust[3], i;
 837
 838         ret_val = 0;
 839         result = inv_i2c_read(st, REG_6500_XG_ST_DATA, 3, regs);
 840         if (result)
 841                 return result;
 842         pr_debug("%s self_test gyro shift_code - %02x %02x %02x\n",
 843                  st->hw->name, regs[0], regs[1], regs[2]);
 844
 845         for (i = 0; i < 3; i++) {
 846                 if (regs[i] != 0) {
 847                         st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
 848                 } else {
 849                         st_shift_prod[i] = 0;
 850                         otp_value_zero = 1;
 851                 }
 852         }
 853         pr_debug("%s self_test gyro st_shift_prod - %+d %+d %+d\n",
 854                  st->hw->name, st_shift_prod[0], st_shift_prod[1],
 855                  st_shift_prod[2]);
 856
 857         for (i = 0; i < 3; i++) {
 858                 st_shift_cust[i] = st_avg[i] - reg_avg[i];
 859                 if (!otp_value_zero) {
 860                         /* Self Test Pass/Fail Criteria A */
 861                         if (st_shift_cust[i] < DEF_6500_GYRO_CT_SHIFT_DELTA
 862                                                 * st_shift_prod[i])
 863                                         ret_val = 1;
 864                 } else {
 865                         /* Self Test Pass/Fail Criteria B */
 866                         if (st_shift_cust[i] < DEF_GYRO_ST_AL *
 867                                                 DEF_SELFTEST_GYRO_SENS *
 868                                                 DEF_ST_PRECISION)
 869                                 ret_val = 1;
 870                 }
 871         }
 872         pr_debug("%s self_test gyro st_shift_cust - %+d %+d %+d\n",
 873                  st->hw->name, st_shift_cust[0], st_shift_cust[1],
 874                  st_shift_cust[2]);
 875
 876         if (ret_val == 0) {
 877                 /* Self Test Pass/Fail Criteria C */
 878                 for (i = 0; i < 3; i++)
 879                         if (abs(reg_avg[i]) > DEF_GYRO_OFFSET_MAX *
 880                                                 DEF_SELFTEST_GYRO_SENS *
 881                                                 DEF_ST_PRECISION)
 882                                 ret_val = 1;
 883         }
 884
 885         return ret_val;
 886 }
 887
 888 /**
 889 * inv_check_6500_accel_self_test() - check 6500 accel self test. this function
 890 *                                   returns zero as success. A non-zero return
 891 *                                   value indicates failure in self test.
 892 *  @*st: main data structure.
 893 *  @*reg_avg: average value of normal test.
 894 *  @*st_avg:  average value of self test
 895 */
 896 static int inv_check_6500_accel_self_test(struct inv_mpu_iio_s *st,
 897                                                 int *reg_avg, int *st_avg) {
 898         int ret_val, result;
 899         int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
 900         u8 regs[3];
 901         int otp_value_zero = 0;
 902
 903 #define ACCEL_ST_AL_MIN ((DEF_ACCEL_ST_AL_MIN * DEF_ST_SCALE \
 904                                  / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
 905 #define ACCEL_ST_AL_MAX ((DEF_ACCEL_ST_AL_MAX * DEF_ST_SCALE \
 906                                  / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
 907
 908         ret_val = 0;
 909         result = inv_i2c_read(st, REG_6500_XA_ST_DATA, 3, regs);
 910         if (result)
 911                 return result;
 912         pr_debug("%s self_test accel shift_code - %02x %02x %02x\n",
 913                  st->hw->name, regs[0], regs[1], regs[2]);
 914
 915         for (i = 0; i < 3; i++) {
 916                 if (regs[i] != 0) {
 917                         st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
 918                 } else {
 919                         st_shift_prod[i] = 0;
 920                         otp_value_zero = 1;
 921                 }
 922         }
 923         pr_debug("%s self_test accel st_shift_prod - %+d %+d %+d\n",
 924                  st->hw->name, st_shift_prod[0], st_shift_prod[1],
 925                  st_shift_prod[2]);
 926
 927         if (!otp_value_zero) {
 928                 /* Self Test Pass/Fail Criteria A */
 929                 for (i = 0; i < 3; i++) {
 930                         st_shift_cust[i] = st_avg[i] - reg_avg[i];
 931                         st_shift_ratio[i] = abs(st_shift_cust[i] /
 932                                         st_shift_prod[i] - DEF_ST_PRECISION);
 933                         if (st_shift_ratio[i] > DEF_6500_ACCEL_ST_SHIFT_DELTA)
 934                                 ret_val = 1;
 935                 }
 936         } else {
 937                 /* Self Test Pass/Fail Criteria B */
 938                 for (i = 0; i < 3; i++) {
 939                         st_shift_cust[i] = abs(st_avg[i] - reg_avg[i]);
 940                         if (st_shift_cust[i] < ACCEL_ST_AL_MIN ||
 941                                         st_shift_cust[i] > ACCEL_ST_AL_MAX)
 942                                 ret_val = 1;
 943                 }
 944         }
 945         pr_debug("%s self_test accel st_shift_cust - %+d %+d %+d\n",
 946                  st->hw->name, st_shift_cust[0], st_shift_cust[1],
 947                  st_shift_cust[2]);
 948
 949         return ret_val;
 950 }
 951
 952 /*
 953  *  inv_do_test() - do the actual test of self testing
 954  */
 955 int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
 956                 int *gyro_result, int *accl_result)
 957 {
 958         struct inv_reg_map_s *reg;
 959         int result, i, j, packet_size;
 960         u8 data[BYTES_PER_SENSOR * 2], d;
 961         bool has_accl;
 962         int fifo_count, packet_count, ind, s;
 963
 964         reg = &st->reg;
 965         has_accl = (st->chip_type != INV_ITG3500);
 966         if (has_accl)
 967                 packet_size = BYTES_PER_SENSOR * 2;
 968         else
 969                 packet_size = BYTES_PER_SENSOR;
 970
 971         result = inv_i2c_single_write(st, reg->int_enable, 0);
 972         if (result)
 973                 return result;
 974         /* disable the sensor output to FIFO */
 975         result = inv_i2c_single_write(st, reg->fifo_en, 0);
 976         if (result)
 977                 return result;
 978         /* disable fifo reading */
 979         result = inv_i2c_single_write(st, reg->user_ctrl, 0);
 980         if (result)
 981                 return result;
 982         /* clear FIFO */
 983         result = inv_i2c_single_write(st, reg->user_ctrl, BIT_FIFO_RST);
 984         if (result)
 985                 return result;
 986         /* setup parameters */
 987         result = inv_i2c_single_write(st, reg->lpf, INV_FILTER_98HZ);
 988         if (result)
 989                 return result;
 990
 991         if (INV_MPU6500 == st->chip_type) {
 992                 /* config accel LPF register for MPU6500 */
 993                 result = inv_i2c_single_write(st, REG_6500_ACCEL_CONFIG2,
 994                                                 DEF_ST_MPU6500_ACCEL_LPF |
 995                                                 BIT_FIFO_SIZE_1K);
 996                 if (result)
 997                         return result;
 998         }
 999
1000         result = inv_i2c_single_write(st, reg->sample_rate_div,
1001                         DEF_SELFTEST_SAMPLE_RATE);
1002         if (result)
1003                 return result;
1004         /* wait for the sampling rate change to stabilize */
1005         mdelay(INV_MPU_SAMPLE_RATE_CHANGE_STABLE);
1006         result = inv_i2c_single_write(st, reg->gyro_config,
1007                 self_test_flag | DEF_SELFTEST_GYRO_FS);
1008         if (result)
1009                 return result;
1010         if (has_accl) {
1011                 if (INV_MPU6500 == st->chip_type)
1012                         d = DEF_SELFTEST_6500_ACCEL_FS;
1013                 else
1014                         d = DEF_SELFTEST_ACCEL_FS;
1015                 d |= self_test_flag;
1016                 result = inv_i2c_single_write(st, reg->accl_config, d);
1017                 if (result)
1018                         return result;
1019         }
1020         /* wait for the output to get stable */
1021         if (self_test_flag) {
1022                 if (INV_MPU6500 == st->chip_type)
1023                         msleep(DEF_ST_6500_STABLE_TIME);
1024                 else
1025                         msleep(DEF_ST_STABLE_TIME);
1026         }
1027
1028         /* enable FIFO reading */
1029         result = inv_i2c_single_write(st, reg->user_ctrl, BIT_FIFO_EN);
1030         if (result)
1031                 return result;
1032         /* enable sensor output to FIFO */
1033         if (has_accl)
1034                 d = BITS_GYRO_OUT | BIT_ACCEL_OUT;
1035         else
1036                 d = BITS_GYRO_OUT;
1037         for (i = 0; i < THREE_AXIS; i++) {
1038                 gyro_result[i] = 0;
1039                 accl_result[i] = 0;
1040         }
1041         s = 0;
1042         while (s < st->self_test.samples) {
1043                 result = inv_i2c_single_write(st, reg->fifo_en, d);
1044                 if (result)
1045                         return result;
1046                 mdelay(DEF_GYRO_WAIT_TIME);
1047                 result = inv_i2c_single_write(st, reg->fifo_en, 0);
1048                 if (result)
1049                         return result;
1050
1051                 result = inv_i2c_read(st, reg->fifo_count_h,
1052                                         FIFO_COUNT_BYTE, data);
1053                 if (result)
1054                         return result;
1055                 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
1056                 pr_debug("%s self_test fifo_count - %d\n",
1057                          st->hw->name, fifo_count);
1058                 packet_count = fifo_count / packet_size;
1059                 i = 0;
1060                 while ((i < packet_count) && (s < st->self_test.samples)) {
1061                         short vals[3];
1062                         result = inv_i2c_read(st, reg->fifo_r_w,
1063                                 packet_size, data);
1064                         if (result)
1065                                 return result;
1066                         ind = 0;
1067                         if (has_accl) {
1068                                 for (j = 0; j < THREE_AXIS; j++) {
1069                                         vals[j] = (short)be16_to_cpup(
1070                                             (__be16 *)(&data[ind + 2 * j]));
1071                                         accl_result[j] += vals[j];
1072                                 }
1073                                 ind += BYTES_PER_SENSOR;
1074                                 pr_debug(
1075                                     "%s self_test accel data - %d %+d %+d %+d",
1076                                     st->hw->name, s, vals[0], vals[1], vals[2]);
1077                         }
1078
1079                         for (j = 0; j < THREE_AXIS; j++) {
1080                                 vals[j] = (short)be16_to_cpup(
1081                                         (__be16 *)(&data[ind + 2 * j]));
1082                                 gyro_result[j] += vals[j];
1083                         }
1084                         pr_debug("%s self_test gyro data - %d %+d %+d %+d",
1085                                  st->hw->name, s, vals[0], vals[1], vals[2]);
1086
1087                         s++;
1088                         i++;
1089                 }
1090         }
1091
1092         if (has_accl) {
1093                 for (j = 0; j < THREE_AXIS; j++) {
1094                         accl_result[j] = accl_result[j] / s;
1095                         accl_result[j] *= DEF_ST_PRECISION;
1096                 }
1097         }
1098         for (j = 0; j < THREE_AXIS; j++) {
1099                 gyro_result[j] = gyro_result[j] / s;
1100                 gyro_result[j] *= DEF_ST_PRECISION;
1101         }
1102
1103         return 0;
1104 }
1105
1106 /*
1107  *  inv_recover_setting() recover the old settings after everything is done
1108  */
1109 void inv_recover_setting(struct inv_mpu_iio_s *st)
1110 {
1111         struct inv_reg_map_s *reg;
1112         int data;
1113
1114         reg = &st->reg;
1115         inv_i2c_single_write(st, reg->gyro_config,
1116                              st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT);
1117         inv_i2c_single_write(st, reg->lpf, st->chip_config.lpf);
1118         data = ONE_K_HZ/st->chip_config.new_fifo_rate - 1;
1119         inv_i2c_single_write(st, reg->sample_rate_div, data);
1120         if (INV_ITG3500 != st->chip_type) {
1121                 inv_i2c_single_write(st, reg->accl_config,
1122                                      (st->chip_config.accl_fs <<
1123                                      ACCL_CONFIG_FSR_SHIFT));
1124         }
1125         st->switch_gyro_engine(st, false);
1126         st->switch_accl_engine(st, false);
1127         st->set_power_state(st, false);
1128 }
1129
1130 static int inv_check_compass_self_test(struct inv_mpu_iio_s *st)
1131 {
1132         int result;
1133         u8 data[6];
1134         u8 counter, cntl;
1135         short x, y, z;
1136         u8 *sens;
1137         sens = st->chip_info.compass_sens;
1138
1139         /* set to bypass mode */
1140         result = inv_i2c_single_write(st, REG_INT_PIN_CFG,
1141                                 st->plat_data.int_config | BIT_BYPASS_EN);
1142         if (result) {
1143                 result = inv_i2c_single_write(st, REG_INT_PIN_CFG,
1144                                 st->plat_data.int_config);
1145                 return result;
1146         }
1147         /* set to power down mode */
1148         result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PD);
1149         if (result)
1150                 goto AKM_fail;
1151
1152         /* write 1 to ASTC register */
1153         result = inv_secondary_write(REG_AKM_ST_CTRL, DATA_AKM_SELF_TEST);
1154         if (result)
1155                 goto AKM_fail;
1156         /* set self test mode */
1157         result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_ST);
1158         if (result)
1159                 goto AKM_fail;
1160         counter = DEF_ST_COMPASS_TRY_TIMES;
1161         while (counter > 0) {
1162                 usleep_range(DEF_ST_COMPASS_WAIT_MIN, DEF_ST_COMPASS_WAIT_MAX);
1163                 result = inv_secondary_read(REG_AKM_STATUS, 1, data);
1164                 if (result)
1165                         goto AKM_fail;
1166                 if ((data[0] & DATA_AKM_DRDY) == 0)
1167                         counter--;
1168                 else
1169                         counter = 0;
1170         }
1171         if ((data[0] & DATA_AKM_DRDY) == 0) {
1172                 result = -EINVAL;
1173                 goto AKM_fail;
1174         }
1175         result = inv_secondary_read(REG_AKM_MEASURE_DATA,
1176                                         BYTES_PER_SENSOR, data);
1177         if (result)
1178                 goto AKM_fail;
1179
1180         x = le16_to_cpup((__le16 *)(&data[0]));
1181         y = le16_to_cpup((__le16 *)(&data[2]));
1182         z = le16_to_cpup((__le16 *)(&data[4]));
1183         x = ((x * (sens[0] + 128)) >> 8);
1184         y = ((y * (sens[1] + 128)) >> 8);
1185         z = ((z * (sens[2] + 128)) >> 8);
1186         if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) {
1187                 result = inv_secondary_read(REG_AKM8963_CNTL1, 1, &cntl);
1188                 if (result)
1189                         goto AKM_fail;
1190                 if (0 == (cntl & DATA_AKM8963_BIT)) {
1191                         x <<= DEF_ST_COMPASS_8963_SHIFT;
1192                         y <<= DEF_ST_COMPASS_8963_SHIFT;
1193                         z <<= DEF_ST_COMPASS_8963_SHIFT;
1194                 }
1195         }
1196         result = -EINVAL;
1197         if (x > st->compass_st_upper[X] || x < st->compass_st_lower[X])
1198                 goto AKM_fail;
1199         if (y > st->compass_st_upper[Y] || y < st->compass_st_lower[Y])
1200                 goto AKM_fail;
1201         if (z > st->compass_st_upper[Z] || z < st->compass_st_lower[Z])
1202                 goto AKM_fail;
1203         result = 0;
1204 AKM_fail:
1205         /*write 0 to ASTC register */
1206         result |= inv_secondary_write(REG_AKM_ST_CTRL, 0);
1207         /*set to power down mode */
1208         result |= inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PD);
1209         /*restore to non-bypass mode */
1210         result |= inv_i2c_single_write(st, REG_INT_PIN_CFG,
1211                         st->plat_data.int_config);
1212         return result;
1213 }
1214
1215 int inv_power_up_self_test(struct inv_mpu_iio_s *st)
1216 {
1217         int result;
1218
1219         result = st->set_power_state(st, true);
1220         if (result)
1221                 return result;
1222         result = st->switch_accl_engine(st, true);
1223         if (result)
1224                 return result;
1225         result = st->switch_gyro_engine(st, true);
1226         if (result)
1227                 return result;
1228
1229         return 0;
1230 }
1231
1232 /*
1233  *  inv_hw_self_test() - main function to do hardware self test
1234  */
1235 int inv_hw_self_test(struct inv_mpu_iio_s *st)
1236 {
1237         int result;
1238         int gyro_bias_st[THREE_AXIS], gyro_bias_regular[THREE_AXIS];
1239         int accl_bias_st[THREE_AXIS], accl_bias_regular[THREE_AXIS];
1240         int test_times;
1241         char compass_result, accel_result, gyro_result;
1242
1243         result = inv_power_up_self_test(st);
1244         if (result)
1245                 return result;
1246         compass_result = 0;
1247         accel_result   = 0;
1248         gyro_result    = 0;
1249         test_times = DEF_ST_TRY_TIMES;
1250         while (test_times > 0) {
1251                 result = inv_do_test(st, 0, gyro_bias_regular,
1252                         accl_bias_regular);
1253                 if (result == -EAGAIN)
1254                         test_times--;
1255                 else
1256                         test_times = 0;
1257         }
1258         if (result)
1259                 goto test_fail;
1260
1261         test_times = DEF_ST_TRY_TIMES;
1262         while (test_times > 0) {
1263                 result = inv_do_test(st, BITS_SELF_TEST_EN, gyro_bias_st,
1264                                         accl_bias_st);
1265                 if (result == -EAGAIN)
1266                         test_times--;
1267                 else
1268                         break;
1269         }
1270         if (result)
1271                 goto test_fail;
1272         if (st->chip_type == INV_ITG3500) {
1273                 gyro_result = !inv_check_3500_gyro_self_test(st,
1274                         gyro_bias_regular, gyro_bias_st);
1275         } else {
1276                 if (st->chip_config.has_compass)
1277                         compass_result = !inv_check_compass_self_test(st);
1278
1279                  if (INV_MPU6050 == st->chip_type) {
1280                         accel_result = !inv_check_accl_self_test(st,
1281                                 accl_bias_regular, accl_bias_st);
1282                         gyro_result = !inv_check_6050_gyro_self_test(st,
1283                                 gyro_bias_regular, gyro_bias_st);
1284                 } else if (INV_MPU6500 == st->chip_type) {
1285                         accel_result = !inv_check_6500_accel_self_test(st,
1286                                 accl_bias_regular, accl_bias_st);
1287                         gyro_result = !inv_check_6500_gyro_self_test(st,
1288                                 gyro_bias_regular, gyro_bias_st);
1289                 }
1290         }
1291 test_fail:
1292         inv_recover_setting(st);
1293
1294         return (compass_result << DEF_ST_COMPASS_RESULT_SHIFT) |
1295                 (accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result;
1296 }
1297
1298 static int inv_load_firmware(struct inv_mpu_iio_s *st,
1299         u8 *data, int size)
1300 {
1301         int bank, write_size;
1302         int result;
1303         u16 memaddr;
1304
1305         /* Write and verify memory */
1306         for (bank = 0; size > 0; bank++,
1307                 size -= write_size,
1308                 data += write_size) {
1309                 if (size > MPU_MEM_BANK_SIZE)
1310                         write_size = MPU_MEM_BANK_SIZE;
1311                 else
1312                         write_size = size;
1313
1314                 memaddr = ((bank << 8) | 0x00);
1315
1316                 result = mem_w(memaddr, write_size, data);
1317                 if (result)
1318                         return result;
1319         }
1320         return 0;
1321 }
1322
1323 static int inv_verify_firmware(struct inv_mpu_iio_s *st,
1324         u8 *data, int size)
1325 {
1326         int bank, write_size;
1327         int result;
1328         u16 memaddr;
1329         u8 firmware[MPU_MEM_BANK_SIZE];
1330
1331         /* Write and verify memory */
1332         for (bank = 0; size > 0; bank++,
1333                 size -= write_size,
1334                 data += write_size) {
1335                 if (size > MPU_MEM_BANK_SIZE)
1336                         write_size = MPU_MEM_BANK_SIZE;
1337                 else
1338                         write_size = size;
1339
1340                 memaddr = ((bank << 8) | 0x00);
1341                 result = mpu_memory_read(st,
1342                         st->i2c_addr, memaddr, write_size, firmware);
1343                 if (result)
1344                         return result;
1345                 if (0 != memcmp(firmware, data, write_size))
1346                         return -EINVAL;
1347         }
1348         return 0;
1349 }
1350
1351 static int inv_set_fifo_div(struct inv_mpu_iio_s *st,
1352                 u16 fifoRate)
1353 {
1354         u8 regs[2];
1355         int result = 0;
1356         /*For some reason DINAC4 is defined as 0xb8, but DINBC4 is not*/
1357         const u8 regs_end[] = {DINAFE, DINAF2, DINAAB, 0xc4,
1358                                         DINAAA, DINAF1, DINADF, DINADF,
1359                                         0xbb, 0xaf, DINADF, DINADF};
1360
1361         regs[0] = (u8)((fifoRate >> 8) & 0xff);
1362         regs[1] = (u8)(fifoRate & 0xff);
1363         result = mem_w_key(KEY_D_0_22, ARRAY_SIZE(regs), regs);
1364         if (result)
1365                 return result;
1366
1367         /*Modify the FIFO handler to reset the tap/orient interrupt flags*/
1368         /* each time the FIFO handler runs*/
1369         result = mem_w_key(KEY_CFG_6, ARRAY_SIZE(regs_end), regs_end);
1370
1371         return result;
1372 }
1373
1374 int inv_send_quaternion(struct inv_mpu_iio_s *st, bool on)
1375 {
1376         const u8 regs_on[] = {DINBC0, DINBC2,
1377                                          DINBC4, DINBC6};
1378         const u8 regs_off[] = {DINA80, DINA80,
1379                                           DINA80, DINA80};
1380         const u8 *regs;
1381         u8 result;
1382         if (on)
1383                 regs = regs_on;
1384         else
1385                 regs = regs_off;
1386         result = mem_w_key(KEY_CFG_LP_QUAT, ARRAY_SIZE(regs_on), regs);
1387
1388         return result;
1389 }
1390
1391 int inv_set_display_orient_interrupt_dmp(struct inv_mpu_iio_s *st,
1392                                                 bool on)
1393 {
1394         /*Turn on the display orientation interrupt in the DMP*/
1395         int result;
1396         u8  regs[] = {0xd8};
1397
1398         if (on)
1399                 regs[0] = 0xd9;
1400         result = mem_w_key(KEY_CFG_DISPLAY_ORIENT_INT, 1, regs);
1401         return result;
1402 }
1403
1404 int inv_set_fifo_rate(struct inv_mpu_iio_s *st, u16 fifo_rate)
1405 {
1406         u8 divider;
1407         int result;
1408
1409         divider = (u8)(ONE_K_HZ / fifo_rate) - 1;
1410         if (divider > DMP_MAX_DIVIDER) {
1411                 st->sample_divider = DMP_MAX_DIVIDER;
1412                 st->fifo_divider =
1413                         (u8)(DMP_DEFAULT_FIFO_RATE / fifo_rate) - 1;
1414         } else {
1415                 st->sample_divider = divider;
1416                 st->fifo_divider = 0;
1417         }
1418
1419         result = inv_set_fifo_div(st, st->fifo_divider);
1420         return result;
1421 }
1422
1423 static int inv_set_tap_interrupt_dmp(struct inv_mpu_iio_s *st,
1424         u8 on)
1425 {
1426         int result;
1427         u8  regs[] = {0};
1428
1429         if (on)
1430                 regs[0] = 0xf8;
1431         else
1432                 regs[0] = DINAD8;
1433         result = mem_w_key(KEY_CFG_20, ARRAY_SIZE(regs), regs);
1434         if (result)
1435                 return result;
1436         return result;
1437 }
1438
1439 int inv_set_tap_threshold_dmp(struct inv_mpu_iio_s *st,
1440                                 u32 axis, u16 threshold)
1441 {
1442         /* Sets the tap threshold in the dmp
1443         Simultaneously sets secondary tap threshold to help correct the tap
1444         direction for soft taps */
1445         int result;
1446         /* DMP Algorithm */
1447         u8 data[2];
1448         int sampleDivider;
1449         int scaledThreshold;
1450         u32 dmpThreshold;
1451         u8 sample_div;
1452         const u32  accel_sens = (0x20000000 / 0x00010000);
1453
1454         if ((axis & ~(INV_TAP_AXIS_ALL)) || (threshold > (1 << 15)))
1455                 return -EINVAL;
1456         sample_div = st->sample_divider;
1457
1458         sampleDivider = (1 + sample_div);
1459         /* Scale factor corresponds linearly using
1460         * 0  : 0
1461         * 25 : 0.0250  g/ms
1462         * 50 : 0.0500  g/ms
1463         * 100: 1.0000  g/ms
1464         * 200: 2.0000  g/ms
1465         * 400: 4.0000  g/ms
1466         * 800: 8.0000  g/ms
1467         */
1468         /*multiply by 1000 to avoid floating point 1000/1000*/
1469         scaledThreshold = threshold;
1470         /* Convert to per sample */
1471         scaledThreshold *= sampleDivider;
1472
1473         /* Scale to DMP 16 bit value */
1474         if (accel_sens != 0)
1475                 dmpThreshold = (u32)(scaledThreshold * accel_sens);
1476         else
1477                 return -EINVAL;
1478         dmpThreshold = dmpThreshold / DMP_PRECISION;
1479
1480         data[0] = dmpThreshold >> 8;
1481         data[1] = dmpThreshold & 0xFF;
1482
1483         /* MPL algorithm */
1484         if (axis & INV_TAP_AXIS_X) {
1485                 result = mem_w_key(KEY_DMP_TAP_THR_X, ARRAY_SIZE(data), data);
1486                 if (result)
1487                         return result;
1488
1489                 /*Also set additional threshold for correcting the direction
1490                 of taps that were very near the threshold. */
1491                 data[0] = (dmpThreshold * 3 / 4) >> 8;
1492                 data[1] = (dmpThreshold * 3 / 4) & 0xFF;
1493                 result = mem_w_key(KEY_D_1_36, ARRAY_SIZE(data), data);
1494                 if (result)
1495                         return result;
1496         }
1497         if (axis & INV_TAP_AXIS_Y) {
1498                 result = mem_w_key(KEY_DMP_TAP_THR_Y, 2, data);
1499                 if (result)
1500                         return result;
1501                 data[0] = (dmpThreshold * 3 / 4) >> 8;
1502                 data[1] = (dmpThreshold * 3 / 4) & 0xFF;
1503
1504                 result = mem_w_key(KEY_D_1_40, ARRAY_SIZE(data), data);
1505                 if (result)
1506                         return result;
1507         }
1508         if (axis & INV_TAP_AXIS_Z) {
1509                 result = mem_w_key(KEY_DMP_TAP_THR_Z, ARRAY_SIZE(data), data);
1510                 if (result)
1511                         return result;
1512                 data[0] = (dmpThreshold * 3 / 4) >> 8;
1513                 data[1] = (dmpThreshold * 3 / 4) & 0xFF;
1514
1515                 result = mem_w_key(KEY_D_1_44, ARRAY_SIZE(data), data);
1516                 if (result)
1517                         return result;
1518         }
1519         return 0;
1520 }
1521
1522 static int inv_set_tap_axes_dmp(struct inv_mpu_iio_s *st,
1523                                 u32 axes)
1524 {
1525         /* Sets a mask in the DMP that indicates what tap events
1526         should result in an interrupt */
1527         u8 regs[4];
1528         u8 result;
1529
1530         /* check if any spurious bit other the ones expected are set */
1531         if (axes & (~(INV_TAP_ALL_DIRECTIONS)))
1532                 return -EINVAL;
1533
1534         regs[0] = (u8)axes;
1535         result = mem_w_key(KEY_D_1_72, 1, regs);
1536
1537         return result;
1538 }
1539
1540 int inv_set_min_taps_dmp(struct inv_mpu_iio_s *st,
1541                                 u16 min_taps) {
1542         /*Indicates the minimum number of consecutive taps required
1543                 before the DMP will generate an interrupt */
1544         u8 regs[1];
1545         u8 result;
1546         /* check if any spurious bit other the ones expected are set */
1547         if ((min_taps > DMP_MAX_MIN_TAPS) || (min_taps < 1))
1548                 return -EINVAL;
1549         regs[0] = (u8)(min_taps-1);
1550         result = mem_w_key(KEY_D_1_79, ARRAY_SIZE(regs), regs);
1551
1552         return result;
1553 }
1554
1555 int  inv_set_tap_time_dmp(struct inv_mpu_iio_s *st, u16 time)
1556 {
1557         /* Determines how long after a tap the DMP requires before
1558           another tap can be registered*/
1559         int result;
1560         /* DMP Algorithm */
1561         u16 dmpTime;
1562         u8 data[2];
1563         u8 sampleDivider;
1564
1565         sampleDivider = st->sample_divider;
1566         sampleDivider++;
1567
1568         /* 60 ms minimum time added */
1569         dmpTime = ((time) / sampleDivider);
1570         data[0] = dmpTime >> 8;
1571         data[1] = dmpTime & 0xFF;
1572
1573         result = mem_w_key(KEY_DMP_TAPW_MIN, ARRAY_SIZE(data), data);
1574
1575         return result;
1576 }
1577
1578 static int inv_set_multiple_tap_time_dmp(struct inv_mpu_iio_s *st,
1579                                         u32 time)
1580 {
1581         /*Determines how close together consecutive taps must occur
1582         to be considered double/triple taps*/
1583         int result;
1584         /* DMP Algorithm */
1585         u16 dmpTime;
1586         u8 data[2];
1587         u8 sampleDivider;
1588
1589         sampleDivider = st->sample_divider;
1590         sampleDivider++;
1591
1592         /* 60 ms minimum time added */
1593         dmpTime = ((time) / sampleDivider);
1594         data[0] = dmpTime >> 8;
1595         data[1] = dmpTime & 0xFF;
1596         result = mem_w_key(KEY_D_1_218, ARRAY_SIZE(data), data);
1597
1598         return result;
1599 }
1600
1601 int inv_q30_mult(int a, int b)
1602 {
1603         u64 temp;
1604         int result;
1605
1606         temp = (u64)a * b;
1607         result = (int)(temp >> DMP_MULTI_SHIFT);
1608
1609         return result;
1610 }
1611
1612 static u16 inv_row_2_scale(const s8 *row)
1613 {
1614         u16 b;
1615
1616         if (row[0] > 0)
1617                 b = 0;
1618         else if (row[0] < 0)
1619                 b = 4;
1620         else if (row[1] > 0)
1621                 b = 1;
1622         else if (row[1] < 0)
1623                 b = 5;
1624         else if (row[2] > 0)
1625                 b = 2;
1626         else if (row[2] < 0)
1627                 b = 6;
1628         else
1629                 b = 7;
1630
1631         return b;
1632 }
1633
1634 /** Converts an orientation matrix made up of 0,+1,and -1 to a scalar
1635 *       representation.
1636 * @param[in] mtx Orientation matrix to convert to a scalar.
1637 * @return Description of orientation matrix. The lowest 2 bits (0 and 1)
1638 * represent the column the one is on for the
1639 * first row, with the bit number 2 being the sign. The next 2 bits
1640 * (3 and 4) represent
1641 * the column the one is on for the second row with bit number 5 being
1642 * the sign.
1643 * The next 2 bits (6 and 7) represent the column the one is on for the
1644 * third row with
1645 * bit number 8 being the sign. In binary the identity matrix would therefor
1646 * be: 010_001_000 or 0x88 in hex.
1647 */
1648 static u16 inv_orientation_matrix_to_scaler(const signed char *mtx)
1649 {
1650
1651         u16 scalar;
1652         scalar = inv_row_2_scale(mtx);
1653         scalar |= inv_row_2_scale(mtx + 3) << 3;
1654         scalar |= inv_row_2_scale(mtx + 6) << 6;
1655
1656         return scalar;
1657 }
1658 #if 0
1659 static int inv_disable_gyro_cal(struct inv_mpu_iio_s *st)
1660 {
1661         const u8 regs[] = {
1662                 0xb8, 0xaa, 0xaa, 0xaa,
1663                 0xb0, 0x88, 0xc3, 0xc5,
1664                 0xc7
1665         };
1666         return mem_w_key(KEY_CFG_MOTION_BIAS, ARRAY_SIZE(regs), regs);
1667 }
1668 #endif
1669
1670 static int inv_gyro_dmp_cal(struct inv_mpu_iio_s *st)
1671 {
1672         int inv_gyro_orient;
1673         u8 regs[3];
1674         int result;
1675
1676         u8 tmpD = DINA4C;
1677         u8 tmpE = DINACD;
1678         u8 tmpF = DINA6C;
1679
1680         inv_gyro_orient =
1681                 inv_orientation_matrix_to_scaler(st->plat_data.orientation);
1682
1683         if ((inv_gyro_orient & 3) == 0)
1684                 regs[0] = tmpD;
1685         else if ((inv_gyro_orient & 3) == 1)
1686                 regs[0] = tmpE;
1687         else if ((inv_gyro_orient & 3) == 2)
1688                 regs[0] = tmpF;
1689         if ((inv_gyro_orient & 0x18) == 0)
1690                 regs[1] = tmpD;
1691         else if ((inv_gyro_orient & 0x18) == 0x8)
1692                 regs[1] = tmpE;
1693         else if ((inv_gyro_orient & 0x18) == 0x10)
1694                 regs[1] = tmpF;
1695         if ((inv_gyro_orient & 0xc0) == 0)
1696                 regs[2] = tmpD;
1697         else if ((inv_gyro_orient & 0xc0) == 0x40)
1698                 regs[2] = tmpE;
1699         else if ((inv_gyro_orient & 0xc0) == 0x80)
1700                 regs[2] = tmpF;
1701
1702         result = mem_w_key(KEY_FCFG_1, ARRAY_SIZE(regs), regs);
1703         if (result)
1704                 return result;
1705
1706         if (inv_gyro_orient & 4)
1707                 regs[0] = DINA36 | 1;
1708         else
1709                 regs[0] = DINA36;
1710         if (inv_gyro_orient & 0x20)
1711                 regs[1] = DINA56 | 1;
1712         else
1713                 regs[1] = DINA56;
1714         if (inv_gyro_orient & 0x100)
1715                 regs[2] = DINA76 | 1;
1716         else
1717                 regs[2] = DINA76;
1718
1719         result = mem_w_key(KEY_FCFG_3, ARRAY_SIZE(regs), regs);
1720
1721         return result;
1722 }
1723
1724 static int inv_accel_dmp_cal(struct inv_mpu_iio_s *st)
1725 {
1726         int inv_accel_orient;
1727         int result;
1728         u8 regs[3];
1729         const u8 tmp[3] = { DINA0C, DINAC9, DINA2C };
1730         inv_accel_orient =
1731                 inv_orientation_matrix_to_scaler(st->plat_data.orientation);
1732
1733         regs[0] = tmp[inv_accel_orient & 3];
1734         regs[1] = tmp[(inv_accel_orient >> 3) & 3];
1735         regs[2] = tmp[(inv_accel_orient >> 6) & 3];
1736         result = mem_w_key(KEY_FCFG_2, ARRAY_SIZE(regs), regs);
1737         if (result)
1738                 return result;
1739
1740         regs[0] = DINA26;
1741         regs[1] = DINA46;
1742         regs[2] = DINA66;
1743         if (inv_accel_orient & 4)
1744                 regs[0] |= 1;
1745         if (inv_accel_orient & 0x20)
1746                 regs[1] |= 1;
1747         if (inv_accel_orient & 0x100)
1748                 regs[2] |= 1;
1749         result = mem_w_key(KEY_FCFG_7, ARRAY_SIZE(regs), regs);
1750
1751         return result;
1752 }
1753
1754 static u16 inv_orientation_matrix_to_scalar(const s8 *mtx)
1755 {
1756
1757         u16 scalar;
1758
1759         /*
1760        XYZ  010_001_000 Identity Matrix
1761        XZY  001_010_000
1762        YXZ  010_000_001
1763        YZX  000_010_001
1764        ZXY  001_000_010
1765        ZYX  000_001_010
1766         */
1767
1768         scalar = inv_row_2_scale(mtx);
1769         scalar |= inv_row_2_scale(mtx + 3) << 3;
1770         scalar |= inv_row_2_scale(mtx + 6) << 6;
1771
1772         return scalar;
1773 }
1774
1775 int inv_set_accel_bias_dmp(struct inv_mpu_iio_s *st)
1776 {
1777         int inv_accel_orient, result, i, accel_bias_body[3], out[3];
1778         int tmp[] = {1, 1, 1};
1779         int mask[] = {4, 0x20, 0x100};
1780         int accel_sf = 0x20000000;/* 536870912 */
1781         u8 *regs;
1782
1783         inv_accel_orient =
1784                 inv_orientation_matrix_to_scalar(st->plat_data.orientation);
1785
1786         for (i = 0; i < 3; i++)
1787                 if (inv_accel_orient & mask[i])
1788                         tmp[i] = -1;
1789
1790         for (i = 0; i < 3; i++)
1791                 accel_bias_body[i] = st->input_accel_bias[(inv_accel_orient >>
1792                                         (i * 3)) & 3] * tmp[i];
1793         for (i = 0; i < 3; i++)
1794                 accel_bias_body[i] = inv_q30_mult(accel_sf,
1795                                         accel_bias_body[i]);
1796         for (i = 0; i < 3; i++)
1797                 out[i] = cpu_to_be32p(&accel_bias_body[i]);
1798         regs = (u8 *)out;
1799         result = mem_w_key(KEY_D_ACCEL_BIAS, sizeof(out), regs);
1800
1801         return result;
1802 }
1803
1804 static int inv_set_gyro_sf_dmp(struct inv_mpu_iio_s *st)
1805 {
1806         /*The gyro threshold, in dps, above which taps will be rejected*/
1807         int result;
1808         /* DMP Algorithm */
1809         u8 sampleDivider;
1810         u32 gyro_sf;
1811         const u32 gyro_sens = 0x03e80000;
1812
1813         sampleDivider = st->sample_divider;
1814         gyro_sf = inv_q30_mult(gyro_sens,
1815                         (int)(DMP_TAP_SCALE * (sampleDivider + 1)));
1816         result = write_be32_key_to_mem(st, gyro_sf, KEY_D_0_104);
1817
1818         return result;
1819 }
1820
1821 static int inv_set_shake_reject_thresh_dmp(struct inv_mpu_iio_s *st,
1822                                                 int thresh)
1823 {       /*THIS FUNCTION FAILS MEM_W*/
1824         /*The gyro threshold, in dps, above which taps will be rejected */
1825         int result;
1826         /* DMP Algorithm */
1827         u8 sampleDivider;
1828         int thresh_scaled;
1829         u32 gyro_sf;
1830         const u32 gyro_sens = 0x03e80000;
1831         sampleDivider = st->sample_divider;
1832         gyro_sf = inv_q30_mult(gyro_sens, (int)(DMP_TAP_SCALE *
1833                         (sampleDivider + 1)));
1834         /* We're in units of DPS, convert it back to chip units*/
1835         /*split the operation to aviod overflow of integer*/
1836         thresh_scaled = gyro_sens / (1L << 16);
1837         thresh_scaled = thresh_scaled / thresh;
1838         thresh_scaled = gyro_sf / thresh_scaled;
1839         result = write_be32_key_to_mem(st, thresh_scaled, KEY_D_1_92);
1840
1841         return result;
1842 }
1843
1844 static int inv_set_shake_reject_time_dmp(struct inv_mpu_iio_s *st,
1845                                                 u32 time)
1846 {
1847         /* How long a gyro axis must remain above its threshold
1848         before taps are rejected */
1849         int result;
1850         /* DMP Algorithm */
1851         u16 dmpTime;
1852         u8 data[2];
1853         u8 sampleDivider;
1854
1855         sampleDivider = st->sample_divider;
1856         sampleDivider++;
1857
1858         /* 60 ms minimum time added */
1859         dmpTime = ((time) / sampleDivider);
1860         data[0] = dmpTime >> 8;
1861         data[1] = dmpTime & 0xFF;
1862
1863         result = mem_w_key(KEY_D_1_88, ARRAY_SIZE(data), data);
1864         return result;
1865 }
1866
1867 static int inv_set_shake_reject_timeout_dmp(struct inv_mpu_iio_s *st,
1868                                                 u32 time)
1869 {
1870         /*How long the gyros must remain below their threshold,
1871         after taps have been rejected, before taps can be detected again*/
1872         int result;
1873         /* DMP Algorithm */
1874         u16 dmpTime;
1875         u8 data[2];
1876         u8 sampleDivider;
1877
1878         sampleDivider = st->sample_divider;
1879         sampleDivider++;
1880
1881         /* 60 ms minimum time added */
1882         dmpTime = ((time) / sampleDivider);
1883         data[0] = dmpTime >> 8;
1884         data[1] = dmpTime & 0xFF;
1885
1886         result = mem_w_key(KEY_D_1_90, ARRAY_SIZE(data), data);
1887         return result;
1888 }
1889
1890 int inv_set_interrupt_on_gesture_event(struct inv_mpu_iio_s *st, bool on)
1891 {
1892         u8 result;
1893         const u8 regs_on[] = {DINADA, DINAB1, DINAB9,
1894                                          DINAF3, DINA8B, DINAA3, DINA91,
1895                                          DINAB6, DINADA, DINAB4, DINADA};
1896         const u8 regs_off[] = {0xd8, 0xb1, 0xb9, 0xf3, 0x8b,
1897                                           0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
1898         /*For some reason DINAC4 is defined as 0xb8,
1899         but DINBC4 is not defined.*/
1900         const u8 regs_end[] = {DINAFE, DINAF2, DINAAB, 0xc4,
1901                                         DINAAA, DINAF1, DINADF, DINADF,
1902                                         0xbb, 0xaf, DINADF, DINADF};
1903         const u8 regs[] = {0, 0};
1904         /* reset fifo count to zero */
1905         result = mem_w_key(KEY_D_1_178, ARRAY_SIZE(regs), regs);
1906         if (result)
1907                 return result;
1908
1909         if (on)
1910                 /*Sets the DMP to send an interrupt and put a FIFO packet
1911                 in the FIFO if and only if a tap/orientation event
1912                 just occurred*/
1913                 result = mem_w_key(KEY_CFG_FIFO_ON_EVENT, ARRAY_SIZE(regs_on),
1914                                         regs_on);
1915         else
1916                 /*Sets the DMP to send an interrupt and put a FIFO packet
1917                 in the FIFO at the rate specified by the FIFO div.
1918                 see inv_set_fifo_div in hw_setup.c to set the FIFO div.*/
1919                 result = mem_w_key(KEY_CFG_FIFO_ON_EVENT, ARRAY_SIZE(regs_off),
1920                                         regs_off);
1921         if (result)
1922                 return result;
1923
1924         result = mem_w_key(KEY_CFG_6, ARRAY_SIZE(regs_end), regs_end);
1925
1926         return result;
1927 }
1928
1929 /**
1930  * inv_enable_tap_dmp() -  calling this function will enable/disable tap function.
1931  */
1932 int inv_enable_tap_dmp(struct inv_mpu_iio_s *st, bool on)
1933 {
1934         int result;
1935
1936         result = inv_set_tap_interrupt_dmp(st, on);
1937         if (result)
1938                 return result;
1939         if (on) {
1940                 result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_X,
1941                                                    st->tap.thresh);
1942                 if (result)
1943                         return result;
1944                 result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_Y,
1945                                                    st->tap.thresh);
1946                 if (result)
1947                         return result;
1948                 result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_Z,
1949                                                    st->tap.thresh);
1950                 if (result)
1951                         return result;
1952         }
1953
1954         result = inv_set_tap_axes_dmp(st, INV_TAP_ALL_DIRECTIONS);
1955         if (result)
1956                 return result;
1957         result = inv_set_min_taps_dmp(st, st->tap.min_count);
1958         if (result)
1959                 return result;
1960
1961         result = inv_set_tap_time_dmp(st, st->tap.time);
1962         if (result)
1963                 return result;
1964
1965         result = inv_set_multiple_tap_time_dmp(st, DMP_MULTI_TAP_TIME);
1966         if (result)
1967                 return result;
1968
1969         result = inv_set_gyro_sf_dmp(st);
1970         if (result)
1971                 return result;
1972
1973         result = inv_set_shake_reject_thresh_dmp(st, DMP_SHAKE_REJECT_THRESH);
1974         if (result)
1975                 return result;
1976
1977         result = inv_set_shake_reject_time_dmp(st, DMP_SHAKE_REJECT_TIME);
1978         if (result)
1979                 return result;
1980
1981         result = inv_set_shake_reject_timeout_dmp(st,
1982                                                   DMP_SHAKE_REJECT_TIMEOUT);
1983         return result;
1984 }
1985
1986 int inv_send_sensor_data(struct inv_mpu_iio_s *st, u16 elements)
1987 {
1988         int result;
1989         u8 regs[] = {DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1990                                 DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1991                                 DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1992                                 DINAA0 + 3};
1993
1994         if (elements & INV_ELEMENT_1)
1995                 regs[0] = DINACA;
1996         if (elements & INV_ELEMENT_2)
1997                 regs[4] = DINBC4;
1998         if (elements & INV_ELEMENT_3)
1999                 regs[5] = DINACC;
2000         if (elements & INV_ELEMENT_4)
2001                 regs[6] = DINBC6;
2002         if ((elements & INV_ELEMENT_5) || (elements & INV_ELEMENT_6) ||
2003             (elements & INV_ELEMENT_7)) {
2004                 regs[1] = DINBC0;
2005                 regs[2] = DINAC8;
2006                 regs[3] = DINBC2;
2007         }
2008         result = mem_w_key(KEY_CFG_15, ARRAY_SIZE(regs), regs);
2009         return result;
2010 }
2011
2012 int inv_send_interrupt_word(struct inv_mpu_iio_s *st, bool on)
2013 {
2014         const u8 regs_on[] = { DINA20 };
2015         const u8 regs_off[] = { DINAA3 };
2016         u8 result;
2017
2018         if (on)
2019                 result = mem_w_key(KEY_CFG_27, ARRAY_SIZE(regs_on), regs_on);
2020         else
2021                 result = mem_w_key(KEY_CFG_27, ARRAY_SIZE(regs_off), regs_off);
2022
2023         return result;
2024 }
2025
2026 /**
2027  * inv_dmp_firmware_write() -  calling this function will load the firmware.
2028  *                        This is the write function of file "dmp_firmware".
2029  */
2030 ssize_t inv_dmp_firmware_write(struct file *fp, struct kobject *kobj,
2031         struct bin_attribute *attr,
2032         char *buf, loff_t pos, size_t size)
2033 {
2034         u8 *firmware;
2035         int result;
2036         struct inv_reg_map_s *reg;
2037         struct iio_dev *indio_dev;
2038         struct inv_mpu_iio_s *st;
2039
2040         indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj));
2041         st = iio_priv(indio_dev);
2042
2043         if (st->chip_config.firmware_loaded)
2044                 return -EINVAL;
2045         if (st->chip_config.enable)
2046                 return -EBUSY;
2047
2048         reg = &st->reg;
2049         if (DMP_IMAGE_SIZE != size) {
2050                 pr_err("wrong DMP image size\n");
2051                 return -EINVAL;
2052         }
2053
2054         firmware = kmalloc(size, GFP_KERNEL);
2055         if (!firmware)
2056                 return -ENOMEM;
2057
2058         mutex_lock(&indio_dev->mlock);
2059
2060         memcpy(firmware, buf, size);
2061         result = crc32(CRC_FIRMWARE_SEED, firmware, size);
2062         if (DMP_IMAGE_CRC_VALUE != result) {
2063                 pr_err("firmware CRC error - 0x%08x vs 0x%08x\n",
2064                         result, DMP_IMAGE_CRC_VALUE);
2065                 result = -EINVAL;
2066                 goto firmware_write_fail;
2067         }
2068
2069         result = st->set_power_state(st, true);
2070         if (result)
2071                 goto firmware_write_fail;
2072
2073         result = inv_load_firmware(st, firmware, size);
2074         if (result)
2075                 goto firmware_write_fail;
2076
2077         result = inv_verify_firmware(st, firmware, size);
2078         if (result)
2079                 goto firmware_write_fail;
2080
2081         result = inv_i2c_single_write(st, reg->prgm_strt_addrh,
2082         st->chip_config.prog_start_addr >> 8);
2083         if (result)
2084                 goto firmware_write_fail;
2085         result = inv_i2c_single_write(st, reg->prgm_strt_addrh + 1,
2086         st->chip_config.prog_start_addr & 0xff);
2087         if (result)
2088                 goto firmware_write_fail;
2089
2090         result = inv_set_fifo_rate(st, DMP_DEFAULT_FIFO_RATE);
2091         if (result)
2092                 goto firmware_write_fail;
2093         result = inv_gyro_dmp_cal(st);
2094         if (result)
2095                 goto firmware_write_fail;
2096         result = inv_accel_dmp_cal(st);
2097         if (result)
2098                 goto firmware_write_fail;
2099         /* result = inv_disable_gyro_cal(st);*/
2100         if (result)
2101                 goto firmware_write_fail;
2102
2103         st->chip_config.firmware_loaded = 1;
2104
2105 firmware_write_fail:
2106         result |= st->set_power_state(st, false);
2107         mutex_unlock(&indio_dev->mlock);
2108         kfree(firmware);
2109         if (result)
2110                 return result;
2111         return size;
2112 }
2113
2114 ssize_t inv_dmp_firmware_read(struct file *filp,
2115                                 struct kobject *kobj,
2116                                 struct bin_attribute *bin_attr,
2117                                 char *buf, loff_t off, size_t count)
2118 {
2119         int bank, write_size, size, data, result;
2120         u16 memaddr;
2121         struct iio_dev *indio_dev;
2122         struct inv_mpu_iio_s *st;
2123
2124         size = count;
2125         indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj));
2126         st = iio_priv(indio_dev);
2127
2128         data = 0;
2129         mutex_lock(&indio_dev->mlock);
2130         if (!st->chip_config.enable) {
2131                 result = st->set_power_state(st, true);
2132                 if (result) {
2133                         mutex_unlock(&indio_dev->mlock);
2134                         return result;
2135                 }
2136         }
2137         for (bank = 0; size > 0; bank++, size -= write_size,
2138                                         data += write_size) {
2139                 if (size > MPU_MEM_BANK_SIZE)
2140                         write_size = MPU_MEM_BANK_SIZE;
2141                 else
2142                         write_size = size;
2143
2144                 memaddr = (bank << 8);
2145                 result = mpu_memory_read(st,
2146                         st->i2c_addr, memaddr, write_size, &buf[data]);
2147                 if (result) {
2148                         mutex_unlock(&indio_dev->mlock);
2149                         return result;
2150                 }
2151         }
2152         if (!st->chip_config.enable)
2153                 result = st->set_power_state(st, false);
2154         mutex_unlock(&indio_dev->mlock);
2155         if (result)
2156                 return result;
2157
2158         return count;
2159 }
2160 /**
2161  *  @}
2162  */