input: misc: Invensense MPU 5.0.1 driver
[linux-2.6.git] / drivers / input / misc / mpu / inv_gyro_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 /**
16  *  @addtogroup  DRIVERS
17  *  @brief       Hardware drivers.
18  *
19  *  @{
20  *      @file    inv_gyro_misc.c
21  *      @brief   A sysfs device driver for Invensense gyroscopes.
22  *      @details This file is part of inv_gyro driver code
23  */
24
25 #include <linux/module.h>
26 #include <linux/init.h>
27 #include <linux/slab.h>
28 #include <linux/i2c.h>
29 #include <linux/err.h>
30 #include <linux/delay.h>
31 #include <linux/sysfs.h>
32 #include <linux/jiffies.h>
33 #include <linux/irq.h>
34 #include <linux/interrupt.h>
35 #include <linux/kfifo.h>
36 #include <linux/poll.h>
37 #include <linux/miscdevice.h>
38
39 #include "inv_gyro.h"
40
41 /*
42     Defines
43 */
44
45 /*--- Test parameters defaults --- */
46
47 #define DEF_OLDEST_SUPP_PROD_REV    (8)
48 #define DEF_OLDEST_SUPP_SW_REV      (2)
49
50 /* sample rate */
51 #define DEF_SELFTEST_SAMPLE_RATE             (0)
52 /* LPF parameter */
53 #define DEF_SELFTEST_LPF_PARA                (1)
54 /* full scale setting dps */
55 #define DEF_SELFTEST_GYRO_FULL_SCALE         (0 << 3)
56 #define DEF_SELFTEST_ACCL_FULL_SCALE         (2 << 3)
57 /* wait time before collecting data */
58 #define DEF_GYRO_WAIT_TIME          (50)
59 #define DEF_GYRO_PACKET_THRESH      DEF_GYRO_WAIT_TIME
60 #define DEF_GYRO_THRESH             (10)
61 #define DEF_GYRO_PRECISION          (1000)
62 #define X                           0
63 #define Y                           1
64 #define Z                           2
65 /*---- MPU6050 notable product revisions ----*/
66 #define MPU_PRODUCT_KEY_B1_E1_5      105
67 #define MPU_PRODUCT_KEY_B2_F1        431
68 /* accelerometer Hw self test min and max bias shift (mg) */
69 #define DEF_ACCEL_ST_SHIFT_MIN      (300)
70 #define DEF_ACCEL_ST_SHIFT_MAX      (950)
71
72 #define DEF_ACCEL_ST_SHIFT_DELTA    (140)
73 #define DEF_GYRO_CT_SHIFT_DELTA     (140)
74 /* gyroscope Coriolis self test min and max bias shift (dps) */
75 #define DEF_GYRO_CT_SHIFT_MIN       (10)
76 #define DEF_GYRO_CT_SHIFT_MAX       (105)
77
78 static struct test_setup_t test_setup = {
79         .gyro_sens   = 32768 / 250,
80         .sample_rate = DEF_SELFTEST_SAMPLE_RATE,
81         .lpf         = DEF_SELFTEST_LPF_PARA,
82         .fsr         = DEF_SELFTEST_GYRO_FULL_SCALE,
83         .accl_fs     = DEF_SELFTEST_ACCL_FULL_SCALE
84 };
85
86 /* NOTE: product entries are in chronological order */
87 static struct prod_rev_map_t prod_rev_map[] = {
88         /* prod_ver = 0 */
89         {MPL_PROD_KEY(0,   1), MPU_SILICON_REV_A2, 131, 16384},
90         {MPL_PROD_KEY(0,   2), MPU_SILICON_REV_A2, 131, 16384},
91         {MPL_PROD_KEY(0,   3), MPU_SILICON_REV_A2, 131, 16384},
92         {MPL_PROD_KEY(0,   4), MPU_SILICON_REV_A2, 131, 16384},
93         {MPL_PROD_KEY(0,   5), MPU_SILICON_REV_A2, 131, 16384},
94         {MPL_PROD_KEY(0,   6), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/C2-1) */
95         /* prod_ver = 1, forced to 0 for MPU6050 A2 */
96         {MPL_PROD_KEY(0,   7), MPU_SILICON_REV_A2, 131, 16384},
97         {MPL_PROD_KEY(0,   8), MPU_SILICON_REV_A2, 131, 16384},
98         {MPL_PROD_KEY(0,   9), MPU_SILICON_REV_A2, 131, 16384},
99         {MPL_PROD_KEY(0,  10), MPU_SILICON_REV_A2, 131, 16384},
100         {MPL_PROD_KEY(0,  11), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/D2-1) */
101         {MPL_PROD_KEY(0,  12), MPU_SILICON_REV_A2, 131, 16384},
102         {MPL_PROD_KEY(0,  13), MPU_SILICON_REV_A2, 131, 16384},
103         {MPL_PROD_KEY(0,  14), MPU_SILICON_REV_A2, 131, 16384},
104         {MPL_PROD_KEY(0,  15), MPU_SILICON_REV_A2, 131, 16384},
105         {MPL_PROD_KEY(0,  27), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/D4)   */
106         /* prod_ver = 1 */
107         {MPL_PROD_KEY(1,  16), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-1) */
108         {MPL_PROD_KEY(1,  17), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-2) */
109         {MPL_PROD_KEY(1,  18), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-3) */
110         {MPL_PROD_KEY(1,  19), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-4) */
111         {MPL_PROD_KEY(1,  20), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-5) */
112         {MPL_PROD_KEY(1,  28), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D4)   */
113         {MPL_PROD_KEY(1,   1), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-1) */
114         {MPL_PROD_KEY(1,   2), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-2) */
115         {MPL_PROD_KEY(1,   3), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-3) */
116         {MPL_PROD_KEY(1,   4), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-4) */
117         {MPL_PROD_KEY(1,   5), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-5) */
118         {MPL_PROD_KEY(1,   6), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-6) */
119         /* prod_ver = 2 */
120         {MPL_PROD_KEY(2,   7), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-1) */
121         {MPL_PROD_KEY(2,   8), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-2) */
122         {MPL_PROD_KEY(2,   9), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-3) */
123         {MPL_PROD_KEY(2,  10), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-4) */
124         {MPL_PROD_KEY(2,  11), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-5) */
125         {MPL_PROD_KEY(2,  12), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-6) */
126         {MPL_PROD_KEY(2,  29), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/D4)   */
127         /* prod_ver = 3 */
128         {MPL_PROD_KEY(3,  30), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E2)   */
129         /* prod_ver = 4 */
130         {MPL_PROD_KEY(4,  31), MPU_SILICON_REV_B1, 131,  8192}, /* (B2/F1)   */
131         {MPL_PROD_KEY(4,   1), MPU_SILICON_REV_B1, 131,  8192}, /* (B3/F1)   */
132         {MPL_PROD_KEY(4,   3), MPU_SILICON_REV_B1, 131,  8192}, /* (B4/F1)   */
133         /* prod_ver = 5 */
134         {MPL_PROD_KEY(5,   3), MPU_SILICON_REV_B1, 131, 16384}, /* (B4/F1)   */
135         /* prod_ver = 6 */
136         {MPL_PROD_KEY(6,  19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2)   */
137         /* prod_ver = 7 */
138         {MPL_PROD_KEY(7,  19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2)   */
139         /* prod_ver = 8 */
140         {MPL_PROD_KEY(8,  19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2)   */
141         /* prod_ver = 9 */
142         {MPL_PROD_KEY(9,  19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2)   */
143         /* prod_ver = 10 */
144         {MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384}  /* (B5/E2)   */
145 };
146
147 /*
148    List of product software revisions
149
150    NOTE :
151    software revision 0 falls back to the old detection method
152    based off the product version and product revision per the
153    table above
154 */
155 static struct prod_rev_map_t sw_rev_map[] = {
156         {0,                  0,   0,     0},
157         {1, MPU_SILICON_REV_B1, 131,  8192},    /* rev C */
158         {2, MPU_SILICON_REV_B1, 131, 16384}     /* rev D */
159 };
160
161 static int accl_st_tb[31] = {
162         340, 351, 363, 375, 388, 401, 414, 428,
163         443, 458, 473, 489, 506, 523, 541, 559,
164         578, 597, 617, 638, 660, 682, 705, 729,
165         753, 779, 805, 832, 860, 889, 919};
166
167 static int gyro_6050_st_tb[31] = {
168         3275, 3425, 3583, 3748, 3920, 4100, 4289, 4486,
169         4693, 4909, 5134, 5371, 5618, 5876, 6146, 6429,
170         6725, 7034, 7358, 7696, 8050, 8421, 8808, 9213,
171         9637, 10080, 10544, 11029, 11537, 12067, 12622};
172
173 static int gyro_3500_st_tb[255] = {
174         2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
175         2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
176         3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
177         3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
178         3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
179         3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
180         4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
181         4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
182         4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
183         5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
184         5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
185         6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
186         6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
187         7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
188         7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
189         8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
190         9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
191         10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
192         10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
193         11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
194         12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
195         13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
196         15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
197         16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
198         17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
199         19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
200         20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
201         22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
202         24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
203         26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
204         28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
205         30903, 31212, 31524, 31839, 32157, 32479, 32804};
206
207 int mpu_memory_write(struct i2c_adapter *i2c_adap,
208                      unsigned char mpu_addr,
209                      unsigned short mem_addr,
210                      unsigned int len, unsigned char const *data)
211 {
212         unsigned char bank[2];
213         unsigned char addr[2];
214         unsigned char buf[513];
215         struct i2c_msg msgs[3];
216         int res;
217
218         if (!data || !i2c_adap)
219                 return -EINVAL;
220
221         if (len >= (sizeof(buf) - 1))
222                 return -ENOMEM;
223
224         bank[0] = REG_BANK_SEL;
225         bank[1] = mem_addr >> 8;
226         addr[0] = REG_MEM_START;
227         addr[1] = mem_addr & 0xFF;
228         buf[0] = REG_MEM_RW;
229         memcpy(buf + 1, data, len);
230         /* write message */
231         msgs[0].addr = mpu_addr;
232         msgs[0].flags = 0;
233         msgs[0].buf = bank;
234         msgs[0].len = sizeof(bank);
235         msgs[1].addr = mpu_addr;
236         msgs[1].flags = 0;
237         msgs[1].buf = addr;
238         msgs[1].len = sizeof(addr);
239         msgs[2].addr = mpu_addr;
240         msgs[2].flags = 0;
241         msgs[2].buf = (unsigned char *)buf;
242         msgs[2].len = len + 1;
243         res = i2c_transfer(i2c_adap, msgs, 3);
244         if (res != 3) {
245                 if (res >= 0)
246                         res = -EIO;
247                 return res;
248         }
249
250         return 0;
251 }
252
253 int mpu_memory_read(struct i2c_adapter *i2c_adap,
254                     unsigned char mpu_addr,
255                     unsigned short mem_addr,
256                     unsigned int len, unsigned char *data)
257 {
258         unsigned char bank[2];
259         unsigned char addr[2];
260         unsigned char buf;
261         struct i2c_msg msgs[4];
262         int res;
263
264         if (!data || !i2c_adap)
265                 return -EINVAL;
266
267         bank[0] = REG_BANK_SEL;
268         bank[1] = mem_addr >> 8;
269         addr[0] = REG_MEM_START;
270         addr[1] = mem_addr & 0xFF;
271         buf = REG_MEM_RW;
272         /* write message */
273         msgs[0].addr = mpu_addr;
274         msgs[0].flags = 0;
275         msgs[0].buf = bank;
276         msgs[0].len = sizeof(bank);
277         msgs[1].addr = mpu_addr;
278         msgs[1].flags = 0;
279         msgs[1].buf = addr;
280         msgs[1].len = sizeof(addr);
281         msgs[2].addr = mpu_addr;
282         msgs[2].flags = 0;
283         msgs[2].buf = &buf;
284         msgs[2].len = 1;
285         msgs[3].addr = mpu_addr;
286         msgs[3].flags = I2C_M_RD;
287         msgs[3].buf = data;
288         msgs[3].len = len;
289         res = i2c_transfer(i2c_adap, msgs, 4);
290         if (res != 4) {
291                 if (res >= 0)
292                         res = -EIO;
293                 return res;
294         }
295
296         return 0;
297 }
298
299 /**
300  *  @internal
301  *  @brief  Inverse lookup of the index of an MPL product key .
302  *  @param  key
303  *              the MPL product indentifier also referred to as 'key'.
304  *  @return the index position of the key in the array, -1 if not found.
305  */
306 static short index_of_key(unsigned short key)
307 {
308         int i;
309
310         for (i = 0; i < NUM_OF_PROD_REVS; i++)
311                 if (prod_rev_map[i].mpl_product_key == key)
312                         return (short)i;
313
314         return -1;
315 }
316
317 int inv_get_silicon_rev_mpu6050(struct inv_gyro_state_s *st)
318 {
319         int result;
320         struct inv_reg_map_s *reg;
321         unsigned char prod_ver = 0x00, prod_rev = 0x00;
322         struct prod_rev_map_t *p_rev;
323         unsigned char bank =
324             (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
325         unsigned short mem_addr = ((bank << 8) | 0x06);
326         unsigned short key;
327         unsigned char regs[5];
328         unsigned short sw_rev;
329         short index;
330         struct inv_chip_info_s *chip_info = &st->chip_info;
331
332         reg = st->reg;
333         result = inv_i2c_read(st, reg->product_id, 1, &prod_ver);
334         if (result)
335                 return result;
336
337         prod_ver &= 0xf;
338         result = mpu_memory_read(st->sl_handle, st->i2c_addr, mem_addr,
339                         1, &prod_rev);
340         mdelay(100);
341         result = mpu_memory_read(st->sl_handle, st->i2c_addr, mem_addr,
342                         1, &prod_rev);
343         if (result)
344                 return result;
345
346         prod_rev >>= 2;
347         /* clean the prefetch and cfg user bank bits */
348         result = inv_i2c_single_write(st, reg->bank_sel, 0);
349         if (result)
350                 return result;
351
352         /* get the software-product version, read from XA_OFFS_L */
353         result = inv_i2c_read(st, 0x7, 5, regs);
354         if (result)
355                 return result;
356
357         sw_rev = (regs[4] & 0x01) << 2 |        /* 0x0b, bit 0 */
358                  (regs[2] & 0x01) << 1 |        /* 0x09, bit 0 */
359                  (regs[0] & 0x01);              /* 0x07, bit 0 */
360         /* if 0, use the product key to determine the type of part */
361         if (sw_rev == 0) {
362                 key = MPL_PROD_KEY(prod_ver, prod_rev);
363                 if (key == 0)
364                         return -1;
365
366                 index = index_of_key(key);
367                 if (index == -1 || index >= NUM_OF_PROD_REVS)
368                         return -1;
369
370                 /* check MPL is compiled for this device */
371                 if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1)
372                         return -1;
373
374                 p_rev = &prod_rev_map[index];
375         /* if valid, use the software product key */
376         } else if (sw_rev < ARRAY_SIZE(sw_rev_map)) {
377                 p_rev = &sw_rev_map[sw_rev];
378         } else {
379                 return -1;
380         }
381
382         chip_info->product_id = prod_ver;
383         chip_info->product_revision = prod_rev;
384         chip_info->silicon_revision = p_rev->silicon_rev;
385         chip_info->software_revision = sw_rev;
386         chip_info->gyro_sens_trim = p_rev->gyro_trim;
387         chip_info->accl_sens_trim = p_rev->accel_trim;
388         if (chip_info->accl_sens_trim == 0)
389                 chip_info->accl_sens_trim = DEFAULT_ACCL_TRIM;
390         chip_info->multi = DEFAULT_ACCL_TRIM/chip_info->accl_sens_trim;
391         if (chip_info->multi != 1)
392                 printk(KERN_ERR"multi is %d\n", chip_info->multi);
393         return result;
394 }
395
396 /**
397  *  @internal
398  *  @brief  read the accelerometer hardware self-test bias shift calculated
399  *          during final production test and stored in chip non-volatile memory.
400  *  @param  mlsl_handle
401  *              serial interface handle to allow serial communication with the
402  *              device, both gyro and accelerometer.
403  *  @param  ct_shift_prod
404  *              A pointer to an array of 3 float elements to hold the values
405  *              for production hardware self-test bias shifts returned to the
406  *              user.
407  *  @return INV_SUCCESS on success, or a non-zero error code otherwise.
408  */
409 static int read_accel_hw_self_test_prod_shift(struct inv_gyro_state_s *st,
410                                         int *st_prod)
411 {
412         unsigned char regs[4];
413         unsigned char shift_code[3];
414         int result, i;
415         st_prod[0] = st_prod[1] = st_prod[2] = 0;
416
417         result = inv_i2c_read(st, 0x0d, 4, regs);
418         if (result)
419                 return result;
420
421         if ((0 == regs[0])  && (0 == regs[1]) &&
422                 (0 == regs[2]) && (0 == regs[3]))
423                 return -1;
424
425         shift_code[X] = ((regs[0] & 0xE0) >> 3) | ((regs[3] & 0x30) >> 4);
426         shift_code[Y] = ((regs[1] & 0xE0) >> 3) | ((regs[3] & 0x0C) >> 2);
427         shift_code[Z] = ((regs[2] & 0xE0) >> 3) |  (regs[3] & 0x03);
428         for (i = 0; i < 3; i++) {
429                 if (shift_code[i] != 0)
430                         st_prod[i] = test_setup.accl_sens[i]*
431                                 accl_st_tb[shift_code[i] - 1];
432         }
433         return 0;
434 }
435
436 static int inv_check_accl_self_test(struct inv_gyro_state_s *st,
437         int *reg_avg, int *st_avg){
438         int gravity, reg_z_avg, g_z_sign, fs, j, ret_val;
439         int tmp1;
440         int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3];
441
442         if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
443                 st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
444                 return 0;
445
446         fs = 8000UL;    /* assume +/- 2 mg as typical */
447         g_z_sign = 1;
448         ret_val = 0;
449         test_setup.accl_sens[X] = (unsigned int)((1L << 15) * 1000 / fs);
450         test_setup.accl_sens[Y] = (unsigned int)((1L << 15) * 1000 / fs);
451         test_setup.accl_sens[Z] = (unsigned int)((1L << 15) * 1000 / fs);
452         if (MPL_PROD_KEY(st->chip_info.product_id,
453                 st->chip_info.product_revision) ==
454                 MPU_PRODUCT_KEY_B1_E1_5) {
455                 /* half sensitivity Z accelerometer parts */
456                 test_setup.accl_sens[Z] /= 2;
457         } else {
458                 /* half sensitivity X, Y, Z accelerometer parts */
459                 test_setup.accl_sens[X] /= st->chip_info.multi;
460                 test_setup.accl_sens[Y] /= st->chip_info.multi;
461                 test_setup.accl_sens[Z] /= st->chip_info.multi;
462         }
463         gravity = test_setup.accl_sens[Z];
464         reg_z_avg = reg_avg[Z] - g_z_sign * gravity * DEF_GYRO_PRECISION;
465         read_accel_hw_self_test_prod_shift(st, st_shift_prod);
466         for (j = 0; j < 3; j++) {
467                 st_shift_cust[j] = abs(reg_avg[j] - st_avg[j]);
468                 if (st_shift_prod[j]) {
469                         tmp1 = st_shift_prod[j]/1000;
470                         st_shift_ratio[j] = st_shift_cust[j]/tmp1
471                                 - 1000;
472                         if (st_shift_ratio[j] > DEF_ACCEL_ST_SHIFT_DELTA)
473                                 ret_val |= 1 << j;
474                         if (st_shift_ratio[j] < -DEF_ACCEL_ST_SHIFT_DELTA)
475                                 ret_val |= 1 << j;
476                 } else {
477                         if (st_shift_cust[j] <
478                                 DEF_ACCEL_ST_SHIFT_MIN*gravity)
479                                 ret_val |= 1 << j;
480                         if (st_shift_cust[j] >
481                                 DEF_ACCEL_ST_SHIFT_MAX*gravity)
482                                 ret_val |= 1 << j;
483                 }
484         }
485         return ret_val;
486 }
487
488 static int inv_check_3500_gyro_self_test(struct inv_gyro_state_s *st,
489         int *reg_avg, int *st_avg){
490         int result;
491         int gst[3], ret_val;
492         int gst_otp[3], i;
493         unsigned char st_code[3];
494
495         ret_val = 0;
496         for (i = 0; i < 3; i++)
497                 gst[i] = st_avg[i] - reg_avg[i];
498         result = inv_i2c_read(st, REG_3500_OTP, 3, st_code);
499         if (result)
500                 return result;
501
502         gst_otp[0] = gst_otp[1] = gst_otp[2] = 0;
503         for (i = 0; i < 3; i++) {
504                 if (st_code[i] != 0)
505                         gst_otp[i] = gyro_3500_st_tb[st_code[i] - 1];
506         }
507         for (i = 0; i < 3; i++) {
508                 if (gst_otp[i] == 0) {
509                         if (abs(gst[i])*4 < 60*2*1000*131)
510                                 ret_val |= (1<<i);
511                 } else {
512                         if (abs(gst[i]/gst_otp[i] - 1000) > 140)
513                                 ret_val |= (1<<i);
514                 }
515         }
516         for (i = 0; i < 3; i++) {
517                 if (abs(reg_avg[i])*4 > 20*2*1000*131)
518                         ret_val |= (1<<i);
519         }
520
521         return ret_val;
522 }
523 static int inv_check_6050_gyro_self_test(struct inv_gyro_state_s *st,
524                                          int *reg_avg, int *st_avg)
525 {
526         int result;
527         int ret_val;
528         int ct_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
529         unsigned char regs[3];
530
531         if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
532                 st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
533                 return 0;
534
535         ret_val = 0;
536         result = inv_i2c_read(st, REG_ST_GCT_X, 3, regs);
537         regs[X] &= 0x1f;
538         regs[Y] &= 0x1f;
539         regs[Z] &= 0x1f;
540         for (i = 0; i < 3; i++) {
541                 if (regs[i] != 0)
542                         ct_shift_prod[i] = gyro_6050_st_tb[regs[i] - 1];
543                 else
544                         ct_shift_prod[i] = 0;
545         }
546         for (i = 0; i < 3; i++) {
547                 st_shift_cust[i] = abs(reg_avg[i] - st_avg[i]);
548                 if (ct_shift_prod[i]) {
549                         st_shift_ratio[i] = st_shift_cust[i]/
550                                 ct_shift_prod[i] - 1000;
551                         if (st_shift_ratio[i] > DEF_GYRO_CT_SHIFT_DELTA)
552                                 ret_val |= 1 << i;
553                         if (st_shift_ratio[i] < -DEF_GYRO_CT_SHIFT_DELTA)
554                                 ret_val |= 1 << i;
555                 } else {
556                         if (st_shift_cust[i] < 1000*DEF_GYRO_CT_SHIFT_MIN*
557                                 test_setup.gyro_sens)
558                                 ret_val |= 1 << i;
559                         if (st_shift_cust[i] > 1000*DEF_GYRO_CT_SHIFT_MAX*
560                                 test_setup.gyro_sens)
561                                 ret_val |= 1 << i;
562                 }
563         }
564         for (i = 0; i < 3; i++) {
565                 if (abs(reg_avg[i])*4 > 20*2*1000*131)
566                         ret_val |= (1<<i);
567         }
568         return ret_val;
569 }
570
571 /**
572  *  inv_do_test() - do the actual test of self testing
573  */
574 static int inv_do_test(struct inv_gyro_state_s *st, int self_test_flag,
575                        int *gyro_result, int *accl_result)
576 {
577         struct inv_reg_map_s *reg;
578         int result, i, packet_size;
579         unsigned char data[12], has_accl;
580         int fifo_count, packet_count, ind;
581
582         reg = st->reg;
583         has_accl = (st->chip_type != INV_ITG3500);
584         packet_size = 6 + 6 * has_accl;
585         result = inv_i2c_single_write(st, reg->pwr_mgmt_1, INV_CLK_PLL);
586         if (result)
587                 return result;
588
589         mdelay(POWER_UP_TIME);
590         result = inv_i2c_single_write(st, reg->pwr_mgmt_2, 0);
591         if (result)
592                 return result;
593
594         mdelay(POWER_UP_TIME);
595         result = inv_i2c_single_write(st, reg->int_enable, 0);
596         if (result)
597                 return result;
598
599         /* disable the sensor output to FIFO */
600         result = inv_i2c_single_write(st, reg->fifo_en, 0);
601         if (result)
602                 return result;
603
604         /* disable fifo reading */
605         result = inv_i2c_single_write(st, reg->user_ctrl, 0);
606         if (result)
607                 return result;
608
609         /* clear FIFO */
610         result = inv_i2c_single_write(st, reg->user_ctrl, BIT_FIFO_RST);
611         if (result)
612                 return result;
613
614         mdelay(15);
615         /* setup parameters */
616         result = inv_i2c_single_write(st, reg->lpf, test_setup.lpf);
617         if (result)
618                 return result;
619
620         result = inv_i2c_single_write(st, reg->sample_rate_div,
621                 test_setup.sample_rate);
622         if (result)
623                 return result;
624
625         result = inv_i2c_single_write(st, reg->gyro_config,
626                 self_test_flag | test_setup.fsr);
627         if (result)
628                 return result;
629
630         if (has_accl) {
631                 result = inv_i2c_single_write(st, reg->accl_config,
632                         self_test_flag | test_setup.accl_fs);
633                 if (result)
634                         return result;
635         }
636
637         /*wait for the output to stable*/
638         if (self_test_flag)
639                 mdelay(200);
640         /* enable FIFO reading */
641         result = inv_i2c_single_write(st, reg->user_ctrl, BIT_FIFO_EN);
642         if (result)
643                 return result;
644
645         /* enable sensor output to FIFO */
646         result = inv_i2c_single_write(st, reg->fifo_en, BITS_GYRO_OUT
647                 | (has_accl << 3));
648         if (result)
649                 return result;
650
651         mdelay(DEF_GYRO_WAIT_TIME);
652         /* stop sending data to FIFO */
653         result = inv_i2c_single_write(st, reg->fifo_en, 0);
654         if (result)
655                 return result;
656
657         result = inv_i2c_read(st, reg->fifo_count_h, 2, data);
658         if (result)
659                 return result;
660
661         fifo_count = (data[0] << 8) + data[1];
662         packet_count = fifo_count / packet_size;
663         gyro_result[0] = gyro_result[1] = gyro_result[2] = 0;
664         accl_result[0] = accl_result[1] = accl_result[2] = 0;
665         if (abs(packet_count - DEF_GYRO_PACKET_THRESH)
666                 <= DEF_GYRO_THRESH) {
667                 for (i = 0; i < packet_count; i++) {
668                         /* getting FIFO data */
669                         result = inv_i2c_read(st, reg->fifo_r_w,
670                                 packet_size, data);
671                         if (result)
672                                 return result;
673
674                         ind = 0;
675                         if (has_accl) {
676                                 accl_result[0] += (short)((data[ind] << 8)
677                                                 | data[ind + 1]);
678                                 accl_result[1] += (short)((data[ind + 2] << 8)
679                                                 | data[ind + 3]);
680                                 accl_result[2] += (short)((data[ind + 4] << 8)
681                                                 | data[ind + 5]);
682                                 ind += 6;
683                         }
684                         gyro_result[0] += (short)((data[ind] << 8) |
685                                 data[ind + 1]);
686                         gyro_result[1] += (short)((data[ind + 2] << 8) |
687                                 data[ind + 3]);
688                         gyro_result[2] += (short)((data[ind + 4] << 8) |
689                                 data[ind + 5]);
690                 }
691         } else {
692                 return -EAGAIN;
693         }
694
695         gyro_result[0] = gyro_result[0] * DEF_GYRO_PRECISION / packet_count;
696         gyro_result[1] = gyro_result[1] * DEF_GYRO_PRECISION / packet_count;
697         gyro_result[2] = gyro_result[2] * DEF_GYRO_PRECISION / packet_count;
698         if (has_accl) {
699                 accl_result[0] =
700                         accl_result[0] * DEF_GYRO_PRECISION / packet_count;
701                 accl_result[1] =
702                         accl_result[1] * DEF_GYRO_PRECISION / packet_count;
703                 accl_result[2] =
704                         accl_result[2] * DEF_GYRO_PRECISION / packet_count;
705         }
706
707         return 0;
708 }
709
710 /**
711  *  inv_recover_setting() recover the old settings after everything is done
712  */
713 static void inv_recover_setting(struct inv_gyro_state_s *st)
714 {
715         struct inv_reg_map_s *reg;
716         int data;
717
718         reg = st->reg;
719         set_inv_enable(st, st->chip_config.enable);
720         inv_i2c_single_write(st, reg->gyro_config, st->chip_config.fsr<<3);
721         inv_i2c_single_write(st, reg->lpf, st->chip_config.lpf);
722         data = ONE_K_HZ/st->chip_config.fifo_rate - 1;
723         inv_i2c_single_write(st, reg->sample_rate_div, data);
724         if (INV_ITG3500 != st->chip_type) {
725                 inv_i2c_single_write(st, reg->accl_config,
726                         (st->chip_config.accl_fs << 3)|0);
727         }
728         if (st->chip_config.is_asleep)
729                 inv_set_power_state(st, 0);
730         else
731                 inv_set_power_state(st, 1);
732 }
733
734 static int inv_check_compass_self_test(struct inv_gyro_state_s *st)
735 {
736         int result;
737         unsigned char data[6];
738         unsigned char counter, cntl;
739         short x, y, z;
740         unsigned char *sens;
741
742         sens = st->chip_info.compass_sens;
743         /*set to bypass mode */
744         result = inv_i2c_single_write(st, REG_INT_PIN_CFG, BIT_BYPASS_EN);
745         if (result) {
746                 inv_i2c_single_write(st, REG_INT_PIN_CFG, 0x0);
747                 return result;
748         }
749
750         /*set to power down mode */
751         result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PW_DN);
752         if (result)
753                 goto AKM_fail;
754
755         /*write 1 to ASTC register */
756         result = inv_secondary_write(REG_AKM_ST_CTRL, DATA_AKM_SELF_TEST);
757         if (result)
758                 goto AKM_fail;
759
760         /*set self test mode */
761         result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PW_ST);
762         if (result)
763                 goto AKM_fail;
764
765         counter = 10;
766         while (counter > 0) {
767                 mdelay(10);
768                 result = inv_secondary_read(REG_AKM_STATUS, 1, data);
769                 if (result)
770                         goto AKM_fail;
771
772                 if (data[0] != DATA_AKM_DRDY)
773                         counter--;
774                 else
775                         counter = 0;
776         }
777
778         if (data[0] != DATA_AKM_DRDY) {
779                 result = -1;
780                 goto AKM_fail;
781         }
782
783         result = inv_secondary_read(REG_AKM_MEASURE_DATA, 6, data);
784         if (result)
785                 goto AKM_fail;
786
787         x = (short)((data[1]<<8) | data[0]);
788         y = (short)((data[3]<<8) | data[2]);
789         z = (short)((data[5]<<8) | data[4]);
790         x = ((x * (sens[0] + 128)) >> 8);
791         y = ((y * (sens[1] + 128)) >> 8);
792         z = ((z * (sens[2] + 128)) >> 8);
793         if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) {
794                 result = inv_secondary_read(REG_AKM8963_CNTL1, 1, &cntl);
795                 if (result)
796                         goto AKM_fail;
797
798                 if (0 == (cntl & DATA_AKM8963_BIT)) {
799                         x <<= 2;
800                         y <<= 2;
801                         z <<= 2;
802                 }
803         }
804
805         result = 1;
806         result = inv_secondary_read(REG_AKM8963_CNTL1, 1, &cntl);
807         if (x > st->compass_st_upper[0] || x < st->compass_st_lower[0])
808                 goto AKM_fail;
809
810         if (y > st->compass_st_upper[1] || y < st->compass_st_lower[1])
811                 goto AKM_fail;
812
813         if (z > st->compass_st_upper[2] || z < st->compass_st_lower[2])
814                 goto AKM_fail;
815
816         result = 0;
817 AKM_fail:
818         /*write 0 to ASTC register */
819         result |= inv_secondary_write(REG_AKM_ST_CTRL, 0);
820         /*set to power down mode */
821         result |= inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PW_DN);
822         /*restore to non-bypass mode */
823         result |= inv_i2c_single_write(st, REG_INT_PIN_CFG, 0x0);
824         return result;
825 }
826
827 /**
828  *  inv_hw_self_test() - main function to do hardware self test
829  */
830 int inv_hw_self_test(struct inv_gyro_state_s *st,
831                      int *gyro_bias_regular)
832 {
833         int result;
834         int gyro_bias_st[3];
835         int accl_bias_st[3], accl_bias_regular[3];
836         int test_times;
837         char compass_result, accel_result, gyro_result;
838
839         compass_result = accel_result = gyro_result = 0;
840         test_times = 2;
841         while (test_times > 0) {
842                 result = inv_do_test(st, 0,  gyro_bias_regular,
843                         accl_bias_regular);
844                 if (result == -EAGAIN)
845                         test_times--;
846                 else
847                         test_times = 0;
848         }
849         if (result)
850                 goto test_fail;
851
852         test_times = 2;
853         while (test_times > 0) {
854                 result = inv_do_test(st, BITS_SELF_TEST_EN, gyro_bias_st,
855                                         accl_bias_st);
856                 if (result == -EAGAIN)
857                         test_times--;
858                 else
859                         break;
860         }
861         if (result)
862                 goto test_fail;
863
864         if (st->chip_type == INV_ITG3500) {
865                 gyro_result = !inv_check_3500_gyro_self_test(st,
866                         gyro_bias_regular, gyro_bias_st);
867         } else {
868                 if (st->has_compass)
869                         compass_result = !inv_check_compass_self_test(st);
870                 accel_result = !inv_check_accl_self_test(st,
871                         accl_bias_regular, accl_bias_st);
872                 gyro_result = !inv_check_6050_gyro_self_test(st,
873                         gyro_bias_regular, gyro_bias_st);
874         }
875 test_fail:
876         inv_recover_setting(st);
877         return (compass_result<<2) | (accel_result<<1) | gyro_result;
878 }
879
880 /**
881  *  inv_get_accl_bias() - main function to do hardware self test
882  */
883 int inv_get_accl_bias(struct inv_gyro_state_s *st, int *accl_bias_regular)
884 {
885         int result;
886         int gyro_bias_regular[3];
887         int test_times;
888
889         test_times = 2;
890         while (test_times > 0) {
891                 result = inv_do_test(st, 0,  gyro_bias_regular,
892                         accl_bias_regular);
893                 if (result == -EAGAIN)
894                         test_times--;
895                 else
896                         test_times = 0;
897         }
898         inv_recover_setting(st);
899         return result;
900 }
901
902 static int inv_load_firmware(struct inv_gyro_state_s *st,
903                              unsigned char *data, int size)
904 {
905         int bank, write_size;
906         int result;
907         unsigned short memaddr;
908
909         /* Write and verify memory */
910         for (bank = 0; size > 0; bank++,
911                 size -= write_size,
912                 data += write_size) {
913                 if (size > MPU_MEM_BANK_SIZE)
914                         write_size = MPU_MEM_BANK_SIZE;
915                 else
916                         write_size = size;
917
918                 memaddr = ((bank << 8) | 0x00);
919                 result = mem_w(memaddr, write_size, data);
920                 if (result)
921                         return result;
922         }
923
924         return 0;
925 }
926
927 static int inv_verify_firmware(struct inv_gyro_state_s *st,
928                                unsigned char *data, int size)
929 {
930         int bank, write_size;
931         int result;
932         unsigned short memaddr;
933         unsigned char firmware[MPU_MEM_BANK_SIZE];
934
935         /* Write and verify memory */
936         for (bank = 0; size > 0; bank++,
937                 size -= write_size,
938                 data += write_size) {
939                 if (size > MPU_MEM_BANK_SIZE)
940                         write_size = MPU_MEM_BANK_SIZE;
941                 else
942                         write_size = size;
943                 memaddr = ((bank << 8) | 0x00);
944                 result = mpu_memory_read(st->sl_handle,
945                         st->i2c_addr, memaddr, write_size, firmware);
946                 if (result)
947                         return result;
948
949                 if (0 != memcmp(firmware, data, write_size))
950                         return -EINVAL;
951         }
952
953         return 0;
954 }
955
956 static int inv_set_fifo_div(struct inv_gyro_state_s *st,
957                             unsigned short fifoRate)
958 {
959         unsigned char regs[2];
960         int result = 0;
961         /* For some reason DINAC4 is defined as 0xb8,
962            but DINBC4 is not defined*/
963         unsigned char regs_end[8] = {DINAFE, DINAF2, DINAAB,
964                 0xC4, DINAAA, DINAF1, DINADF, DINADF};
965
966         regs[0] = (unsigned char)((fifoRate >> 8) & 0xff);
967         regs[1] = (unsigned char)(fifoRate & 0xff);
968         result = mem_w_key(KEY_D_0_22, 2, regs);
969         if (result)
970                 return result;
971
972         /*Modify the FIFO handler to reset the tap/orient interrupt flags*/
973         /* each time the FIFO handler runs*/
974         result = mem_w_key(KEY_CFG_6, 8, regs_end);
975
976         return result;
977 }
978
979 int inv_set_fifo_rate(struct inv_gyro_state_s *st, unsigned long fifo_rate)
980 {
981         unsigned char divider;
982         int result;
983
984         divider = (unsigned char)(1000/fifo_rate) - 1;
985         if (divider > 4) {
986                 st->sample_divider = 4;
987                 st->fifo_divider = (unsigned char)(200/fifo_rate) - 1;
988         } else {
989                 st->sample_divider = divider;
990                 st->fifo_divider = 0;
991         }
992         result = inv_set_fifo_div(st, st->fifo_divider);
993         return result;
994 }
995
996 static int inv_set_tap_interrupt_dmp(struct inv_gyro_state_s *st,
997                                      unsigned char on)
998 {
999         int result;
1000         unsigned char  regs[2] = {0};
1001
1002         if (on)
1003                 regs[0] = 0xf8;
1004         else
1005                 regs[0] = DINAD8;
1006         result = mem_w_key(KEY_CFG_20, 1, regs);
1007         if (result)
1008                 return result;
1009
1010         return result;
1011 }
1012
1013 static int inv_set_orientation_interrupt_dmp(struct inv_gyro_state_s *st,
1014                                              unsigned char on)
1015 {
1016         int result;
1017         unsigned char  regs[2] = {0};
1018
1019         if (on) {
1020                 regs[0] = DINBF8;
1021                 regs[1] = DINBF8;
1022         } else {
1023                 regs[0] = DINAD8;
1024                 regs[1] = DINAD8;
1025         }
1026         result = mem_w_key(KEY_CFG_ORIENT_IRQ_1, 2, regs);
1027         if (result)
1028                 return result;
1029
1030         return result;
1031 }
1032
1033 static int inv_set_tap_threshold_dmp(struct inv_gyro_state_s *st,
1034                                      unsigned int axis,
1035                                      unsigned short threshold)
1036 {
1037         /* Sets the tap threshold in the dmp
1038         Simultaneously sets secondary tap threshold to help correct the tap
1039         direction for soft taps */
1040         int result;
1041         /* DMP Algorithm */
1042         unsigned char data[2];
1043         int sampleDivider;
1044         int scaledThreshold;
1045         unsigned int dmpThreshold;
1046         unsigned char sample_div;
1047 #define accel_sens  (0x20000000/(0x00010000))
1048
1049         if ((axis & ~(INV_TAP_AXIS_ALL)) || (threshold > (1<<15)))
1050                 return -EINVAL;
1051
1052         sample_div = st->sample_divider;
1053         sampleDivider = (1 + sample_div);
1054         /* Scale factor corresponds linearly using
1055         * 0  : 0
1056         * 25 : 0.0250  g/ms
1057         * 50 : 0.0500  g/ms
1058         * 100: 1.0000  g/ms
1059         * 200: 2.0000  g/ms
1060         * 400: 4.0000  g/ms
1061         * 800: 8.0000  g/ms
1062         */
1063         /*multiply by 1000 to avoid floating point 1000/1000*/
1064         scaledThreshold = threshold;
1065         /* Convert to per sample */
1066         scaledThreshold *= sampleDivider;
1067         /* Scale to DMP 16 bit value */
1068         if (accel_sens != 0)
1069                 dmpThreshold = (unsigned int)(scaledThreshold*accel_sens);
1070         else
1071                 return -EINVAL;
1072
1073         dmpThreshold = dmpThreshold/1000;
1074         data[0] = dmpThreshold >> 8;
1075         data[1] = dmpThreshold & 0xFF;
1076         /* MPL algorithm */
1077         if (axis & INV_TAP_AXIS_X) {
1078                 result = mem_w_key(KEY_DMP_TAP_THR_X, 2, data);
1079                 if (result)
1080                         return result;
1081
1082                 /*Also set additional threshold for correcting the direction
1083                 of taps that were very near the threshold. */
1084                 data[0] = (dmpThreshold*3/4) >> 8;
1085                 data[1] = (dmpThreshold*3/4) & 0xFF;
1086                 result = mem_w_key(KEY_D_1_36, 2, data);
1087                 if (result)
1088                         return result;
1089         }
1090
1091         if (axis & INV_TAP_AXIS_Y) {
1092                 result = mem_w_key(KEY_DMP_TAP_THR_Y, 2, data);
1093                 if (result)
1094                         return result;
1095
1096                 data[0] = (dmpThreshold*3/4) >> 8;
1097                 data[1] = (dmpThreshold*3/4) & 0xFF;
1098                 result = mem_w_key(KEY_D_1_40, 2, data);
1099                 if (result)
1100                         return result;
1101         }
1102
1103         if (axis & INV_TAP_AXIS_Z) {
1104                 result = mem_w_key(KEY_DMP_TAP_THR_Z, 2, data);
1105                 if (result)
1106                         return result;
1107
1108                 data[0] = (dmpThreshold*3/4) >> 8;
1109                 data[1] = (dmpThreshold*3/4) & 0xFF;
1110                 result = mem_w_key(KEY_D_1_44, 2, data);
1111                 if (result)
1112                         return result;
1113         }
1114
1115         return 0;
1116 }
1117
1118 static int inv_set_tap_axes_dmp(struct inv_gyro_state_s *st,
1119                                 unsigned int axes)
1120 {
1121         /* Sets a mask in the DMP that indicates what tap events
1122         should result in an interrupt */
1123         unsigned char regs[4];
1124         unsigned char result;
1125
1126         /* check if any spurious bit other the ones expected are set */
1127         if (axes & (~(INV_TAP_ALL_DIRECTIONS)))
1128                 return -EINVAL;
1129
1130         regs[0] = (unsigned char)axes;
1131         result = mem_w_key(KEY_D_1_72, 1, regs);
1132         return result;
1133 }
1134
1135 static int inv_set_min_taps_dmp(struct inv_gyro_state_s *st,
1136                                 unsigned int min_taps) {
1137         /*Indicates the minimum number of consecutive taps required
1138                 before the DMP will generate an interrupt */
1139         unsigned char regs[1];
1140         unsigned char result;
1141
1142         /* check if any spurious bit other the ones expected are set */
1143         if ((min_taps > 4) || (min_taps < 1))
1144                 return -EINVAL;
1145
1146         regs[0] = (unsigned char)(min_taps-1);
1147         result = mem_w_key(KEY_D_1_79, 1, regs);
1148         return result;
1149 }
1150
1151 static int  inv_set_tap_time_dmp(struct inv_gyro_state_s *st,
1152                                  unsigned int time)
1153 {
1154         /* Determines how long after a tap the DMP requires before
1155           another tap can be registered*/
1156         int result;
1157         /* DMP Algorithm */
1158         unsigned short dmpTime;
1159         unsigned char data[2];
1160         unsigned char sampleDivider;
1161
1162         sampleDivider = st->sample_divider;
1163         sampleDivider++;
1164         /* 60 ms minimum time added */
1165         dmpTime = ((time) / sampleDivider);
1166         data[0] = dmpTime >> 8;
1167         data[1] = dmpTime & 0xFF;
1168         result = mem_w_key(KEY_DMP_TAPW_MIN, 2, data);
1169         return result;
1170 }
1171
1172 static int inv_set_multiple_tap_time_dmp(struct inv_gyro_state_s *st,
1173                                          unsigned int time)
1174 {
1175         /*Determines how close together consecutive taps must occur
1176         to be considered double/triple taps*/
1177         int result;
1178         /* DMP Algorithm */
1179         unsigned short dmpTime;
1180         unsigned char data[2];
1181         unsigned char sampleDivider;
1182
1183         sampleDivider = st->sample_divider;
1184         sampleDivider++;
1185         /* 60 ms minimum time added */
1186         dmpTime = ((time) / sampleDivider);
1187         data[0] = dmpTime >> 8;
1188         data[1] = dmpTime & 0xFF;
1189         result = mem_w_key(KEY_D_1_218, 2, data);
1190         return result;
1191 }
1192
1193 static long inv_q30_mult(long a, long b)
1194 {
1195         long long temp;
1196         long result;
1197
1198         temp = (long long)a * b;
1199         result = (long)(temp >> 30);
1200         return result;
1201 }
1202
1203 #define gyro_sens 0x03e80000
1204
1205 static int inv_set_gyro_sf_dmp(struct inv_gyro_state_s *st)
1206 {
1207         /*The gyro threshold, in dps, above which taps will be rejected*/
1208         int result, out;
1209         /* DMP Algorithm */
1210         unsigned char sampleDivider;
1211         unsigned char *regs;
1212         int gyro_sf;
1213
1214         sampleDivider = st->sample_divider;
1215         gyro_sf = inv_q30_mult(gyro_sens,
1216                         (int)((767603923/5) * (sampleDivider+1)));
1217         out = cpu_to_be32p(&gyro_sf);
1218         regs = (unsigned char *)&out;
1219         result = mem_w_key(KEY_D_0_104, 4, regs);
1220         return result;
1221 }
1222
1223 static int inv_set_shake_reject_thresh_dmp(struct inv_gyro_state_s *st,
1224                                            int thresh)
1225 {/*THIS FUNCTION FAILS MEM_W*/
1226         /*The gyro threshold, in dps, above which taps will be rejected */
1227         int result, out;
1228         /* DMP Algorithm */
1229         unsigned char sampleDivider;
1230         int thresh_scaled;
1231         unsigned char *regs;
1232         long gyro_sf;
1233
1234         sampleDivider = st->sample_divider;
1235         gyro_sf = inv_q30_mult(gyro_sens, (int)((767603923/5) *
1236                         (sampleDivider+1)));
1237         /* We're in units of DPS, convert it back to chip units*/
1238         /*split the operation to aviod overflow of integer*/
1239         thresh_scaled = gyro_sens/(1L<<16);
1240         thresh_scaled = thresh_scaled/thresh;
1241         thresh_scaled = gyro_sf / thresh_scaled;
1242         out = cpu_to_be32p(&thresh_scaled);
1243         regs = (unsigned char *)&out;
1244         result = mem_w_key(KEY_D_1_92, 4, regs);
1245         return result;
1246 }
1247
1248 static int inv_set_shake_reject_time_dmp(struct inv_gyro_state_s *st,
1249                                          unsigned int time)
1250 {
1251         /* How long a gyro axis must remain above its threshold
1252         before taps are rejected */
1253         int result;
1254         /* DMP Algorithm */
1255         unsigned short dmpTime;
1256         unsigned char data[2];
1257         unsigned char sampleDivider;
1258
1259         sampleDivider = st->sample_divider;
1260         sampleDivider++;
1261         /* 60 ms minimum time added */
1262         dmpTime = ((time) / sampleDivider);
1263         data[0] = dmpTime >> 8;
1264         data[1] = dmpTime & 0xFF;
1265         result = mem_w_key(KEY_D_1_88, 2, data);
1266         return result;
1267 }
1268
1269 static int inv_set_shake_reject_timeout_dmp(struct inv_gyro_state_s *st,
1270                                             unsigned int time)
1271 {
1272         /*How long the gyros must remain below their threshold,
1273         after taps have been rejected, before taps can be detected again*/
1274         int result;
1275         /* DMP Algorithm */
1276         unsigned short dmpTime;
1277         unsigned char data[2];
1278         unsigned char sampleDivider;
1279
1280         sampleDivider = st->sample_divider;
1281         sampleDivider++;
1282         /* 60 ms minimum time added */
1283         dmpTime = ((time) / sampleDivider);
1284         data[0] = dmpTime >> 8;
1285         data[1] = dmpTime & 0xFF;
1286         result = mem_w_key(KEY_D_1_90, 2, data);
1287         return result;
1288 }
1289
1290 static int inv_set_interrupt_on_gesture_event(struct inv_gyro_state_s *st,
1291                                               char on)
1292 {
1293         unsigned char result;
1294         unsigned char regs_on[12] = {DINADA, DINADA, DINAB1, DINAB9,
1295                                         DINAF3, DINA8B, DINAA3, DINA91,
1296                                         DINAB6, DINADA, DINAB4, DINADA};
1297         unsigned char regs_off[12] = {0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b,
1298                                         0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
1299         /*For some reason DINAC4 is defined as 0xb8,
1300         but DINBC4 is not defined.*/
1301         unsigned char regs_end[8] = {DINAFE, DINAF2, DINAAB, 0xc4,
1302                                         DINAAA, DINAF1, DINADF, DINADF};
1303
1304         if (on) {
1305                 /*Sets the DMP to send an interrupt and put a FIFO packet
1306                 in the FIFO if and only if a tap/orientation event
1307                 just occurred*/
1308                 result = mem_w_key(KEY_CFG_FIFO_ON_EVENT, 12, regs_on);
1309                 if (result)
1310                         return result;
1311
1312         } else {
1313                 /*Sets the DMP to send an interrupt and put a FIFO packet
1314                 in the FIFO at the rate specified by the FIFO div.
1315                 see inv_set_fifo_div in hw_setup.c to set the FIFO div.*/
1316                 result = mem_w_key(KEY_CFG_FIFO_ON_EVENT, 12, regs_off);
1317                 if (result)
1318                         return result;
1319         }
1320
1321         result = mem_w_key(KEY_CFG_6, 8, regs_end);
1322         return result;
1323 }
1324
1325 /**
1326  * inv_enable_tap_dmp() -  calling this function will enable/disable tap function.
1327  */
1328 int inv_enable_tap_dmp(struct inv_gyro_state_s *st, unsigned char on)
1329 {
1330         int result;
1331
1332         result = inv_set_tap_interrupt_dmp(st, on);
1333         if (result)
1334                 return result;
1335
1336         if (on) {
1337                 result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_X, 100) ;
1338                 if (result)
1339                         return result;
1340
1341                 result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_Y, 100) ;
1342                 if (result)
1343                         return result;
1344
1345                 result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_Z, 100) ;
1346                 if (result)
1347                         return result;
1348         }
1349
1350         result = inv_set_tap_axes_dmp(st, INV_TAP_ALL_DIRECTIONS) ;
1351         if (result)
1352                 return result;
1353
1354         result = inv_set_min_taps_dmp(st, 2);
1355         if (result)
1356                 return result;
1357
1358         result = inv_set_tap_time_dmp(st, 100);
1359         if (result)
1360                 return result;
1361
1362         result = inv_set_multiple_tap_time_dmp(st, 500) ;
1363         if (result)
1364                 return result;
1365
1366         result = inv_set_gyro_sf_dmp(st);
1367         if (result)
1368                 return result;
1369
1370         result = inv_set_shake_reject_thresh_dmp(st, 100) ;
1371         if (result)
1372                 return result;
1373
1374         result = inv_set_shake_reject_time_dmp(st, 10) ;
1375         if (result)
1376                 return result;
1377
1378         result = inv_set_shake_reject_timeout_dmp(st, 10);
1379         if (result)
1380                 return result;
1381
1382         result = inv_set_interrupt_on_gesture_event(st, 0);
1383         return result;
1384 }
1385
1386 static int inv_set_orientation_dmp(struct inv_gyro_state_s *st,
1387                                    int orientation)
1388 {
1389         /*Set a mask in the DMP determining what orientations
1390                         will trigger interrupts*/
1391         unsigned char regs[4];
1392         unsigned char result;
1393
1394         /* check if any spurious bit other the ones expected are set */
1395         if (orientation & (~(INV_ORIENTATION_ALL | INV_ORIENTATION_FLIP)))
1396                 return -EINVAL;
1397
1398         regs[0] = (unsigned char)orientation;
1399         result = mem_w_key(KEY_D_1_74, 1, regs);
1400         return result;
1401 }
1402
1403 static int inv_set_orientation_thresh_dmp(struct inv_gyro_state_s *st,
1404                                           int angle)
1405 {
1406         /*Set an angle threshold in the DMP determining
1407                 when orientations change*/
1408         unsigned char *regs;
1409         unsigned char result;
1410         unsigned int out;
1411         unsigned int threshold;
1412
1413         /*threshold = (long)((1<<29) * sin((angle * M_PI) / 180.));*/
1414         threshold = 464943848;
1415         out = cpu_to_be32p(&threshold);
1416         regs = (unsigned char *)&out;
1417         result = mem_w_key(KEY_D_1_232, 4, regs);
1418         return result;
1419 }
1420
1421 static int inv_set_orientation_time_dmp(struct inv_gyro_state_s *st,
1422                                         unsigned int time)
1423 {
1424         /*Determines the stability time required before a
1425         new orientation can be adopted */
1426         unsigned short dmpTime;
1427         unsigned char data[2];
1428         unsigned char sampleDivider;
1429         unsigned char result;
1430
1431         /* First check if we are allowed to call this function here */
1432         sampleDivider = st->sample_divider;
1433         sampleDivider++;
1434         /* 60 ms minimum time added */
1435         dmpTime = ((time) / sampleDivider);
1436         data[0] = dmpTime >> 8;
1437         data[1] = dmpTime & 0xFF;
1438         result = mem_w_key(KEY_D_1_250, 2, data);
1439         return result;
1440 }
1441
1442 /**
1443  * inv_enable_orientation_dmp() -  calling this function will
1444  *                  enable/disable orientation function.
1445  */
1446 int inv_enable_orientation_dmp(struct inv_gyro_state_s *st)
1447 {
1448         int result;
1449
1450         result = inv_set_orientation_interrupt_dmp(st, 1);
1451         if (result)
1452                 return result;
1453
1454         result = inv_set_orientation_dmp(st, 0x40 | INV_ORIENTATION_ALL);
1455         if (result)
1456                 return result;
1457
1458         result = inv_set_gyro_sf_dmp(st);
1459         if (result)
1460                 return result;
1461
1462         result = inv_set_orientation_thresh_dmp(st, 60);
1463         if (result)
1464                 return result;
1465
1466         result = inv_set_orientation_time_dmp(st, 500);
1467         return result;
1468 }
1469
1470 static int inv_send_sensor_data(struct inv_gyro_state_s *st,
1471                                 unsigned short elements)
1472 {
1473         int result;
1474         unsigned char regs[10] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1475                                   DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1476                                   DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1477                                   DINAA0 + 3 };
1478
1479         if (elements & INV_ELEMENT_1)
1480                 regs[0] = DINACA;
1481         if (elements & INV_ELEMENT_2)
1482                 regs[4] = DINBC4;
1483         if (elements & INV_ELEMENT_3)
1484                 regs[5] = DINACC;
1485         if (elements & INV_ELEMENT_4)
1486                 regs[6] = DINBC6;
1487         if ((elements & INV_ELEMENT_5) || (elements & INV_ELEMENT_6)
1488                 || (elements & INV_ELEMENT_7)) {
1489                 regs[1] = DINBC0;
1490                 regs[2] = DINAC8;
1491                 regs[3] = DINBC2;
1492         }
1493         result = mem_w_key(KEY_CFG_15, 10, regs);
1494         return result;
1495 }
1496
1497 static int inv_send_interrupt_word(struct inv_gyro_state_s *st)
1498 {
1499         unsigned char regs[1] = { DINA20 };
1500         unsigned char result;
1501
1502         result = mem_w_key(KEY_CFG_27, 1, regs);
1503         return result;
1504 }
1505
1506 /**
1507  * inv_dmp_firmware_write() -  calling this function will load the firmware.
1508  *                        This is the write function of file "dmp_firmware".
1509  */
1510 ssize_t inv_dmp_firmware_write(struct file *fp, struct kobject *kobj,
1511         struct bin_attribute *attr,
1512         char *buf, loff_t pos, size_t size)
1513 {
1514         struct inv_gyro_state_s *st;
1515         unsigned char *firmware;
1516         int result;
1517         struct inv_reg_map_s *reg;
1518
1519         st = dev_get_drvdata(container_of(kobj, struct device, kobj));
1520         if (st->chip_config.is_asleep)
1521                 return -EPERM;
1522
1523         if (1 == st->chip_config.firmware_loaded)
1524                 return -EINVAL;
1525
1526         reg = st->reg;
1527         firmware = kmalloc(size, GFP_KERNEL);
1528         if (!firmware)
1529                 return -ENOMEM;
1530
1531         memcpy(firmware, buf, size);
1532         result = inv_load_firmware(st, firmware, size);
1533         if (result)
1534                 goto firmware_write_fail;
1535
1536         result = inv_verify_firmware(st, firmware, size);
1537         if (result)
1538                 goto firmware_write_fail;
1539
1540         result = inv_i2c_single_write(st, reg->prgm_strt_addrh,
1541         st->chip_config.prog_start_addr >> 8);
1542         if (result)
1543                 goto firmware_write_fail;
1544
1545         result = inv_i2c_single_write(st, reg->prgm_strt_addrh + 1,
1546         st->chip_config.prog_start_addr & 0xff);
1547         if (result)
1548                 goto firmware_write_fail;
1549
1550         result = inv_verify_firmware(st, firmware, size);
1551         if (result)
1552                 goto firmware_write_fail;
1553
1554         result = inv_set_fifo_rate(st, 200);
1555         if (result)
1556                 goto firmware_write_fail;
1557
1558         result = inv_enable_tap_dmp(st, 1);
1559         if (result)
1560                 goto firmware_write_fail;
1561
1562         result = inv_enable_orientation_dmp(st);
1563         if (result)
1564                 goto firmware_write_fail;
1565
1566         result = inv_send_sensor_data(st, INV_GYRO_ACC_MASK);
1567         if (result)
1568                 goto firmware_write_fail;
1569
1570         result = inv_send_interrupt_word(st);
1571         if (result)
1572                 goto firmware_write_fail;
1573
1574         st->chip_config.firmware_loaded = 1;
1575         st->chip_config.dmp_on = 1;
1576         result = size;
1577 firmware_write_fail:
1578         kfree(firmware);
1579         return result;
1580 }
1581
1582 ssize_t inv_dmp_firmware_read(struct file *filp, struct kobject *kobj,
1583                               struct bin_attribute *bin_attr,
1584                               char *buf, loff_t off, size_t count)
1585 {
1586         struct inv_gyro_state_s *st;
1587         int bank, write_size, size, data, result;
1588         unsigned short memaddr;
1589         size = count;
1590
1591         st = dev_get_drvdata(container_of(kobj, struct device, kobj));
1592         data = 0;
1593         for (bank = 0; size > 0; bank++, size -= write_size,
1594                                  data += write_size) {
1595                 if (size > MPU_MEM_BANK_SIZE)
1596                         write_size = MPU_MEM_BANK_SIZE;
1597                 else
1598                         write_size = size;
1599                 memaddr = ((bank << 8) | 0x00);
1600                 result = mpu_memory_read(st->sl_handle,
1601                         st->i2c_addr, memaddr, write_size, &buf[data]);
1602                 if (result)
1603                         return result;
1604         }
1605
1606         return count;
1607 }
1608