2 * Copyright (C) 2012 Invensense, Inc.
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.
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.
13 #define pr_fmt(fmt) "inv_mpu: " fmt
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>
29 #include "inv_mpu_iio.h"
31 /* full scale and LPF setting */
32 #define SELFTEST_GYRO_FS ((0 << 3) | 1)
33 #define SELFTEST_ACCEL_FS ((7 << 3) | 1)
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
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
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
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
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 */
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 */
88 static struct recover_regs saved_regs;
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
126 static void inv_show_saved_setting(struct inv_mpu_state *st)
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);
147 static int inv_save_setting(struct inv_mpu_state *st)
151 result = inv_set_bank(st, BANK_SEL_0);
155 result = inv_plat_read(st, REG_FIFO_CFG, 1, &saved_regs.fifo_cfg);
158 result = inv_plat_read(st, REG_USER_CTRL, 1, &saved_regs.user_ctrl);
161 result = inv_plat_read(st, REG_LP_CONFIG, 1, &saved_regs.lp_config);
164 result = inv_plat_read(st, REG_INT_ENABLE, 1, &saved_regs.int_enable);
167 result = inv_plat_read(st, REG_INT_ENABLE_1, 1,
168 &saved_regs.int_enable_1);
171 result = inv_plat_read(st, REG_FIFO_EN, 1, &saved_regs.fifo_en);
174 result = inv_plat_read(st, REG_FIFO_EN_2, 1, &saved_regs.fifo_en_2);
177 result = inv_plat_read(st, REG_FIFO_RST, 1, &saved_regs.fifo_rst);
181 result = inv_set_bank(st, BANK_SEL_2);
185 result = inv_plat_read(st, REG_GYRO_SMPLRT_DIV, 1,
186 &saved_regs.gyro_smplrt_div);
189 result = inv_plat_read(st, REG_GYRO_CONFIG_1, 1,
190 &saved_regs.gyro_config_1);
193 result = inv_plat_read(st, REG_GYRO_CONFIG_2, 1,
194 &saved_regs.gyro_config_2);
197 result = inv_plat_read(st, REG_ACCEL_SMPLRT_DIV_1, 1,
198 &saved_regs.accel_smplrt_div_1);
201 result = inv_plat_read(st, REG_ACCEL_SMPLRT_DIV_2, 1,
202 &saved_regs.accel_smplrt_div_2);
205 result = inv_plat_read(st, REG_ACCEL_CONFIG, 1,
206 &saved_regs.accel_config);
209 result = inv_plat_read(st, REG_ACCEL_CONFIG_2, 1,
210 &saved_regs.accel_config_2);
214 result = inv_set_bank(st, BANK_SEL_0);
218 inv_show_saved_setting(st);
223 static int inv_recover_setting(struct inv_mpu_state *st)
227 result = inv_set_bank(st, BANK_SEL_0);
232 result = inv_plat_single_write(st, REG_PWR_MGMT_2,
233 BIT_PWR_PRESSURE_STBY | BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY);
237 result = inv_set_bank(st, BANK_SEL_2);
241 /* Restore sensor configurations */
242 result = inv_plat_single_write(st, REG_GYRO_SMPLRT_DIV,
243 saved_regs.gyro_smplrt_div);
246 result = inv_plat_single_write(st, REG_GYRO_CONFIG_1,
247 saved_regs.gyro_config_1);
250 result = inv_plat_single_write(st, REG_GYRO_CONFIG_2,
251 saved_regs.gyro_config_2);
254 result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_1,
255 saved_regs.accel_smplrt_div_1);
258 result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_2,
259 saved_regs.accel_smplrt_div_2);
262 result = inv_plat_single_write(st, REG_ACCEL_CONFIG,
263 saved_regs.accel_config);
266 result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2,
267 saved_regs.accel_config_2);
271 result = inv_set_bank(st, BANK_SEL_0);
275 /* Restore FIFO configurations */
276 result = inv_plat_single_write(st, REG_FIFO_CFG, saved_regs.fifo_cfg);
279 result = inv_plat_single_write(st, REG_LP_CONFIG, saved_regs.lp_config);
282 result = inv_plat_single_write(st, REG_INT_ENABLE,
283 saved_regs.int_enable);
286 result = inv_plat_single_write(st, REG_INT_ENABLE_1,
287 saved_regs.int_enable_1);
290 result = inv_plat_single_write(st, REG_FIFO_EN, saved_regs.fifo_en);
293 result = inv_plat_single_write(st, REG_FIFO_EN_2, saved_regs.fifo_en_2);
296 result = inv_plat_single_write(st, REG_FIFO_RST, MAX_5_BIT_VALUE);
299 result = inv_plat_single_write(st, REG_FIFO_RST, saved_regs.fifo_rst);
304 result = inv_plat_single_write(st, REG_USER_CTRL,
305 (saved_regs.user_ctrl & (~BIT_FIFO_EN)) | BIT_DMP_RST);
308 msleep(DMP_RESET_TIME);
311 inv_set_secondary(st);
312 inv_mpu_setup_compass_slave(st);
313 result = inv_set_power(st, false);
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
325 static int inv_check_gyro_self_test(struct inv_mpu_state *st,
326 int *reg_avg, int *st_avg) {
329 int otp_value_zero = 0;
330 int st_shift_prod[3], st_shift_cust[3], i;
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]);
337 for (i = 0; i < 3; i++) {
339 st_shift_prod[i] = mpu_st_tb[regs[i] - 1];
341 st_shift_prod[i] = 0;
345 pr_debug("self_test gyro st_shift_prod - %+d %+d %+d\n",
346 st_shift_prod[0], st_shift_prod[1],
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
357 /* Self Test Pass/Fail Criteria B */
358 if (st_shift_cust[i] < DEF_GYRO_ST_AL *
359 DEF_SELFTEST_GYRO_SENS *
364 pr_debug("self_test gyro st_shift_cust - %+d %+d %+d\n",
365 st_shift_cust[0], st_shift_cust[1],
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 *
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
387 static int inv_check_accel_self_test(struct inv_mpu_state *st,
388 int *reg_avg, int *st_avg)
391 int st_shift_prod[3], st_shift_cust[3], i;
393 int otp_value_zero = 0;
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)
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]);
405 for (i = 0; i < 3; i++) {
407 st_shift_prod[i] = mpu_st_tb[regs[i] - 1];
409 st_shift_prod[i] = 0;
413 pr_debug("self_test accel st_shift_prod - %+d %+d %+d\n",
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
424 if (st_shift_cust[i] > DEF_ACCEL_ST_SHIFT_DELTA_MAX
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))
437 pr_debug("self_test accel st_shift_cust - %+d %+d %+d\n",
438 st_shift_cust[0], st_shift_cust[1],
444 static int inv_setup_selftest(struct inv_mpu_state *st)
448 result = inv_set_bank(st, BANK_SEL_0);
453 result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_CLK_PLL);
457 /* Save the current settings */
458 result = inv_save_setting(st);
463 result = inv_plat_single_write(st, REG_PWR_MGMT_2,
464 BIT_PWR_PRESSURE_STBY | BIT_PWR_ACCEL_STBY |
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
473 result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_H_RESET);
476 msleep(POWER_UP_TIME);
479 result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_CLK_PLL);
483 result = inv_plat_single_write(st, REG_USER_CTRL,
484 BIT_FIFO_EN | st->i2c_dis);
489 result = inv_plat_single_write(st, REG_FIFO_CFG, BIT_SINGLE_FIFO_CFG);
494 result = inv_plat_single_write(st, REG_LP_CONFIG,
495 BIT_I2C_MST_CYCLE | BIT_ACCEL_CYCLE | BIT_GYRO_CYCLE);
499 result = inv_set_bank(st, BANK_SEL_2);
503 /* Configure FSR and DLPF */
504 result = inv_plat_single_write(st, REG_GYRO_SMPLRT_DIV,
505 SELFTEST_GYRO_SMPLRT_DIV);
508 result = inv_plat_single_write(st, REG_GYRO_CONFIG_1, SELFTEST_GYRO_FS);
511 result = inv_plat_single_write(st, REG_GYRO_CONFIG_2,
512 SELFTEST_GYRO_AVGCFG);
515 result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_1, 0);
518 result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_2,
519 SELFTEST_ACCEL_SMPLRT_DIV);
522 result = inv_plat_single_write(st, REG_ACCEL_CONFIG, SELFTEST_ACCEL_FS);
525 result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2,
526 SELFTEST_ACCEL_DEC3_CFG);
530 result = inv_set_bank(st, BANK_SEL_1);
534 /* Read selftest values */
535 result = inv_plat_read(st, REG_SELF_TEST1, 1, &st->gyro_st_data[0]);
538 result = inv_plat_read(st, REG_SELF_TEST2, 1, &st->gyro_st_data[1]);
541 result = inv_plat_read(st, REG_SELF_TEST3, 1, &st->gyro_st_data[2]);
544 result = inv_plat_read(st, REG_SELF_TEST4, 1, &st->accel_st_data[0]);
547 result = inv_plat_read(st, REG_SELF_TEST5, 1, &st->accel_st_data[1]);
550 result = inv_plat_read(st, REG_SELF_TEST6, 1, &st->accel_st_data[2]);
554 result = inv_set_bank(st, BANK_SEL_0);
556 /* Restart sensors */
557 result = inv_plat_single_write(st, REG_PWR_MGMT_2,
558 BIT_PWR_PRESSURE_STBY);
561 msleep(GYRO_ENGINE_UP_TIME);
566 static int inv_selftest_read_samples(struct inv_mpu_state *st, enum INV_SENSORS
567 type, int *sum_result, int *s)
572 u8 d[MAX_PACKETS * BYTES_PER_SENSOR];
573 int r, i, j, t, packet_count;
575 r = inv_plat_single_write(st, REG_FIFO_EN_2, 0);
580 r = inv_plat_single_write(st, REG_FIFO_RST, 0x1F);
583 r = inv_plat_single_write(st, REG_FIFO_RST, 0x1E);
587 if (SENSOR_GYRO == type)
588 w = BITS_GYRO_FIFO_EN;
590 w = BIT_ACCEL_FIFO_EN;
592 while (*s < DEF_ST_SAMPLES) {
593 r = inv_plat_single_write(st, REG_FIFO_EN_2, w);
597 msleep(SELFTEST_WAIT_TIME);
599 r = inv_plat_single_write(st, REG_FIFO_EN_2, 0);
603 r = inv_plat_read(st, REG_FIFO_COUNT_H, FIFO_COUNT_BYTE, d);
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;
613 r = inv_plat_read(st, REG_FIFO_R_W, fifo_count, d);
614 packet_count = fifo_count / BYTES_PER_SENSOR;
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];
626 pr_debug("self_test data - %d %+d %+d %+d",
627 *s, vals[0], vals[1], vals[2]);
636 * inv_do_test_accel() - do the actual test of self testing
638 static int inv_do_test_accel(struct inv_mpu_state *st, int *accel_result,
639 int *accel_st_result)
645 for (i = 0; i < THREE_AXES; i++) {
647 accel_st_result[i] = 0;
651 result = inv_selftest_read_samples(st, SENSOR_ACCEL,
652 accel_result, &accel_s);
656 for (j = 0; j < THREE_AXES; j++) {
657 accel_result[j] = accel_result[j] / accel_s;
658 accel_result[j] *= DEF_ST_PRECISION;
661 /* Set Self-Test Bit */
662 result = inv_set_bank(st, BANK_SEL_2);
665 w = BIT_ACCEL_CTEN | SELFTEST_ACCEL_DEC3_CFG;
666 result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, w);
669 result = inv_set_bank(st, BANK_SEL_0);
670 msleep(DEF_ST_STABLE_TIME);
673 result = inv_selftest_read_samples(st, SENSOR_ACCEL,
674 accel_st_result, &accel_s);
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;
683 pr_debug("accel=%d, %d, %d\n", accel_result[0], accel_result[1],
689 * inv_do_test() - do the actual test of self testing
691 static int inv_do_test_gyro(struct inv_mpu_state *st, int *gyro_result,
698 for (i = 0; i < THREE_AXES; i++) {
700 gyro_st_result[i] = 0;
704 result = inv_selftest_read_samples(st, SENSOR_GYRO,
705 gyro_result, &gyro_s);
709 for (j = 0; j < THREE_AXES; j++) {
710 gyro_result[j] = gyro_result[j] / gyro_s;
711 gyro_result[j] *= DEF_ST_PRECISION;
714 /* Set Self-Test Bit */
715 result = inv_set_bank(st, BANK_SEL_2);
718 w = BIT_GYRO_CTEN | SELFTEST_GYRO_AVGCFG;
719 result = inv_plat_single_write(st, REG_GYRO_CONFIG_2, w);
722 result = inv_set_bank(st, BANK_SEL_0);
723 msleep(DEF_ST_STABLE_TIME);
726 result = inv_selftest_read_samples(st, SENSOR_GYRO,
727 gyro_st_result, &gyro_s);
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;
736 pr_debug("gyro=%d, %d, %d\n", gyro_result[0], gyro_result[1],
743 * inv_hw_self_test() - main function to do hardware self test
745 int inv_hw_self_test(struct inv_mpu_state *st)
748 int gyro_bias_st[THREE_AXES], gyro_bias_regular[THREE_AXES];
749 int accel_bias_st[THREE_AXES], accel_bias_regular[THREE_AXES];
751 char accel_result, gyro_result, compass_result;
756 test_times = DEF_ST_TRY_TIMES;
757 result = inv_setup_selftest(st);
760 while (test_times > 0) {
761 result = inv_do_test_gyro(st, gyro_bias_regular,
763 if (result == -EAGAIN)
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",
779 test_times = DEF_ST_TRY_TIMES;
780 while (test_times > 0) {
781 result = inv_do_test_accel(st, accel_bias_regular,
783 if (result == -EAGAIN)
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",
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;
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);
812 inv_recover_setting(st);
813 return (compass_result << DEF_ST_COMPASS_RESULT_SHIFT) |
814 (accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result;