drivers/iio/imu/inv_mpu/inv530/inv_mpu_selftest.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 #define pr_fmt(fmt) "inv_mpu: " fmt
  14
  15 #include <linux/module.h>
  16 #include <linux/init.h>
  17 #include <linux/slab.h>
  18 #include <linux/err.h>
  19 #include <linux/delay.h>
  20 #include <linux/sysfs.h>
  21 #include <linux/jiffies.h>
  22 #include <linux/irq.h>
  23 #include <linux/interrupt.h>
  24 #include <linux/kfifo.h>
  25 #include <linux/poll.h>
  26 #include <linux/miscdevice.h>
  27 #include <linux/crc32.h>
  28
  29 #include "inv_mpu_iio.h"
  30
  31 /* full scale and LPF setting */
  32 #define SELFTEST_GYRO_FS            ((0 << 3) | 1)
  33 #define SELFTEST_ACCEL_FS           ((7 << 3) | 1)
  34
  35 /* register settings */
  36 #define SELFTEST_GYRO_SMPLRT_DIV        10
  37 #define SELFTEST_GYRO_AVGCFG            3
  38 #define SELFTEST_ACCEL_SMPLRT_DIV       10
  39 #define SELFTEST_ACCEL_DEC3_CFG         2
  40
  41 #define DEF_SELFTEST_GYRO_SENS          (32768 / 250)
  42 /* wait time before collecting data */
  43 #define MAX_PACKETS                     20
  44 #define SELFTEST_WAIT_TIME              (MAX_PACKETS * 10)
  45 #define DEF_ST_STABLE_TIME              20
  46 #define DEF_GYRO_SCALE                  131
  47 #define DEF_ST_PRECISION                1000
  48 #define DEF_ST_ACCEL_FS_MG              2000UL
  49 #define DEF_ST_SCALE                    32768
  50 #define DEF_ST_TRY_TIMES                2
  51 #define DEF_ST_ACCEL_RESULT_SHIFT       1
  52 #define DEF_ST_COMPASS_RESULT_SHIFT     2
  53 #define DEF_ST_SAMPLES                  200
  54
  55 #define DEF_ACCEL_ST_SHIFT_DELTA_MIN    500
  56 #define DEF_ACCEL_ST_SHIFT_DELTA_MAX    1500
  57 #define DEF_GYRO_CT_SHIFT_DELTA         500
  58
  59 /* Gyro Offset Max Value (dps) */
  60 #define DEF_GYRO_OFFSET_MAX             20
  61 /* Gyro Self Test Absolute Limits ST_AL (dps) */
  62 #define DEF_GYRO_ST_AL                  60
  63 /* Accel Self Test Absolute Limits ST_AL (mg) */
  64 #define DEF_ACCEL_ST_AL_MIN             225
  65 #define DEF_ACCEL_ST_AL_MAX             675
  66
  67 struct recover_regs {
  68         /* Bank#0 */
  69         u8 fifo_cfg;                    /* REG_FIFO_CFG */
  70         u8 user_ctrl;                   /* REG_USER_CTRL */
  71         u8 lp_config;                   /* REG_LP_CONFIG */
  72         u8 int_enable;                  /* REG_INT_ENABLE */
  73         u8 int_enable_1;                /* REG_INT_ENABLE_1 */
  74         u8 fifo_en;                             /* REG_FIFO_EN */
  75         u8 fifo_en_2;                   /* REG_FIFO_EN_2 */
  76         u8 fifo_rst;                    /* REG_FIFO_RST */
  77
  78         /* Bank#2 */
  79         u8 gyro_smplrt_div;             /* REG_GYRO_SMPLRT_DIV */
  80         u8 gyro_config_1;               /* REG_GYRO_CONFIG_1 */
  81         u8 gyro_config_2;               /* REG_GYRO_CONFIG_2 */
  82         u8 accel_smplrt_div_1;  /* REG_ACCEL_SMPLRT_DIV_1 */
  83         u8 accel_smplrt_div_2;  /* REG_ACCEL_SMPLRT_DIV_2 */
  84         u8 accel_config;                /* REG_ACCEL_CONFIG */
  85         u8 accel_config_2;              /* REG_ACCEL_CONFIG_2 */
  86 };
  87
  88 static struct recover_regs saved_regs;
  89
  90
  91 static const u16 mpu_st_tb[256] = {
  92         2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
  93         2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
  94         3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
  95         3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
  96         3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
  97         3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
  98         4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
  99         4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
 100         4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
 101         5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
 102         5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
 103         6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
 104         6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
 105         7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
 106         7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
 107         8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
 108         9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
 109         10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
 110         10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
 111         11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
 112         12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
 113         13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
 114         15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
 115         16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
 116         17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
 117         19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
 118         20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
 119         22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
 120         24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
 121         26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
 122         28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
 123         30903, 31212, 31524, 31839, 32157, 32479, 32804
 124 };
 125
 126 static void inv_show_saved_setting(struct inv_mpu_state *st)
 127 {
 128         pr_debug(" REG_FIFO_CFG : 0x%02X\n", saved_regs.fifo_cfg);
 129         pr_debug(" REG_USER_CTRL : 0x%02X\n", saved_regs.user_ctrl);
 130         pr_debug(" REG_LP_CONFIG : 0x%02X\n", saved_regs.lp_config);
 131         pr_debug(" REG_INT_ENABLE : 0x%02X\n", saved_regs.int_enable);
 132         pr_debug(" REG_INT_ENABLE_1 : 0x%02X\n", saved_regs.int_enable_1);
 133         pr_debug(" REG_FIFO_EN : 0x%02x\n", saved_regs.fifo_en);
 134         pr_debug(" REG_FIFO_EN_2 : 0x%02x\n", saved_regs.fifo_en_2);
 135         pr_debug(" REG_FIFO_RST : 0x%02x\n", saved_regs.fifo_rst);
 136         pr_debug(" REG_GYRO_SMPLRT_DIV : 0x%02x\n", saved_regs.gyro_smplrt_div);
 137         pr_debug(" REG_GYRO_CONFIG_1 : 0x%02x\n", saved_regs.gyro_config_1);
 138         pr_debug(" REG_GYRO_CONFIG_2 : 0x%02x\n", saved_regs.gyro_config_2);
 139         pr_debug(" REG_ACCEL_SMPLRT_DIV_1 : 0x%02x\n",
 140                         saved_regs.accel_smplrt_div_1);
 141         pr_debug(" REG_ACCEL_SMPLRT_DIV_2 : 0x%02x\n",
 142                         saved_regs.accel_smplrt_div_2);
 143         pr_debug(" REG_ACCEL_CONFIG : 0x%02x\n", saved_regs.accel_config);
 144         pr_debug(" REG_ACCEL_CONFIG_2 : 0x%02x\n", saved_regs.accel_config_2);
 145 }
 146
 147 static int inv_save_setting(struct inv_mpu_state *st)
 148 {
 149         int result;
 150
 151         result = inv_set_bank(st, BANK_SEL_0);
 152         if (result)
 153                 return result;
 154
 155         result = inv_plat_read(st, REG_FIFO_CFG, 1, &saved_regs.fifo_cfg);
 156         if (result)
 157                 return result;
 158         result = inv_plat_read(st, REG_USER_CTRL, 1, &saved_regs.user_ctrl);
 159         if (result)
 160                 return result;
 161         result = inv_plat_read(st, REG_LP_CONFIG, 1, &saved_regs.lp_config);
 162         if (result)
 163                 return result;
 164         result = inv_plat_read(st, REG_INT_ENABLE, 1, &saved_regs.int_enable);
 165         if (result)
 166                 return result;
 167         result = inv_plat_read(st, REG_INT_ENABLE_1, 1,
 168                         &saved_regs.int_enable_1);
 169         if (result)
 170                 return result;
 171         result = inv_plat_read(st, REG_FIFO_EN, 1, &saved_regs.fifo_en);
 172         if (result)
 173                 return result;
 174         result = inv_plat_read(st, REG_FIFO_EN_2, 1, &saved_regs.fifo_en_2);
 175         if (result)
 176                 return result;
 177         result = inv_plat_read(st, REG_FIFO_RST, 1, &saved_regs.fifo_rst);
 178         if (result)
 179                 return result;
 180
 181         result = inv_set_bank(st, BANK_SEL_2);
 182         if (result)
 183                 return result;
 184
 185         result = inv_plat_read(st, REG_GYRO_SMPLRT_DIV, 1,
 186                         &saved_regs.gyro_smplrt_div);
 187         if (result)
 188                 return result;
 189         result = inv_plat_read(st, REG_GYRO_CONFIG_1, 1,
 190                         &saved_regs.gyro_config_1);
 191         if (result)
 192                 return result;
 193         result = inv_plat_read(st, REG_GYRO_CONFIG_2, 1,
 194                         &saved_regs.gyro_config_2);
 195         if (result)
 196                 return result;
 197         result = inv_plat_read(st, REG_ACCEL_SMPLRT_DIV_1, 1,
 198                         &saved_regs.accel_smplrt_div_1);
 199         if (result)
 200                 return result;
 201         result = inv_plat_read(st, REG_ACCEL_SMPLRT_DIV_2, 1,
 202                         &saved_regs.accel_smplrt_div_2);
 203         if (result)
 204                 return result;
 205         result = inv_plat_read(st, REG_ACCEL_CONFIG, 1,
 206                         &saved_regs.accel_config);
 207         if (result)
 208                 return result;
 209         result = inv_plat_read(st, REG_ACCEL_CONFIG_2, 1,
 210                         &saved_regs.accel_config_2);
 211         if (result)
 212                 return result;
 213
 214         result = inv_set_bank(st, BANK_SEL_0);
 215         if (result)
 216                 return result;
 217
 218         inv_show_saved_setting(st);
 219
 220         return result;
 221 }
 222
 223 static int inv_recover_setting(struct inv_mpu_state *st)
 224 {
 225         int result;
 226
 227         result = inv_set_bank(st, BANK_SEL_0);
 228         if (result)
 229                 return result;
 230
 231         /* Stop sensors */
 232         result = inv_plat_single_write(st, REG_PWR_MGMT_2,
 233                 BIT_PWR_PRESSURE_STBY | BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY);
 234         if (result)
 235                 return result;
 236
 237         result = inv_set_bank(st, BANK_SEL_2);
 238         if (result)
 239                 return result;
 240
 241         /* Restore sensor configurations */
 242         result = inv_plat_single_write(st, REG_GYRO_SMPLRT_DIV,
 243                         saved_regs.gyro_smplrt_div);
 244         if (result)
 245                 return result;
 246         result = inv_plat_single_write(st, REG_GYRO_CONFIG_1,
 247                         saved_regs.gyro_config_1);
 248         if (result)
 249                 return result;
 250         result = inv_plat_single_write(st, REG_GYRO_CONFIG_2,
 251                         saved_regs.gyro_config_2);
 252         if (result)
 253                 return result;
 254         result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_1,
 255                         saved_regs.accel_smplrt_div_1);
 256         if (result)
 257                 return result;
 258         result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_2,
 259                         saved_regs.accel_smplrt_div_2);
 260         if (result)
 261                 return result;
 262         result = inv_plat_single_write(st, REG_ACCEL_CONFIG,
 263                         saved_regs.accel_config);
 264         if (result)
 265                 return result;
 266         result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2,
 267                         saved_regs.accel_config_2);
 268         if (result)
 269                 return result;
 270
 271         result = inv_set_bank(st, BANK_SEL_0);
 272         if (result)
 273                 return result;
 274
 275         /* Restore FIFO configurations */
 276         result = inv_plat_single_write(st, REG_FIFO_CFG, saved_regs.fifo_cfg);
 277         if (result)
 278                 return result;
 279         result = inv_plat_single_write(st, REG_LP_CONFIG, saved_regs.lp_config);
 280         if (result)
 281                 return result;
 282         result = inv_plat_single_write(st, REG_INT_ENABLE,
 283                         saved_regs.int_enable);
 284         if (result)
 285                 return result;
 286         result = inv_plat_single_write(st, REG_INT_ENABLE_1,
 287                         saved_regs.int_enable_1);
 288         if (result)
 289                 return result;
 290         result = inv_plat_single_write(st, REG_FIFO_EN, saved_regs.fifo_en);
 291         if (result)
 292                 return result;
 293         result = inv_plat_single_write(st, REG_FIFO_EN_2, saved_regs.fifo_en_2);
 294         if (result)
 295                 return result;
 296         result = inv_plat_single_write(st, REG_FIFO_RST, MAX_5_BIT_VALUE);
 297         if (result)
 298                 return result;
 299         result = inv_plat_single_write(st, REG_FIFO_RST, saved_regs.fifo_rst);
 300         if (result)
 301                 return result;
 302
 303         /* Reset DMP */
 304         result = inv_plat_single_write(st, REG_USER_CTRL,
 305                         (saved_regs.user_ctrl & (~BIT_FIFO_EN)) | BIT_DMP_RST);
 306         if (result)
 307                 return result;
 308         msleep(DMP_RESET_TIME);
 309
 310         inv_set_dmp(st);
 311         inv_set_secondary(st);
 312         inv_mpu_setup_compass_slave(st);
 313         result = inv_set_power(st, false);
 314         return result;
 315 }
 316
 317 /**
 318 * inv_check_gyro_self_test() - check gyro self test. this function
 319 *                                   returns zero as success. A non-zero return
 320 *                                   value indicates failure in self test.
 321 *  @*st: main data structure.
 322 *  @*reg_avg: average value of normal test.
 323 *  @*st_avg:  average value of self test
 324 */
 325 static int inv_check_gyro_self_test(struct inv_mpu_state *st,
 326                                                 int *reg_avg, int *st_avg) {
 327         u8 *regs;
 328         int ret_val;
 329         int otp_value_zero = 0;
 330         int st_shift_prod[3], st_shift_cust[3], i;
 331
 332         ret_val = 0;
 333         regs = st->gyro_st_data;
 334         pr_debug("self_test gyro shift_code - %02x %02x %02x\n",
 335                                                 regs[0], regs[1], regs[2]);
 336
 337         for (i = 0; i < 3; i++) {
 338                 if (regs[i] != 0) {
 339                         st_shift_prod[i] = mpu_st_tb[regs[i] - 1];
 340                 } else {
 341                         st_shift_prod[i] = 0;
 342                         otp_value_zero = 1;
 343                 }
 344         }
 345         pr_debug("self_test gyro st_shift_prod - %+d %+d %+d\n",
 346                                         st_shift_prod[0], st_shift_prod[1],
 347                                                         st_shift_prod[2]);
 348
 349         for (i = 0; i < 3; i++) {
 350                 st_shift_cust[i] = st_avg[i] - reg_avg[i];
 351                 if (!otp_value_zero) {
 352                         /* Self Test Pass/Fail Criteria A */
 353                         if (st_shift_cust[i] < DEF_GYRO_CT_SHIFT_DELTA
 354                                                         * st_shift_prod[i])
 355                                         ret_val = 1;
 356                 } else {
 357                         /* Self Test Pass/Fail Criteria B */
 358                         if (st_shift_cust[i] < DEF_GYRO_ST_AL *
 359                                                 DEF_SELFTEST_GYRO_SENS *
 360                                                 DEF_ST_PRECISION)
 361                                 ret_val = 1;
 362                 }
 363         }
 364         pr_debug("self_test gyro st_shift_cust - %+d %+d %+d\n",
 365                                         st_shift_cust[0], st_shift_cust[1],
 366                                                         st_shift_cust[2]);
 367         if (ret_val == 0) {
 368                 /* Self Test Pass/Fail Criteria C */
 369                 for (i = 0; i < 3; i++)
 370                         if (abs(reg_avg[i]) > DEF_GYRO_OFFSET_MAX *
 371                                                 DEF_SELFTEST_GYRO_SENS *
 372                                                 DEF_ST_PRECISION)
 373                                 ret_val = 1;
 374         }
 375
 376         return ret_val;
 377 }
 378
 379 /**
 380 * inv_check_accel_self_test() - check accel self test. this function
 381 *                                   returns zero as success. A non-zero return
 382 *                                   value indicates failure in self test.
 383 *  @*st: main data structure.
 384 *  @*reg_avg: average value of normal test.
 385 *  @*st_avg:  average value of self test
 386 */
 387 static int inv_check_accel_self_test(struct inv_mpu_state *st,
 388                                                 int *reg_avg, int *st_avg)
 389 {
 390         int ret_val;
 391         int st_shift_prod[3], st_shift_cust[3], i;
 392         u8 *regs;
 393         int otp_value_zero = 0;
 394
 395 #define ACCEL_ST_AL_MIN ((DEF_ACCEL_ST_AL_MIN * DEF_ST_SCALE \
 396                                  / DEF_ST_ACCEL_FS_MG) * DEF_ST_PRECISION)
 397 #define ACCEL_ST_AL_MAX ((DEF_ACCEL_ST_AL_MAX * DEF_ST_SCALE \
 398                                  / DEF_ST_ACCEL_FS_MG) * DEF_ST_PRECISION)
 399
 400         ret_val = 0;
 401         regs = st->accel_st_data;
 402         pr_debug("self_test accel shift_code - %02x %02x %02x\n",
 403                                                 regs[0], regs[1], regs[2]);
 404
 405         for (i = 0; i < 3; i++) {
 406                 if (regs[i] != 0) {
 407                         st_shift_prod[i] = mpu_st_tb[regs[i] - 1];
 408                 } else {
 409                         st_shift_prod[i] = 0;
 410                         otp_value_zero = 1;
 411                 }
 412         }
 413         pr_debug("self_test accel st_shift_prod - %+d %+d %+d\n",
 414                                                         st_shift_prod[0],
 415                                                         st_shift_prod[1],
 416                                                         st_shift_prod[2]);
 417         if (!otp_value_zero) {
 418                 /* Self Test Pass/Fail Criteria A */
 419                 for (i = 0; i < 3; i++) {
 420                         st_shift_cust[i] = st_avg[i] - reg_avg[i];
 421                         if (st_shift_cust[i] < DEF_ACCEL_ST_SHIFT_DELTA_MIN
 422                                                         * st_shift_prod[i])
 423                                 ret_val = 1;
 424                         if (st_shift_cust[i] > DEF_ACCEL_ST_SHIFT_DELTA_MAX
 425                                         * st_shift_prod[i])
 426                                 ret_val = 1;
 427                 }
 428         } else {
 429                 /* Self Test Pass/Fail Criteria B */
 430                 for (i = 0; i < 3; i++) {
 431                         st_shift_cust[i] = abs(st_avg[i] - reg_avg[i]);
 432                         if ((st_shift_cust[i] < ACCEL_ST_AL_MIN) ||
 433                                         (st_shift_cust[i] > ACCEL_ST_AL_MAX))
 434                                 ret_val = 1;
 435                 }
 436         }
 437         pr_debug("self_test accel st_shift_cust - %+d %+d %+d\n",
 438                  st_shift_cust[0], st_shift_cust[1],
 439                  st_shift_cust[2]);
 440
 441         return ret_val;
 442 }
 443
 444 static int inv_setup_selftest(struct inv_mpu_state *st)
 445 {
 446         int result;
 447
 448         result = inv_set_bank(st, BANK_SEL_0);
 449         if (result)
 450                 return result;
 451
 452         /* Wake up */
 453         result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_CLK_PLL);
 454         if (result)
 455                 return result;
 456
 457         /* Save the current settings */
 458         result = inv_save_setting(st);
 459         if (result)
 460                 return result;
 461
 462         /* Stop sensors */
 463         result = inv_plat_single_write(st, REG_PWR_MGMT_2,
 464                         BIT_PWR_PRESSURE_STBY | BIT_PWR_ACCEL_STBY |
 465                         BIT_PWR_GYRO_STBY);
 466         if (result)
 467                 return result;
 468
 469         /* Perform a soft-reset of the chip by setting
 470          * the MSB of PWR_MGMT_1 register
 471          * This will clear any prior states in the chip
 472          */
 473         result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_H_RESET);
 474         if (result)
 475                 return result;
 476         msleep(POWER_UP_TIME);
 477
 478         /* Wake up */
 479         result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_CLK_PLL);
 480         if (result)
 481                 return result;
 482
 483         result = inv_plat_single_write(st, REG_USER_CTRL,
 484                         BIT_FIFO_EN | st->i2c_dis);
 485         if (result)
 486                 return result;
 487
 488         /* Configure FIFO */
 489         result = inv_plat_single_write(st, REG_FIFO_CFG, BIT_SINGLE_FIFO_CFG);
 490         if (result)
 491                 return result;
 492
 493         /* Set cycle mode */
 494         result = inv_plat_single_write(st, REG_LP_CONFIG,
 495                         BIT_I2C_MST_CYCLE | BIT_ACCEL_CYCLE | BIT_GYRO_CYCLE);
 496         if (result)
 497                 return result;
 498
 499         result = inv_set_bank(st, BANK_SEL_2);
 500         if (result)
 501                 return result;
 502
 503         /* Configure FSR and DLPF */
 504         result = inv_plat_single_write(st, REG_GYRO_SMPLRT_DIV,
 505                         SELFTEST_GYRO_SMPLRT_DIV);
 506         if (result)
 507                 return result;
 508         result = inv_plat_single_write(st, REG_GYRO_CONFIG_1, SELFTEST_GYRO_FS);
 509         if (result)
 510                 return result;
 511         result = inv_plat_single_write(st, REG_GYRO_CONFIG_2,
 512                         SELFTEST_GYRO_AVGCFG);
 513         if (result)
 514                 return result;
 515         result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_1, 0);
 516         if (result)
 517                 return result;
 518         result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_2,
 519                         SELFTEST_ACCEL_SMPLRT_DIV);
 520         if (result)
 521                 return result;
 522         result = inv_plat_single_write(st, REG_ACCEL_CONFIG, SELFTEST_ACCEL_FS);
 523         if (result)
 524                 return result;
 525         result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2,
 526                         SELFTEST_ACCEL_DEC3_CFG);
 527         if (result)
 528                 return result;
 529
 530         result = inv_set_bank(st, BANK_SEL_1);
 531         if (result)
 532                 return result;
 533
 534     /* Read selftest values */
 535         result = inv_plat_read(st, REG_SELF_TEST1, 1, &st->gyro_st_data[0]);
 536         if (result)
 537                 return result;
 538         result = inv_plat_read(st, REG_SELF_TEST2, 1, &st->gyro_st_data[1]);
 539         if (result)
 540                 return result;
 541         result = inv_plat_read(st, REG_SELF_TEST3, 1, &st->gyro_st_data[2]);
 542         if (result)
 543                 return result;
 544         result = inv_plat_read(st, REG_SELF_TEST4, 1, &st->accel_st_data[0]);
 545         if (result)
 546                 return result;
 547         result = inv_plat_read(st, REG_SELF_TEST5, 1, &st->accel_st_data[1]);
 548         if (result)
 549                 return result;
 550         result = inv_plat_read(st, REG_SELF_TEST6, 1, &st->accel_st_data[2]);
 551         if (result)
 552                 return result;
 553
 554         result = inv_set_bank(st, BANK_SEL_0);
 555
 556         /* Restart sensors */
 557         result = inv_plat_single_write(st, REG_PWR_MGMT_2,
 558                         BIT_PWR_PRESSURE_STBY);
 559         if (result)
 560                 return result;
 561         msleep(GYRO_ENGINE_UP_TIME);
 562
 563         return result;
 564 }
 565
 566 static int inv_selftest_read_samples(struct inv_mpu_state *st, enum INV_SENSORS
 567                                         type, int *sum_result, int *s)
 568 {
 569         u8 w;
 570         u16 fifo_count;
 571         s16 vals[3];
 572         u8 d[MAX_PACKETS * BYTES_PER_SENSOR];
 573         int r, i, j, t, packet_count;
 574
 575         r = inv_plat_single_write(st, REG_FIFO_EN_2, 0);
 576         if (r)
 577                 return r;
 578
 579         /* Reset FIFO */
 580         r = inv_plat_single_write(st, REG_FIFO_RST, 0x1F);
 581         if (r)
 582                 return r;
 583         r = inv_plat_single_write(st, REG_FIFO_RST, 0x1E);
 584         if (r)
 585                 return r;
 586
 587         if (SENSOR_GYRO == type)
 588                 w = BITS_GYRO_FIFO_EN;
 589         else
 590                 w = BIT_ACCEL_FIFO_EN;
 591
 592         while (*s < DEF_ST_SAMPLES) {
 593                 r = inv_plat_single_write(st, REG_FIFO_EN_2, w);
 594                 if (r)
 595                         return r;
 596
 597                 msleep(SELFTEST_WAIT_TIME);
 598
 599                 r = inv_plat_single_write(st, REG_FIFO_EN_2, 0);
 600                 if (r)
 601                         return r;
 602
 603                 r = inv_plat_read(st, REG_FIFO_COUNT_H, FIFO_COUNT_BYTE, d);
 604                 if (r)
 605                         return r;
 606                 fifo_count = be16_to_cpup((__be16 *)(&d[0]));
 607                 pr_debug("self_test fifo_count - %d\n",  fifo_count);
 608                 if (MAX_PACKETS * BYTES_PER_SENSOR < fifo_count) {
 609                         r = inv_plat_read(st, REG_FIFO_R_W,
 610                                         MAX_PACKETS * BYTES_PER_SENSOR, d);
 611                         packet_count = MAX_PACKETS;
 612                 } else {
 613                         r = inv_plat_read(st, REG_FIFO_R_W, fifo_count, d);
 614                         packet_count = fifo_count / BYTES_PER_SENSOR;
 615                 }
 616                 if (r)
 617                         return r;
 618
 619                 i = 0;
 620                 while (i < packet_count) {
 621                         for (j = 0; j < THREE_AXES; j++) {
 622                                 t = 2 * j + i * BYTES_PER_SENSOR;
 623                                 vals[j] = (s16)be16_to_cpup((__be16 *)(&d[t]));
 624                                 sum_result[j] += vals[j];
 625                         }
 626                         pr_debug("self_test data - %d %+d %+d %+d",
 627                                                 *s, vals[0], vals[1], vals[2]);
 628                         (*s)++;
 629                         i++;
 630                 }
 631         }
 632         return 0;
 633 }
 634
 635 /*
 636  *  inv_do_test_accel() - do the actual test of self testing
 637  */
 638 static int inv_do_test_accel(struct inv_mpu_state *st, int *accel_result,
 639                 int *accel_st_result)
 640 {
 641         int result, i, j;
 642         int accel_s;
 643         u8 w;
 644
 645         for (i = 0; i < THREE_AXES; i++) {
 646                 accel_result[i] = 0;
 647                 accel_st_result[i] = 0;
 648         }
 649
 650         accel_s = 0;
 651         result = inv_selftest_read_samples(st, SENSOR_ACCEL,
 652                         accel_result, &accel_s);
 653         if (result)
 654                 return result;
 655
 656         for (j = 0; j < THREE_AXES; j++) {
 657                 accel_result[j] = accel_result[j] / accel_s;
 658                 accel_result[j] *= DEF_ST_PRECISION;
 659         }
 660
 661         /* Set Self-Test Bit */
 662         result = inv_set_bank(st, BANK_SEL_2);
 663         if (result)
 664                 return result;
 665         w = BIT_ACCEL_CTEN | SELFTEST_ACCEL_DEC3_CFG;
 666         result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, w);
 667         if (result)
 668                 return result;
 669         result = inv_set_bank(st, BANK_SEL_0);
 670         msleep(DEF_ST_STABLE_TIME);
 671
 672         accel_s = 0;
 673         result = inv_selftest_read_samples(st, SENSOR_ACCEL,
 674                         accel_st_result, &accel_s);
 675         if (result)
 676                 return result;
 677
 678         for (j = 0; j < THREE_AXES; j++) {
 679                 accel_st_result[j] = accel_st_result[j] / accel_s;
 680                 accel_st_result[j] *= DEF_ST_PRECISION;
 681         }
 682
 683         pr_debug("accel=%d, %d, %d\n", accel_result[0], accel_result[1],
 684                                                         accel_result[2]);
 685
 686         return 0;
 687 }
 688 /*
 689  *  inv_do_test() - do the actual test of self testing
 690  */
 691 static int inv_do_test_gyro(struct inv_mpu_state *st, int *gyro_result,
 692                 int *gyro_st_result)
 693 {
 694         int result, i, j;
 695         int gyro_s;
 696         u8 w;
 697
 698         for (i = 0; i < THREE_AXES; i++) {
 699                 gyro_result[i] = 0;
 700                 gyro_st_result[i] = 0;
 701         }
 702
 703         gyro_s = 0;
 704         result = inv_selftest_read_samples(st, SENSOR_GYRO,
 705                         gyro_result, &gyro_s);
 706         if (result)
 707                 return result;
 708
 709         for (j = 0; j < THREE_AXES; j++) {
 710                 gyro_result[j] = gyro_result[j] / gyro_s;
 711                 gyro_result[j] *= DEF_ST_PRECISION;
 712         }
 713
 714         /* Set Self-Test Bit */
 715         result = inv_set_bank(st, BANK_SEL_2);
 716         if (result)
 717                 return result;
 718         w = BIT_GYRO_CTEN | SELFTEST_GYRO_AVGCFG;
 719         result = inv_plat_single_write(st, REG_GYRO_CONFIG_2, w);
 720         if (result)
 721                 return result;
 722         result = inv_set_bank(st, BANK_SEL_0);
 723         msleep(DEF_ST_STABLE_TIME);
 724
 725         gyro_s = 0;
 726         result = inv_selftest_read_samples(st, SENSOR_GYRO,
 727                         gyro_st_result, &gyro_s);
 728         if (result)
 729                 return result;
 730
 731         for (j = 0; j < THREE_AXES; j++) {
 732                 gyro_st_result[j] = gyro_st_result[j] / gyro_s;
 733                 gyro_st_result[j] *= DEF_ST_PRECISION;
 734         }
 735
 736         pr_debug("gyro=%d, %d, %d\n", gyro_result[0], gyro_result[1],
 737                                                         gyro_result[2]);
 738
 739         return 0;
 740 }
 741
 742 /*
 743  *  inv_hw_self_test() - main function to do hardware self test
 744  */
 745 int inv_hw_self_test(struct inv_mpu_state *st)
 746 {
 747         int result;
 748         int gyro_bias_st[THREE_AXES], gyro_bias_regular[THREE_AXES];
 749         int accel_bias_st[THREE_AXES], accel_bias_regular[THREE_AXES];
 750         int test_times, i;
 751         char accel_result, gyro_result, compass_result;
 752
 753         accel_result = 0;
 754         gyro_result = 0;
 755         compass_result = 0;
 756         test_times = DEF_ST_TRY_TIMES;
 757         result = inv_setup_selftest(st);
 758         if (result)
 759                 goto test_fail;
 760         while (test_times > 0) {
 761                 result = inv_do_test_gyro(st, gyro_bias_regular,
 762                                 gyro_bias_st);
 763                 if (result == -EAGAIN)
 764                         test_times--;
 765                 else
 766                         test_times = 0;
 767         }
 768         if (result)
 769                 goto test_fail;
 770         pr_debug("self_test gyro bias_regular - %+d %+d %+d\n",
 771                                                 gyro_bias_regular[0],
 772                                                 gyro_bias_regular[1],
 773                                                 gyro_bias_regular[2]);
 774         pr_debug("self_test gyro bias_st - %+d %+d %+d\n",
 775                                                 gyro_bias_st[0],
 776                                                 gyro_bias_st[1],
 777                                                 gyro_bias_st[2]);
 778
 779         test_times = DEF_ST_TRY_TIMES;
 780         while (test_times > 0) {
 781                 result = inv_do_test_accel(st, accel_bias_regular,
 782                                 accel_bias_st);
 783                 if (result == -EAGAIN)
 784                         test_times--;
 785                 else
 786                         break;
 787         }
 788         if (result)
 789                 goto test_fail;
 790         pr_debug("self_test accel bias_regular - %+d %+d %+d\n",
 791                                                 accel_bias_regular[0],
 792                                                 accel_bias_regular[1],
 793                                                 accel_bias_regular[2]);
 794         pr_debug("self_test accel bias_st - %+d %+d %+d\n",
 795                                                 accel_bias_st[0],
 796                                                 accel_bias_st[1],
 797                                                 accel_bias_st[2]);
 798
 799         for (i = 0; i < 3; i++) {
 800                 st->gyro_bias[i] = gyro_bias_regular[i] / DEF_ST_PRECISION;
 801                 st->accel_bias[i] = accel_bias_regular[i] / DEF_ST_PRECISION;
 802         }
 803
 804         accel_result = !inv_check_accel_self_test(st,
 805                                 accel_bias_regular, accel_bias_st);
 806         gyro_result = !inv_check_gyro_self_test(st,
 807                                 gyro_bias_regular, gyro_bias_st);
 808         if (st->chip_config.has_compass)
 809                 compass_result = !st->slave_compass->self_test(st);
 810
 811 test_fail:
 812         inv_recover_setting(st);
 813         return (compass_result << DEF_ST_COMPASS_RESULT_SHIFT) |
 814                 (accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result;
 815 }