cfa76ab46865682c015eb2f436ca7233b855e64f
[linux-3.10.git] / drivers / input / misc / mpu / inv_gyro.c
1 /*
2 * Copyright (C) 2012 Invensense, Inc.
3 * Copyright (c) 2013, NVIDIA CORPORATION.  All rights reserved.
4 *
5 * This software is licensed under the terms of the GNU General Public
6 * License version 2, as published by the Free Software Foundation, and
7 * may be copied, distributed, and modified under those terms.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 * GNU General Public License for more details.
13 *
14 */
15
16 /**
17  *  @addtogroup  DRIVERS
18  *  @brief       Hardware drivers.
19  *
20  *  @{
21  *      @file    inv_gyro.c
22  *      @brief   A sysfs device driver for Invensense devices
23  *      @details This driver currently works for the ITG3500, MPU6050, MPU9150
24  *               MPU3050
25  */
26
27 #include <linux/module.h>
28 #include <linux/init.h>
29 #include <linux/slab.h>
30 #include <linux/i2c.h>
31 #include <linux/err.h>
32 #include <linux/delay.h>
33 #include <linux/sysfs.h>
34 #include <linux/jiffies.h>
35 #include <linux/irq.h>
36 #include <linux/interrupt.h>
37 #include <linux/kfifo.h>
38 #include <linux/regulator/consumer.h>
39 #include <linux/byteorder/generic.h>
40 #include <linux/poll.h>
41 #include <linux/miscdevice.h>
42 #include <linux/spinlock.h>
43 #include <linux/mpu.h>
44
45 #include "inv_gyro.h"
46
47
48 /* regulator names in order of powering on */
49 static char *nvi_vregs[] = {
50         "vdd",
51         "vlogic",
52 };
53
54 static struct inv_reg_map_s chip_reg = {
55         .who_am_i               = 0x75,
56         .sample_rate_div        = 0x19,
57         .lpf                    = 0x1A,
58         .product_id             = 0x0C,
59         .bank_sel               = 0x6D,
60         .user_ctrl              = 0x6A,
61         .fifo_en                = 0x23,
62         .gyro_config            = 0x1B,
63         .accl_config            = 0x1C,
64         .fifo_count_h           = 0x72,
65         .fifo_r_w               = 0x74,
66         .raw_gyro               = 0x43,
67         .raw_accl               = 0x3B,
68         .temperature            = 0x41,
69         .int_enable             = 0x38,
70         .int_status             = 0x3A,
71         .pwr_mgmt_1             = 0x6B,
72         .pwr_mgmt_2             = 0x6C,
73         .mem_start_addr         = 0x6E,
74         .mem_r_w                = 0x6F,
75         .prgm_strt_addrh        = 0x70
76 };
77
78 static const struct inv_hw_s hw_info[INV_NUM_PARTS] = {
79         {119, "ITG3500"},
80         { 63, "MPU3050"},
81         {118, "MPU6050"},
82         {118, "MPU9150"}
83 };
84
85 static unsigned long nvi_lpf_us_tbl[] = {
86         3906,   /* 256Hz */
87         5319,   /* 188Hz */
88         10204,  /* 98Hz */
89         23810,  /* 42Hz */
90         50000,  /* 20Hz */
91         100000, /* 10Hz */
92         /* 200000, 5Hz */
93 };
94
95 static unsigned long nvi_lpa_delay_us_tbl[] = {
96         800000,
97         200000,
98         50000,
99         /* 25000, */
100 };
101
102 static struct inv_gyro_state_s *inf_local;
103
104 s64 get_time_ns(void)
105 {
106         struct timespec ts;
107         ktime_get_ts(&ts);
108         return timespec_to_ns(&ts);
109 }
110
111 /**
112  *  inv_i2c_read_base() - Read one or more bytes from the device registers.
113  *  @st:        Device driver instance.
114  *  @reg:       First device register to be read from.
115  *  @length:    Number of bytes to read.
116  *  @data:      Data read from device.
117  *  NOTE: The slave register will not increment when reading from the FIFO.
118  */
119 int inv_i2c_read_base(struct inv_gyro_state_s *st, unsigned short i2c_addr,
120                       unsigned char reg, unsigned short length,
121                       unsigned char *data)
122 {
123         struct i2c_msg msgs[2];
124         int res;
125
126         if (st->shutdown)
127                 return 0;
128
129         if (!data)
130                 return -EINVAL;
131
132         msgs[0].addr = i2c_addr;
133         msgs[0].flags = 0;      /* write */
134         msgs[0].buf = &reg;
135         msgs[0].len = 1;
136
137         msgs[1].addr = i2c_addr;
138         msgs[1].flags = I2C_M_RD;
139         msgs[1].buf = data;
140         msgs[1].len = length;
141
142         res = i2c_transfer(st->sl_handle, msgs, 2);
143         pr_debug("%s RD%02X%02X%02X res=%d\n",
144                 st->hw_s->name, i2c_addr, reg, length, res);
145         if (res < 2) {
146                 if (res >= 0)
147                         res = -EIO;
148                 return res;
149         }
150
151         return 0;
152 }
153
154 /**
155  *  inv_i2c_single_write_base() - Write a byte to a device register.
156  *  @st:        Device driver instance.
157  *  @reg:       Device register to be written to.
158  *  @data:      Byte to write to device.
159  */
160 int inv_i2c_single_write_base(struct inv_gyro_state_s *st,
161                               unsigned short i2c_addr, unsigned char reg,
162                               unsigned char data)
163 {
164         unsigned char tmp[2];
165         struct i2c_msg msg;
166         int res;
167
168         if (st->shutdown)
169                 return 0;
170
171         tmp[0] = reg;
172         tmp[1] = data;
173
174         msg.addr = i2c_addr;
175         msg.flags = 0;  /* write */
176         msg.buf = tmp;
177         msg.len = 2;
178
179         res = i2c_transfer(st->sl_handle, &msg, 1);
180         pr_debug("%s WS%02X%02X%02X res=%d\n",
181                  st->hw_s->name, i2c_addr, reg, data, res);
182         if (res < 1) {
183                 if (res == 0)
184                         res = -EIO;
185                 return res;
186         }
187
188         return 0;
189 }
190
191
192 /* Register SMPLRT_DIV (0x19) */
193 static int nvi_smplrt_div_wr(struct inv_gyro_state_s *inf,
194                              unsigned char smplrt_div)
195 {
196         int err = 0;
197
198         if (smplrt_div != inf->hw.smplrt_div) {
199                 err = inv_i2c_single_write(inf, inf->reg->sample_rate_div,
200                                            smplrt_div);
201                 if (!err)
202                         inf->hw.smplrt_div = smplrt_div;
203         }
204         return err;
205 }
206
207 /* Register CONFIG (0x1A) */
208 static int nvi_config_wr(struct inv_gyro_state_s *inf, unsigned char lpf)
209 {
210         int err = 0;
211
212         if (lpf != inf->hw.config) {
213                 err = inv_i2c_single_write(inf, inf->reg->lpf, lpf);
214                 if (!err)
215                         inf->hw.config = lpf;
216         }
217         return err;
218 }
219
220 /* Register GYRO_CONFIG (0x1B) */
221 static int nvi_gyro_config_wr(struct inv_gyro_state_s *inf, unsigned char fsr)
222 {
223         unsigned char val;
224         int err = 0;
225
226         if (fsr != inf->hw.gyro_config) {
227                 val = (fsr << 3);
228                 err = inv_i2c_single_write(inf, inf->reg->gyro_config, val);
229                 if (!err) {
230                         inf->hw.gyro_config = val;
231                         err = 1; /* flag change made */
232                 }
233         }
234         return err;
235 }
236
237 /* Register ACCEL_CONFIG (0x1C) */
238 static int nvi_accel_config_wr(struct inv_gyro_state_s *inf,
239                                unsigned char fsr, unsigned char hpf)
240 {
241         unsigned char val;
242         int err = 0;
243
244         val = (fsr << 3) | hpf;
245         if (val != inf->hw.accl_config) {
246                 err = inv_i2c_single_write(inf, inf->reg->accl_config,
247                                            val);
248                 if (!err) {
249                         inf->hw.accl_config = val;
250                         err = 1; /* flag change made */
251                         if (hpf != 7)
252                                 inf->mot_enable = false;
253                 }
254         }
255         return err;
256 }
257
258 /* Register MOT_THR (0x1F) */
259 static int nvi_mot_thr_wr(struct inv_gyro_state_s *inf, unsigned char mot_thr)
260 {
261         int err = 0;
262
263         if (mot_thr != inf->hw.mot_thr) {
264                 err = inv_i2c_single_write(inf, REG_MOT_THR, mot_thr);
265                 if (!err)
266                         inf->hw.mot_thr = mot_thr;
267         }
268         return err;
269 }
270
271 /* Register MOT_DUR (0x20) */
272 static int nvi_mot_dur_wr(struct inv_gyro_state_s *inf, unsigned char mot_dur)
273 {
274         int err = 0;
275
276         if (mot_dur != inf->hw.mot_dur) {
277                 err = inv_i2c_single_write(inf, REG_MOT_DUR, mot_dur);
278                 if (!err)
279                         inf->hw.mot_dur = mot_dur;
280         }
281         return err;
282 }
283
284 /* Register FIFO_EN (0x23) */
285 static int nvi_fifo_en_wr(struct inv_gyro_state_s *inf, unsigned char fifo_en)
286 {
287         int err = 0;
288
289         if (fifo_en != inf->hw.fifo_en) {
290                 err = inv_i2c_single_write(inf, inf->reg->fifo_en, fifo_en);
291                 if (!err)
292                         inf->hw.fifo_en = fifo_en;
293         }
294         return err;
295 }
296
297 /* Register I2C_MST_CTRL (0x24) */
298 static int nvi_i2c_mst_ctrl_wr(struct inv_gyro_state_s *inf,
299                                bool port3_fifo_en)
300 {
301         unsigned char val;
302         int err = 0;
303
304         val = inf->aux.clock_i2c;
305         val |= BIT_WAIT_FOR_ES | BIT_I2C_MST_P_NSR;
306         if (port3_fifo_en)
307                 val |= BIT_SLV3_FIFO_EN;
308         if (val != inf->hw.i2c_mst_ctrl) {
309                 err = inv_i2c_single_write(inf, REG_I2C_MST_CTRL, val);
310                 if (!err)
311                         inf->hw.i2c_mst_ctrl = val;
312         }
313         return err;
314 }
315
316 /* Register I2C_SLV4_CTRL (0x34) */
317 static int nvi_i2c_slv4_ctrl_wr(struct inv_gyro_state_s *inf, bool slv4_en)
318 {
319         unsigned char val;
320         int err = 0;
321
322         val = inf->aux.delay_hw;
323         val |= (inf->aux.port[AUX_PORT_SPECIAL].nmp.ctrl &
324                                                          BITS_I2C_SLV_REG_DIS);
325         if (slv4_en)
326                 val |= BIT_SLV_EN;
327         if (val != inf->hw.i2c_slv4_ctrl) {
328                 err = inv_i2c_single_write(inf, REG_I2C_SLV4_CTRL, val);
329                 if (!err)
330                         inf->hw.i2c_slv4_ctrl = val;
331         }
332         return err;
333 }
334
335 /* Register INT_PIN_CFG (0x37) */
336 static int nvi_int_pin_cfg_wr(struct inv_gyro_state_s *inf, unsigned char val)
337 {
338         int err = 0;
339
340         if (val != inf->hw.int_pin_cfg) {
341                 err = inv_i2c_single_write(inf, REG_INT_PIN_CFG, val);
342                 if (!err)
343                         inf->hw.int_pin_cfg = val;
344         }
345         return err;
346 }
347
348 /* Register INT_ENABLE (0x38) */
349 static int nvi_int_enable_wr(struct inv_gyro_state_s *inf, bool enable)
350 {
351         unsigned char val = 0;
352         int err = 0;
353
354         if (enable) {
355                 if ((inf->hw.user_ctrl & BIT_I2C_MST_EN) ||
356                                                 inf->chip_config.gyro_enable) {
357                         val = BIT_DATA_RDY_EN;
358                 } else if (inf->chip_config.accl_enable) {
359                         if (inf->mot_enable && (!inf->mot_cnt)) {
360                                 val = BIT_MOT_EN;
361                                 if (inf->mot_dbg)
362                                         pr_info("%s motion detect on",
363                                                 __func__);
364                         } else {
365                                 val = BIT_DATA_RDY_EN;
366                         }
367                 }
368         }
369         if ((val != inf->hw.int_enable) && (inf->pm > NVI_PM_OFF)) {
370                 err = inv_i2c_single_write(inf, inf->reg->int_enable, val);
371                 if (!err)
372                         inf->hw.int_enable = val;
373         }
374         return err;
375 }
376
377 /* Register I2C_MST_DELAY_CTRL (0x67) */
378 static int nvi_i2c_mst_delay_ctrl_wr(struct inv_gyro_state_s *inf,
379                                      unsigned char i2c_mst_delay_ctrl)
380 {
381         int err = 0;
382
383         if (i2c_mst_delay_ctrl != inf->hw.i2c_mst_delay_ctrl) {
384                 err = inv_i2c_single_write(inf, REG_I2C_MST_DELAY_CTRL,
385                                            i2c_mst_delay_ctrl);
386                 if (!err)
387                         inf->hw.i2c_mst_delay_ctrl = i2c_mst_delay_ctrl;
388         }
389         return err;
390 }
391
392 /* Register MOT_DETECT_CTRL (0x69) */
393 static int nvi_mot_detect_ctrl_wr(struct inv_gyro_state_s *inf,
394                                   unsigned char val)
395 {
396         int err = 0;
397
398         if (val != inf->hw.mot_detect_ctrl) {
399                 err = inv_i2c_single_write(inf, REG_MOT_DETECT_CTRL, val);
400                 if (!err)
401                         inf->hw.mot_detect_ctrl = val;
402         }
403         return err;
404 }
405
406 /* Register USER_CTRL (0x6A) */
407 static int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf,
408                                   unsigned char val)
409 {
410         int i;
411         int err;
412         int err_t;
413
414         err_t =  inv_i2c_single_write(inf, inf->reg->user_ctrl, val);
415         for (i = 0; i < POWER_UP_TIME; i++) {
416                 val = -1;
417                 err = inv_i2c_read(inf, inf->reg->user_ctrl, 1, &val);
418                 if (!(val & (BIT_FIFO_RST | BIT_I2C_MST_RST)))
419                         break;
420
421                 mdelay(1);
422         }
423         err_t |= err;
424         inf->hw.user_ctrl = val;
425         return err_t;
426 }
427
428 /* Register USER_CTRL (0x6A) */
429 static int nvi_user_ctrl_en_wr(struct inv_gyro_state_s *inf,
430                                bool fifo_enable, bool i2c_enable)
431 {
432         unsigned char val;
433         bool en;
434         int i;
435         int err = 0;
436
437         if (inf->lpa_enable)
438                 fifo_enable = false;
439         val = 0;
440         if (fifo_enable) {
441                 for (i = 0; i < (AUX_PORT_SPECIAL - 1); i++) {
442                         if (inf->aux.port[i].fifo_en && inf->aux.port[i].hw_en)
443                                 val |= (1 << i);
444                 }
445                 if (inf->chip_config.gyro_fifo_enable)
446                         val |= (inf->chip_config.gyro_enable << 4);
447                 if (inf->chip_config.accl_fifo_enable)
448                         val |= BIT_ACCEL_OUT;
449                 if (inf->chip_config.temp_fifo_enable)
450                         val |= BIT_TEMP_FIFO_EN;
451                 if (inf->aux.port[3].fifo_en && inf->aux.port[3].hw_en)
452                         en = true;
453                 else
454                         en = false;
455                 err |= nvi_i2c_mst_ctrl_wr(inf, en);
456                 if (val || en)
457                         en = true;
458                 else
459                         en = false;
460         } else {
461                 err |= nvi_i2c_mst_ctrl_wr(inf, false);
462                 en = false;
463         }
464         err |= nvi_fifo_en_wr(inf, val);
465         val = 0;
466         if (fifo_enable && en)
467                 val |= BIT_FIFO_EN;
468         if (i2c_enable && inf->aux.enable)
469                 val |= BIT_I2C_MST_EN;
470         if (val != inf->hw.user_ctrl) {
471                 err |= inv_i2c_single_write(inf, inf->reg->user_ctrl, val);
472                 if (!err)
473                         inf->hw.user_ctrl = val;
474         }
475         return err;
476 }
477
478 /* Register PWR_MGMT_1 (0x6B) */
479 static int nvi_pwr_mgmt_1_wr(struct inv_gyro_state_s *inf, unsigned char pm1)
480 {
481         unsigned char val;
482         int i;
483         int err;
484
485         for (i = 0; i < POWER_UP_TIME; i++) {
486                 inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, pm1);
487                 val = -1;
488                 err = inv_i2c_read(inf, inf->reg->pwr_mgmt_1, 1, &val);
489                 if (!val)
490                         break;
491
492                 mdelay(1);
493         }
494         inf->hw.pwr_mgmt_1 = val;
495         return err;
496 }
497
498 static bool nvi_lpa_able(struct inv_gyro_state_s *inf)
499 {
500         bool lpa_enable;
501         int i;
502
503         if (inf->mot_enable) {
504                 lpa_enable = true;
505         } else if (inf->chip_config.lpa_delay_us) {
506                 if (inf->chip_config.accl_delay_us <
507                                                  inf->chip_config.lpa_delay_us)
508                         lpa_enable = false;
509                 else
510                         lpa_enable = true;
511         } else {
512                 lpa_enable = false;
513         }
514         for (i = 0; i < ARRAY_SIZE(nvi_lpa_delay_us_tbl); i++) {
515                 if (inf->chip_config.accl_delay_us >= nvi_lpa_delay_us_tbl[i])
516                         break;
517         }
518         inf->lpa_hw = i;
519         return lpa_enable;
520 }
521
522 static int nvi_vreg_dis(struct inv_gyro_state_s *inf, unsigned int i)
523 {
524         int err = 0;
525
526         if (inf->vreg[i].ret && (inf->vreg[i].consumer != NULL)) {
527                 err = regulator_disable(inf->vreg[i].consumer);
528                 if (!err) {
529                         inf->vreg[i].ret = 0;
530                         dev_dbg(&inf->i2c->dev, "%s %s\n",
531                                 __func__, inf->vreg[i].supply);
532                 } else {
533                         dev_err(&inf->i2c->dev, "%s %s ERR\n",
534                                 __func__, inf->vreg[i].supply);
535                 }
536         }
537         return err;
538 }
539
540 static int nvi_vreg_dis_all(struct inv_gyro_state_s *inf)
541 {
542         unsigned int i;
543         int err = 0;
544
545         for (i = ARRAY_SIZE(nvi_vregs); i > 0; i--)
546                 err |= nvi_vreg_dis(inf, (i - 1));
547         return err;
548 }
549
550 static int nvi_vreg_en(struct inv_gyro_state_s *inf, unsigned int i)
551 {
552         int err = 0;
553
554         if ((!inf->vreg[i].ret) && (inf->vreg[i].consumer != NULL)) {
555                 err = regulator_enable(inf->vreg[i].consumer);
556                 if (!err) {
557                         inf->vreg[i].ret = 1;
558                         dev_dbg(&inf->i2c->dev, "%s %s\n",
559                                 __func__, inf->vreg[i].supply);
560                         err = 1; /* flag regulator state change */
561                 } else {
562                         dev_err(&inf->i2c->dev, "%s %s ERR\n",
563                                 __func__, inf->vreg[i].supply);
564                 }
565         }
566         return err;
567 }
568
569 static int nvi_vreg_en_all(struct inv_gyro_state_s *inf)
570 {
571         unsigned i;
572         int err = 0;
573
574         for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++)
575                 err |= nvi_vreg_en(inf, i);
576         return err;
577 }
578
579 static void nvi_vreg_exit(struct inv_gyro_state_s *inf)
580 {
581         int i;
582
583         for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++) {
584                 if (inf->vreg[i].consumer != NULL) {
585                         devm_regulator_put(inf->vreg[i].consumer);
586                         inf->vreg[i].consumer = NULL;
587                         dev_dbg(&inf->i2c->dev, "%s %s\n",
588                                 __func__, inf->vreg[i].supply);
589                 }
590         }
591 }
592
593 static int nvi_vreg_init(struct inv_gyro_state_s *inf)
594 {
595         unsigned int i;
596         int err = 0;
597
598         for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++) {
599                 inf->vreg[i].supply = nvi_vregs[i];
600                 inf->vreg[i].ret = 0;
601                 inf->vreg[i].consumer = devm_regulator_get(&inf->i2c->dev,
602                                                           inf->vreg[i].supply);
603                 if (IS_ERR(inf->vreg[i].consumer)) {
604                         err = PTR_ERR(inf->vreg[i].consumer);
605                         dev_err(&inf->i2c->dev, "%s ERR %d for %s\n",
606                                 __func__, err, inf->vreg[i].supply);
607                         inf->vreg[i].consumer = NULL;
608                 } else {
609                         dev_dbg(&inf->i2c->dev, "%s %s\n",
610                                 __func__, inf->vreg[i].supply);
611                 }
612         }
613         return err;
614 }
615
616 static int nvi_pm_wr_on(struct inv_gyro_state_s *inf, u8 pm1, u8 pm2)
617 {
618         unsigned char val;
619         int i;
620         int err;
621         int err_t = 0;
622
623         err = nvi_vreg_en_all(inf);
624         if (err) {
625                 err_t |= nvi_pwr_mgmt_1_wr(inf, 0);
626                 err_t |= inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1,
627                                               BIT_RESET);
628                 for (i = 0; i < POWER_UP_TIME; i++) {
629                         val = -1;
630                         err = inv_i2c_read(inf, inf->reg->pwr_mgmt_1, 1, &val);
631                         if (!(val & BIT_RESET))
632                                 break;
633
634                         mdelay(1);
635                 }
636                 err_t |= err;
637                 err_t |= nvi_pwr_mgmt_1_wr(inf, 0);
638                 err_t |= nvi_user_ctrl_reset_wr(inf, (BIT_FIFO_RST |
639                                                       BIT_I2C_MST_RST));
640                 for (i = 0; i < AUX_PORT_MAX; i++) {
641                         inf->aux.port[i].hw_valid = false;
642                         inf->aux.port[i].hw_en = false;
643                 }
644                 memset(&inf->hw, 0, sizeof(struct nvi_hw));
645                 inf->sample_delay_us = 0;
646                 inf->mot_enable = false;
647         } else {
648                 err_t |= nvi_pwr_mgmt_1_wr(inf, 0);
649         }
650         if (pm2 != inf->hw.pwr_mgmt_2) {
651                 err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_2, pm2);
652                 if (err)
653                         err_t |= err;
654                 else
655                         inf->hw.pwr_mgmt_2 = pm2;
656         }
657         if (pm1 != inf->hw.pwr_mgmt_1) {
658                 err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, pm1);
659                 if (err)
660                         err_t |= err;
661                 else
662                         inf->hw.pwr_mgmt_1 = pm1;
663         }
664         return err_t;
665 }
666
667 static int nvi_pm_wr(struct inv_gyro_state_s *inf, int pm, int stby)
668 {
669         int err = 0;
670
671         if ((pm == inf->pm) && (stby == inf->stby))
672                 return err;
673
674         switch (pm) {
675         case NVI_PM_OFF_FORCE:
676         case NVI_PM_OFF:
677                 err = nvi_pm_wr_on(inf, BIT_SLEEP, stby);
678                 err |= nvi_vreg_dis_all(inf);
679                 break;
680
681         case NVI_PM_STDBY:
682                 err = nvi_pm_wr_on(inf, BIT_SLEEP, stby);
683                 break;
684
685         case NVI_PM_ON_CYCLE:
686                 err = nvi_pm_wr_on(inf, BIT_CYCLE, stby);
687                 break;
688
689         case NVI_PM_ON:
690                 err = nvi_pm_wr_on(inf, INV_CLK_INTERNAL, stby);
691                 break;
692
693         case NVI_PM_ON_FULL:
694                 err = nvi_pm_wr_on(inf, INV_CLK_PLL, stby);
695                 break;
696
697         default:
698                 err = -EINVAL;
699                 break;
700         }
701
702         if (err < 0) {
703                 dev_err(&inf->i2c->dev, "%s requested pm=%d ERR=%d\n",
704                         __func__, pm, err);
705                 pm = NVI_PM_ERR;
706         } else {
707                 inf->stby = stby;
708         }
709         inf->pm = pm;
710         dev_dbg(&inf->i2c->dev, "%s pm=%d stby=%x\n", __func__, pm, stby);
711         if (err > 0)
712                 err = 0;
713         return err;
714 }
715
716 static int nvi_pm(struct inv_gyro_state_s *inf, int pm_req)
717 {
718         bool irq;
719         int stby;
720         int pm;
721         int err;
722
723         nvi_int_enable_wr(inf, false);
724         inf->lpa_enable = false;
725         if ((pm_req == NVI_PM_OFF_FORCE) || (pm_req == NVI_PM_OFF)) {
726                 stby = 0x3F;
727                 pm = NVI_PM_OFF_FORCE;
728         } else {
729                 stby = ((~inf->chip_config.accl_enable) & 0x07) << 3;
730                 stby |= (~inf->chip_config.gyro_enable) & 0x07;
731                 if (inf->chip_config.gyro_enable ||
732                                         (inf->hw.user_ctrl & BIT_I2C_MST_EN)) {
733                         if (inf->chip_config.gyro_enable)
734                                 pm = NVI_PM_ON_FULL;
735                         else
736                                 pm = NVI_PM_ON;
737                 } else if (inf->chip_config.accl_enable) {
738                         if (nvi_lpa_able(inf)) {
739                                 inf->lpa_enable = true;
740                                 stby |= (inf->lpa_hw << 6);
741                                 pm = NVI_PM_ON_CYCLE;
742                         } else {
743                                 pm = NVI_PM_ON;
744                         }
745                 } else if (inf->chip_config.enable || inf->aux.bypass_lock) {
746                         pm = NVI_PM_STDBY;
747                 } else {
748                         pm = NVI_PM_OFF;
749                 }
750         }
751         if (pm_req > pm)
752                 pm = pm_req;
753         err = nvi_pm_wr(inf, pm, stby);
754         if (pm_req == NVI_PM_AUTO) {
755                 nvi_user_ctrl_en_wr(inf, true, true);
756                 irq = true;
757         } else {
758                 irq = false;
759         }
760         nvi_int_enable_wr(inf, irq);
761         return err;
762 }
763
764 static void nvi_pm_exit(struct inv_gyro_state_s *inf)
765 {
766         nvi_pm(inf, NVI_PM_OFF_FORCE);
767         nvi_vreg_exit(inf);
768 }
769
770 static int nvi_pm_init(struct inv_gyro_state_s *inf)
771 {
772         int err = 0;
773
774         nvi_vreg_init(inf);
775         inf->pm = NVI_PM_ERR;
776         inf->stby = 0;
777         err = nvi_pm(inf, NVI_PM_ON_FULL);
778         return err;
779 }
780
781 static int nvi_motion_detect_enable(struct inv_gyro_state_s *inf, u8 mot_thr)
782 {
783         int err;
784         int err_t = 0;
785
786         if (mot_thr) {
787                 err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0);
788                 if (err < 0)
789                         err_t |= err;
790                 err_t |= nvi_config_wr(inf, 0);
791                 if (!inf->hw.mot_dur)
792                         err_t |= nvi_mot_dur_wr(inf, 1);
793                 err_t |= nvi_mot_detect_ctrl_wr(inf, inf->chip_config.mot_ctrl);
794                 err_t |= nvi_mot_thr_wr(inf, mot_thr);
795                 mdelay(5);
796                 err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 7);
797                 if (err < 0)
798                         err_t |= err;
799                 if (!err_t)
800                         inf->mot_enable = true;
801         } else {
802                 err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0);
803                 if (err < 0)
804                         err_t |= err;
805         }
806         return err_t;
807 }
808
809 static int nvi_aux_delay(struct inv_gyro_state_s *inf,
810                          int port, unsigned int delay_ms)
811 {
812         unsigned char val;
813         unsigned char i;
814         unsigned int ms;
815         unsigned int delay_new;
816         int delay_rtn;
817         int err;
818
819         if (port != AUX_PORT_BYPASS)
820                 inf->aux.port[port].nmp.delay_ms = delay_ms;
821         /* determine valid delays by ports enabled */
822         delay_new = 0;
823         delay_rtn = 0;
824         for (i = 0; i < AUX_PORT_MAX; i++) {
825                 if (delay_rtn < inf->aux.port[i].nmp.delay_ms)
826                         delay_rtn = inf->aux.port[i].nmp.delay_ms;
827                 if (inf->aux.port[i].hw_en) {
828                         if (delay_new < inf->aux.port[i].nmp.delay_ms)
829                                 delay_new = inf->aux.port[i].nmp.delay_ms;
830                 }
831         }
832         if (!(inf->hw.user_ctrl & BIT_I2C_MST_EN)) {
833                 /* delay will execute when re-enabled */
834                 if (delay_ms)
835                         return delay_rtn;
836                 else
837                         return 0;
838         }
839
840         /* HW global delay */
841         for (i = 1; i < (BITS_I2C_MST_DLY - 1); i++) {
842                 ms = (inf->sample_delay_us - (inf->sample_delay_us /
843                                               (i + 1))) * 1000;
844                 if (ms >= delay_new)
845                         break;
846         }
847         inf->aux.delay_hw = i;
848         err = nvi_i2c_slv4_ctrl_wr(inf, inf->aux.port[AUX_PORT_SPECIAL].hw_en);
849         /* HW port delay enable */
850         val = BIT_DELAY_ES_SHADOW;
851         for (i = 0; i < AUX_PORT_MAX; i++) {
852                 if (inf->aux.port[i].nmp.delay_ms)
853                         val |= (1 << i);
854         }
855         nvi_i2c_mst_delay_ctrl_wr(inf, val);
856         if (delay_ms)
857                 return delay_rtn;
858         else
859                 return 0;
860 }
861
862 static int nvi_global_delay(struct inv_gyro_state_s *inf)
863 {
864         unsigned long delay_us;
865         unsigned long delay_min;
866         unsigned char val;
867         int i;
868         int err = 0;
869
870         /* find the fastest polling of all the devices */
871         delay_us = -1;
872         for (i = 0; i < AUX_PORT_MAX; i++) {
873                 if (inf->aux.port[i].enable && inf->aux.port[i].nmp.delay_us) {
874                         if (inf->aux.port[i].nmp.delay_us < delay_us)
875                                 delay_us = inf->aux.port[i].nmp.delay_us;
876                 }
877         }
878         if (inf->chip_config.gyro_enable && inf->chip_config.gyro_delay_us) {
879                 if (inf->chip_config.gyro_delay_us < delay_us)
880                         delay_us = inf->chip_config.gyro_delay_us;
881         }
882         if (inf->chip_config.accl_enable && inf->chip_config.accl_delay_us) {
883                 if (inf->chip_config.accl_delay_us < delay_us)
884                         delay_us = inf->chip_config.accl_delay_us;
885         }
886         if (delay_us == -1)
887                 delay_us = NVI_DELAY_DEFAULT; /* default if nothing found */
888         /* find what the fastest the external devices will let MPU go */
889         delay_min = 0;
890         for (i = 0; i < AUX_PORT_MAX; i++) {
891                 if (inf->aux.port[i].enable) {
892                         if (inf->aux.port[i].nmp.delay_ms > delay_min)
893                                 delay_min = inf->aux.port[i].nmp.delay_ms;
894                 }
895         }
896         if (!delay_min)
897                 /* default if nothing found */
898                 delay_min = inf->chip_config.min_delay_us;
899         else
900                 delay_min *= 1000L; /* ms => us */
901         /* set the limits */
902         if (delay_us < delay_min)
903                 delay_us = delay_min;
904         if (delay_us > MAX_FIFO_RATE)
905                 delay_us = MAX_FIFO_RATE;
906         /* program if new value */
907         if (delay_us != inf->sample_delay_us) {
908                 dev_dbg(&inf->i2c->dev, "%s %lu\n", __func__, delay_us);
909                 inf->sample_delay_us = delay_us;
910                 inf->irq_dur_us = delay_us;
911                 delay_us <<= 1;
912                 for (val = 1; val < ARRAY_SIZE(nvi_lpf_us_tbl); val++) {
913                         if (delay_us < nvi_lpf_us_tbl[val])
914                                 break;
915                 }
916                 err |= nvi_config_wr(inf, val);
917                 if (val)
918                         delay_min = 1000;
919                 else
920                         delay_min = 8000;
921                 val = inf->sample_delay_us / delay_min - 1;
922                 err |= nvi_smplrt_div_wr(inf, val);
923                 inf->last_isr_time = get_time_ns();
924         }
925         nvi_aux_delay(inf, AUX_PORT_BYPASS, 0);
926         return err;
927 }
928
929 static void nvi_aux_dbg(struct inv_gyro_state_s *inf, char *tag, int val)
930 {
931         struct nvi_mpu_port *n;
932         struct aux_port *p;
933         struct aux_ports *a;
934         unsigned char data[4];
935         int i;
936
937         if (!inf->aux.dbg)
938                 return;
939
940         pr_info("%s %s %d\n", __func__, tag, val);
941         for (i = 0; i < AUX_PORT_MAX; i++) {
942                 inv_i2c_read(inf, (REG_I2C_SLV0_ADDR + (i * 3)), 3, data);
943                 inv_i2c_read(inf, (REG_I2C_SLV0_DO + i), 1, &data[3]);
944                 pr_info("PT=%d AD=%x RG=%x CL=%x DO=%x\n",
945                         i, data[0], data[1], data[2], data[3]);
946                 n = &inf->aux.port[i].nmp;
947                 pr_info("PT=%d AD=%x RG=%x CL=%x DO=%x MS=%u US=%lu SB=%x\n",
948                         i, n->addr, n->reg, n->ctrl, n->data_out, n->delay_ms,
949                         n->delay_us, n->shutdown_bypass);
950                 p = &inf->aux.port[i];
951                 pr_info("PT=%d OF=%u EN=%x FE=%x HE=%x HD=%x HV=%x NS=%lld\n",
952                         i, p->ext_data_offset, p->enable, p->fifo_en, p->hw_en,
953                         p->hw_do, p->hw_valid, p->delay_ns);
954         }
955         a = &inf->aux;
956         pr_info("EN=%x GE=%x MD=%x GD=%lu DN=%u BE=%x BL=%d SB=%d\n",
957                 a->enable, (inf->hw.user_ctrl & BIT_I2C_MST_EN),
958                 (inf->hw.i2c_slv4_ctrl & BITS_I2C_MST_DLY),
959                 inf->sample_delay_us, a->ext_data_n,
960                 (inf->hw.int_pin_cfg & BIT_BYPASS_EN), a->bypass_lock,
961                 atomic_read(&inf->mutex.count));
962 }
963
964 static void nvi_aux_read(struct inv_gyro_state_s *inf)
965 {
966         struct aux_port *ap;
967         long long timestamp1;
968         long long timestamp2;
969         unsigned int i;
970         unsigned int len;
971         u8 *p;
972         int err;
973
974         if ((inf->aux.ext_data_n == 0) ||
975                                        (!(inf->hw.user_ctrl & BIT_I2C_MST_EN)))
976                 return;
977
978         timestamp1 = get_time_ns();
979         err = inv_i2c_read(inf, REG_EXT_SENS_DATA_00,
980                            inf->aux.ext_data_n,
981                            (unsigned char *)&inf->aux.ext_data);
982         if (err)
983                 return;
984
985         timestamp2 = get_time_ns();
986         timestamp1 = timestamp1 + ((timestamp2 - timestamp1) / 2);
987         for (i = 0; i < AUX_PORT_SPECIAL; i++) {
988                 ap = &inf->aux.port[i];
989                 if ((ap->nmp.addr & BIT_I2C_READ) &&
990                                                    (ap->nmp.handler != NULL)) {
991                         if ((unsigned long)(timestamp2 - ap->delay_ns)
992                                              >= (ap->nmp.delay_us * 1000)) {
993                                 ap->delay_ns = timestamp2;
994                                 p = &inf->aux.ext_data[ap->ext_data_offset];
995                                 len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
996                                 ap->nmp.handler(p, len, timestamp1,
997                                                 ap->nmp.ext_driver);
998                         }
999                 }
1000         }
1001 }
1002
1003 static void nvi_aux_ext_data_offset(struct inv_gyro_state_s *inf)
1004 {
1005         int i;
1006         unsigned short offset;
1007
1008         offset = 0;
1009         for (i = 0; i < AUX_PORT_SPECIAL; i++) {
1010                 if ((inf->aux.port[i].hw_en) && (inf->aux.port[i].nmp.addr &
1011                                                  BIT_I2C_READ)) {
1012                         inf->aux.port[i].ext_data_offset = offset;
1013                         offset += (inf->aux.port[i].nmp.ctrl &
1014                                    BITS_I2C_SLV_CTRL_LEN);
1015                 }
1016         }
1017         if (offset > AUX_EXT_DATA_REG_MAX) {
1018                 offset = AUX_EXT_DATA_REG_MAX;
1019                 dev_err(&inf->i2c->dev,
1020                         "%s ERR MPU slaves exceed data storage\n", __func__);
1021         }
1022         inf->aux.ext_data_n = offset;
1023         return;
1024 }
1025
1026 static int nvi_aux_port_do(struct inv_gyro_state_s *inf,
1027                            int port, u8 data_out)
1028 {
1029         unsigned char reg;
1030         int err;
1031
1032         if (port == AUX_PORT_SPECIAL)
1033                 reg = REG_I2C_SLV4_DO;
1034         else
1035                 reg = (REG_I2C_SLV0_DO + port);
1036         err = inv_i2c_single_write(inf, reg, data_out);
1037         return err;
1038 }
1039
1040 static int nvi_aux_port_data_out(struct inv_gyro_state_s *inf,
1041                                  int port, u8 data_out)
1042 {
1043         int err;
1044
1045         err = nvi_aux_port_do(inf, port, data_out);
1046         if (!err) {
1047                 inf->aux.port[port].nmp.data_out = data_out;
1048                 inf->aux.port[port].hw_do = true;
1049         } else {
1050                 inf->aux.port[port].hw_do = false;
1051         }
1052         return err;
1053 }
1054
1055 static int nvi_aux_port_wr(struct inv_gyro_state_s *inf, int port)
1056 {
1057         struct aux_port *ap;
1058         int err;
1059
1060         ap = &inf->aux.port[port];
1061         err = inv_i2c_single_write(inf, (REG_I2C_SLV0_ADDR + (port * 3)),
1062                                    ap->nmp.addr);
1063         err |= inv_i2c_single_write(inf, (REG_I2C_SLV0_REG + (port * 3)),
1064                                     ap->nmp.reg);
1065         err |= nvi_aux_port_do(inf, port, ap->nmp.data_out);
1066         return err;
1067 }
1068
1069 static int nvi_aux_port_en(struct inv_gyro_state_s *inf,
1070                            int port, bool en)
1071 {
1072         struct aux_port *ap;
1073         unsigned char reg;
1074         unsigned char val;
1075         int err = 0;
1076
1077         inf->aux.ext_data_n = 0;
1078         ap = &inf->aux.port[port];
1079         if ((!ap->hw_valid) && en) {
1080                 err = nvi_aux_port_wr(inf, port);
1081                 if (!err) {
1082                         ap->hw_valid = true;
1083                         ap->hw_do = true;
1084                 }
1085         }
1086         if ((!ap->hw_do) && en)
1087                 nvi_aux_port_data_out(inf, port, ap->nmp.data_out);
1088         if (port == AUX_PORT_SPECIAL) {
1089                 err = nvi_i2c_slv4_ctrl_wr(inf, en);
1090         } else {
1091                 reg = (REG_I2C_SLV0_CTRL + (port * 3));
1092                 if (en)
1093                         val = (ap->nmp.ctrl | BIT_SLV_EN);
1094                 else
1095                         val = 0;
1096                 err = inv_i2c_single_write(inf, reg, val);
1097         }
1098         if (!err) {
1099                 ap->hw_en = en;
1100                 nvi_aux_ext_data_offset(inf);
1101         }
1102         return err;
1103 }
1104
1105 static int nvi_aux_enable(struct inv_gyro_state_s *inf, bool enable)
1106 {
1107         bool en;
1108         unsigned int i;
1109         int err;
1110
1111         if (inf->hw.int_pin_cfg & BIT_BYPASS_EN)
1112                 enable = false;
1113
1114         en = false;
1115         if (enable) {
1116                 /* global enable is honored only if a port is enabled */
1117                 for (i = 0; i < AUX_PORT_MAX; i++) {
1118                         if (inf->aux.port[i].enable) {
1119                                 en = true;
1120                                 break;
1121                         }
1122                 }
1123                 if (en == (inf->hw.user_ctrl & BIT_I2C_MST_EN)) {
1124                         /* if already on then just update delays */
1125                         nvi_global_delay(inf);
1126                 }
1127         }
1128         inf->aux.enable = en;
1129         if ((inf->hw.user_ctrl & BIT_I2C_MST_EN) == en)
1130                 return 0;
1131
1132         if (en) {
1133                 for (i = 0; i < AUX_PORT_MAX; i++) {
1134                         if (inf->aux.port[i].enable)
1135                                 err |= nvi_aux_port_en(inf, i, true);
1136                 }
1137                 nvi_motion_detect_enable(inf, 0);
1138         } else {
1139                 for (i = 0; i < AUX_PORT_MAX; i++) {
1140                         if (inf->aux.port[i].hw_valid)
1141                                 nvi_aux_port_en(inf, i, false);
1142                 }
1143         }
1144         err = nvi_global_delay(inf);
1145         err |= nvi_user_ctrl_en_wr(inf, true, en);
1146         return err;
1147 }
1148
1149 static int nvi_aux_port_enable(struct inv_gyro_state_s *inf,
1150                                int port, bool enable, bool fifo_enable)
1151 {
1152         struct aux_port *ap;
1153         int err;
1154
1155         ap = &inf->aux.port[port];
1156         ap->enable = enable;
1157         ap->fifo_en = false;
1158         if (enable && (inf->hw.int_pin_cfg & BIT_BYPASS_EN))
1159                 return 0;
1160
1161         err = nvi_aux_port_en(inf, port, enable);
1162         err |= nvi_aux_enable(inf, true);
1163         return err;
1164 }
1165
1166 static int nvi_reset(struct inv_gyro_state_s *inf,
1167                      bool reset_fifo, bool reset_i2c)
1168 {
1169         unsigned long flags;
1170         unsigned char val;
1171         int i;
1172         int err;
1173
1174         err = nvi_int_enable_wr(inf, false);
1175         val = 0;
1176         if (reset_i2c) {
1177                 /* nvi_aux_bypass_enable(inf, false)? */
1178                 err |= nvi_aux_enable(inf, false);
1179                 for (i = 0; i < AUX_PORT_MAX; i++) {
1180                         inf->aux.port[i].hw_valid = false;
1181                         inf->aux.port[i].hw_en = false;
1182                 }
1183                 inf->aux.need_reset = false;
1184                 val |= BIT_I2C_MST_RST;
1185         }
1186         if (reset_fifo)
1187                 val |= BIT_FIFO_RST;
1188         err |= nvi_user_ctrl_en_wr(inf, ~reset_fifo, ~reset_i2c);
1189         val |= inf->hw.user_ctrl;
1190         err |= nvi_user_ctrl_reset_wr(inf, val);
1191         if (reset_fifo) {
1192                 spin_lock_irqsave(&inf->time_stamp_lock, flags);
1193                 kfifo_reset(&inf->trigger.timestamps);
1194                 spin_unlock_irqrestore(&inf->time_stamp_lock, flags);
1195                 inf->last_isr_time = get_time_ns();
1196         }
1197         if (reset_i2c)
1198                 err |= nvi_aux_enable(inf, true);
1199         else
1200                 err |= nvi_user_ctrl_en_wr(inf, true, true);
1201         err |= nvi_int_enable_wr(inf, true);
1202         return err;
1203 }
1204
1205 static int nvi_aux_port_free(struct inv_gyro_state_s *inf, int port)
1206 {
1207         bool hw_valid;
1208         int err = 0;
1209
1210         hw_valid = inf->aux.port[port].hw_valid;
1211         memset(&inf->aux.port[port], 0, sizeof(struct aux_port));
1212         if (hw_valid) {
1213                 nvi_aux_port_wr(inf, port);
1214                 nvi_aux_port_en(inf, port, false);
1215                 nvi_aux_enable(inf, false);
1216                 nvi_aux_enable(inf, true);
1217                 if (port != AUX_PORT_SPECIAL)
1218                         inf->aux.need_reset = true;
1219         }
1220         return err;
1221 }
1222
1223 static int nvi_aux_port_alloc(struct inv_gyro_state_s *inf,
1224                               struct nvi_mpu_port *nmp, int port)
1225 {
1226         int i;
1227
1228         if (inf->aux.need_reset)
1229                 nvi_reset(inf, false, true);
1230         if (port < 0) {
1231                 for (i = 0; i < AUX_PORT_SPECIAL; i++) {
1232                         if (inf->aux.port[i].nmp.addr == 0)
1233                                 break;
1234                 }
1235                 if (i == AUX_PORT_SPECIAL)
1236                         return -ENODEV;
1237         } else {
1238                 if (inf->aux.port[port].nmp.addr == 0)
1239                         i = port;
1240                 else
1241                         return -ENODEV;
1242         }
1243
1244         memset(&inf->aux.port[i], 0, sizeof(struct aux_port));
1245         memcpy(&inf->aux.port[i].nmp, nmp, sizeof(struct nvi_mpu_port));
1246         return i;
1247 }
1248
1249 static int nvi_aux_bypass_enable(struct inv_gyro_state_s *inf, bool enable)
1250 {
1251         unsigned char val;
1252         int err;
1253
1254         if ((bool)(inf->hw.int_pin_cfg & BIT_BYPASS_EN) == enable)
1255                 return 0;
1256
1257         val = inf->hw.int_pin_cfg;
1258         if (enable) {
1259                 err = nvi_aux_enable(inf, false);
1260                 if (!err) {
1261                         val |= BIT_BYPASS_EN;
1262                         err = nvi_int_pin_cfg_wr(inf, val);
1263                 }
1264         } else {
1265                 val &= ~BIT_BYPASS_EN;
1266                 err = nvi_int_pin_cfg_wr(inf, val);
1267                 if (!err)
1268                         nvi_aux_enable(inf, true);
1269         }
1270         return err;
1271 }
1272
1273 static int nvi_aux_bypass_request(struct inv_gyro_state_s *inf, bool enable)
1274 {
1275         int err = 0;
1276
1277         if ((bool)(inf->hw.int_pin_cfg & BIT_BYPASS_EN) == enable) {
1278                 inf->aux.bypass_lock++;
1279         } else {
1280                 if (inf->aux.bypass_lock) {
1281                         err = -EBUSY;
1282                 } else {
1283                         err = nvi_aux_bypass_enable(inf, enable);
1284                         if (err)
1285                                 dev_err(&inf->i2c->dev, "%s ERR=%d\n",
1286                                         __func__, err);
1287                         else
1288                                 inf->aux.bypass_lock++;
1289                 }
1290         }
1291         return err;
1292 }
1293
1294 static int nvi_aux_bypass_release(struct inv_gyro_state_s *inf)
1295 {
1296         int err;
1297
1298         if (inf->aux.bypass_lock)
1299                 inf->aux.bypass_lock--;
1300         if (!inf->aux.bypass_lock) {
1301                 err = nvi_aux_bypass_enable(inf, false);
1302                 if (err)
1303                         dev_err(&inf->i2c->dev, "%s ERR=%d\n", __func__, err);
1304         }
1305         return 0;
1306 }
1307
1308 static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf,
1309                              struct nvi_mpu_port *nmp, u8 *data)
1310 {
1311         unsigned char val;
1312         int i;
1313         int err;
1314
1315         /* turn off bypass */
1316         err = nvi_aux_bypass_request(inf, false);
1317         if (err)
1318                 return -EBUSY;
1319
1320         /* grab the special port */
1321         err = nvi_aux_port_alloc(inf, nmp, AUX_PORT_SPECIAL);
1322         if (err != AUX_PORT_SPECIAL) {
1323                 nvi_aux_bypass_release(inf);
1324                 return -EBUSY;
1325         }
1326
1327         /* enable it */
1328         inf->aux.port[AUX_PORT_SPECIAL].nmp.delay_ms = 0;
1329         err = nvi_aux_port_enable(inf, AUX_PORT_SPECIAL, true, false);
1330         if (err) {
1331                 nvi_aux_port_free(inf, AUX_PORT_SPECIAL);
1332                 nvi_aux_bypass_release(inf);
1333                 return -EBUSY;
1334         }
1335
1336         /* now turn off all the other ports for fastest response */
1337         for (i = 0; i < AUX_PORT_SPECIAL; i++) {
1338                 if (inf->aux.port[i].hw_valid)
1339                         nvi_aux_port_en(inf, i, false);
1340         }
1341         /* start reading the results */
1342         for (i = 0; i < AUX_DEV_VALID_READ_MAX; i++) {
1343                 mdelay(1);
1344                 val = 0;
1345                 err = inv_i2c_read(inf, REG_I2C_MST_STATUS, 1, &val);
1346                 if (err)
1347                         continue;
1348
1349                 if (val & 0x50)
1350                         break;
1351         }
1352         /* these will restore all previously disabled ports */
1353         nvi_aux_bypass_release(inf);
1354         nvi_aux_port_free(inf, AUX_PORT_SPECIAL);
1355         if (i == AUX_DEV_VALID_READ_MAX)
1356                 return -ENODEV;
1357
1358         if (val & 0x10) /* NACK */
1359                 return -EIO;
1360
1361         if (nmp->addr & BIT_I2C_READ) {
1362                 err = inv_i2c_read(inf, REG_I2C_SLV4_DI, 1, &val);
1363                 if (err)
1364                         return -EBUSY;
1365
1366                 *data = (u8)val;
1367                 dev_info(&inf->i2c->dev, "%s MPU read 0x%x from device 0x%x\n",
1368                         __func__, val, (nmp->addr & ~BIT_I2C_READ));
1369         } else {
1370                 dev_info(&inf->i2c->dev, "%s MPU found device 0x%x\n",
1371                         __func__, (nmp->addr & ~BIT_I2C_READ));
1372         }
1373         return 0;
1374 }
1375
1376 static int nvi_aux_mpu_call_pre(struct inv_gyro_state_s *inf, int port)
1377 {
1378         if ((port < 0) || (port >= AUX_PORT_SPECIAL))
1379                 return -EINVAL;
1380
1381         if (inf->shutdown)
1382                 return -EPERM;
1383
1384         if (!inf->aux.port[port].nmp.addr)
1385                 return -EINVAL;
1386
1387         mutex_lock(&inf->mutex);
1388         return 0;
1389 }
1390
1391 static int nvi_aux_mpu_call_post(struct inv_gyro_state_s *inf,
1392                                  char *tag, int err)
1393 {
1394         if (err < 0)
1395                 err = -EBUSY;
1396         mutex_unlock(&inf->mutex);
1397         nvi_aux_dbg(inf, tag, err);
1398         return err;
1399 }
1400
1401 /* See the mpu.h file for details on the nvi_mpu_ calls.
1402  */
1403 int nvi_mpu_dev_valid(struct nvi_mpu_port *nmp, u8 *data)
1404 {
1405         struct inv_gyro_state_s *inf;
1406         int err;
1407
1408         inf = inf_local;
1409         if (inf != NULL) {
1410                 if (inf->aux.dbg)
1411                         pr_info("%s\n", __func__);
1412         } else {
1413                 pr_debug("%s\n", __func__);
1414                 return -EAGAIN;
1415         }
1416
1417         if (nmp == NULL)
1418                 return -EINVAL;
1419
1420         if ((nmp->addr & BIT_I2C_READ) && (data == NULL))
1421                 return -EINVAL;
1422
1423         if (inf->shutdown)
1424                 return -EPERM;
1425
1426         mutex_lock(&inf->mutex);
1427         nvi_pm(inf, NVI_PM_ON);
1428         err = nvi_aux_dev_valid(inf, nmp, data);
1429         nvi_pm(inf, NVI_PM_AUTO);
1430         mutex_unlock(&inf->mutex);
1431         nvi_aux_dbg(inf, "nvi_mpu_dev_valid err: ", err);
1432         return err;
1433 }
1434 EXPORT_SYMBOL(nvi_mpu_dev_valid);
1435
1436 int nvi_mpu_port_alloc(struct nvi_mpu_port *nmp)
1437 {
1438         struct inv_gyro_state_s *inf;
1439         int err;
1440
1441         inf = inf_local;
1442         if (inf != NULL) {
1443                 if (inf->aux.dbg)
1444                         pr_info("%s\n", __func__);
1445         } else {
1446                 pr_debug("%s\n", __func__);
1447                 return -EAGAIN;
1448         }
1449
1450         if (nmp == NULL)
1451                 return -EINVAL;
1452
1453         if (!(nmp->ctrl & BITS_I2C_SLV_CTRL_LEN))
1454                 return -EINVAL;
1455
1456         if (inf->shutdown)
1457                 return -EPERM;
1458
1459         mutex_lock(&inf->mutex);
1460         nvi_pm(inf, NVI_PM_ON);
1461         err = nvi_aux_port_alloc(inf, nmp, -1);
1462         nvi_pm(inf, NVI_PM_AUTO);
1463         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_port_alloc err/port: ", err);
1464         return err;
1465 }
1466 EXPORT_SYMBOL(nvi_mpu_port_alloc);
1467
1468 int nvi_mpu_port_free(int port)
1469 {
1470         struct inv_gyro_state_s *inf;
1471         int err;
1472
1473         inf = inf_local;
1474         if (inf != NULL) {
1475                 if (inf->aux.dbg)
1476                         pr_info("%s port %d\n", __func__, port);
1477         } else {
1478                 pr_debug("%s port %d\n", __func__, port);
1479                 return -EAGAIN;
1480         }
1481
1482         err = nvi_aux_mpu_call_pre(inf, port);
1483         if (err)
1484                 return err;
1485
1486         nvi_pm(inf, NVI_PM_ON);
1487         err = nvi_aux_port_free(inf, port);
1488         nvi_pm(inf, NVI_PM_AUTO);
1489         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_port_free err: ", err);
1490         return err;
1491 }
1492 EXPORT_SYMBOL(nvi_mpu_port_free);
1493
1494 int nvi_mpu_enable(int port, bool enable, bool fifo_enable)
1495 {
1496         struct inv_gyro_state_s *inf;
1497         int err;
1498
1499         inf = inf_local;
1500         if (inf != NULL) {
1501                 if (inf->aux.dbg)
1502                         pr_info("%s port %d: %x\n", __func__, port, enable);
1503         } else {
1504                 pr_debug("%s port %d: %x\n", __func__, port, enable);
1505                 return -EAGAIN;
1506         }
1507
1508         err = nvi_aux_mpu_call_pre(inf, port);
1509         if (err)
1510                 return err;
1511
1512         nvi_pm(inf, NVI_PM_ON);
1513         err = nvi_aux_port_enable(inf, port, enable, fifo_enable);
1514         nvi_pm(inf, NVI_PM_AUTO);
1515         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_enable err: ", err);
1516         return err;
1517 }
1518 EXPORT_SYMBOL(nvi_mpu_enable);
1519
1520 int nvi_mpu_delay_ms(int port, u8 delay_ms)
1521 {
1522         struct inv_gyro_state_s *inf;
1523         int err;
1524
1525         inf = inf_local;
1526         if (inf != NULL) {
1527                 if (inf->aux.dbg)
1528                         pr_info("%s port %d: %u\n", __func__, port, delay_ms);
1529         } else {
1530                 pr_debug("%s port %d: %u\n", __func__, port, delay_ms);
1531                 return -EAGAIN;
1532         }
1533
1534         err = nvi_aux_mpu_call_pre(inf, port);
1535         if (err)
1536                 return err;
1537
1538         if (inf->aux.port[port].hw_en) {
1539                 err = nvi_aux_delay(inf, port, delay_ms);
1540                 nvi_global_delay(inf);
1541         } else {
1542                 inf->aux.port[port].nmp.delay_ms = delay_ms;
1543         }
1544         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_delay_ms err: ", err);
1545         return err;
1546 }
1547 EXPORT_SYMBOL(nvi_mpu_delay_ms);
1548
1549 int nvi_mpu_delay_us(int port, unsigned long delay_us)
1550 {
1551         struct inv_gyro_state_s *inf;
1552         int err;
1553
1554         inf = inf_local;
1555         if (inf != NULL) {
1556                 if (inf->aux.dbg)
1557                         pr_info("%s port %d: %lu\n", __func__, port, delay_us);
1558         } else {
1559                 pr_debug("%s port %d: %lu\n", __func__, port, delay_us);
1560                 return -EAGAIN;
1561         }
1562
1563         err = nvi_aux_mpu_call_pre(inf, port);
1564         if (err)
1565                 return err;
1566
1567         inf->aux.port[port].nmp.delay_us = delay_us;
1568         if (inf->aux.port[port].hw_en)
1569                 err = nvi_global_delay(inf);
1570         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_delay_us err: ", err);
1571         return err;
1572 }
1573 EXPORT_SYMBOL(nvi_mpu_delay_us);
1574
1575 int nvi_mpu_data_out(int port, u8 data_out)
1576 {
1577         struct inv_gyro_state_s *inf;
1578         int err = 0;
1579
1580         inf = inf_local;
1581         if (inf == NULL)
1582                 return -EAGAIN;
1583
1584         err = nvi_aux_mpu_call_pre(inf, port);
1585         if (err)
1586                 return err;
1587
1588         if (inf->aux.port[port].hw_en) {
1589                 err = nvi_aux_port_data_out(inf, port, data_out);
1590         } else {
1591                 inf->aux.port[port].nmp.data_out = data_out;
1592                 inf->aux.port[port].hw_do = false;
1593         }
1594         if (err < 0)
1595                 err = -EBUSY;
1596         mutex_unlock(&inf->mutex);
1597         return err;
1598 }
1599 EXPORT_SYMBOL(nvi_mpu_data_out);
1600
1601 int nvi_mpu_bypass_request(bool enable)
1602 {
1603         struct inv_gyro_state_s *inf;
1604         int err;
1605
1606         inf = inf_local;
1607         if (inf != NULL) {
1608                 if (inf->aux.dbg)
1609                         pr_info("%s\n", __func__);
1610         } else {
1611                 pr_debug("%s\n", __func__);
1612                 return -EAGAIN;
1613         }
1614
1615         if (inf->shutdown)
1616                 return -EPERM;
1617
1618         mutex_lock(&inf->mutex);
1619         nvi_pm(inf, NVI_PM_ON);
1620         err = nvi_aux_bypass_request(inf, enable);
1621         nvi_pm(inf, NVI_PM_AUTO);
1622         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_bypass_request err: ", err);
1623         return err;
1624 }
1625 EXPORT_SYMBOL(nvi_mpu_bypass_request);
1626
1627 int nvi_mpu_bypass_release(void)
1628 {
1629         struct inv_gyro_state_s *inf;
1630         int err;
1631
1632         inf = inf_local;
1633         if (inf != NULL) {
1634                 if (inf->aux.dbg)
1635                         pr_info("%s\n", __func__);
1636         } else {
1637                 pr_debug("%s\n", __func__);
1638                 return -EAGAIN;
1639         }
1640
1641         if (inf->shutdown)
1642                 return -EPERM;
1643
1644         mutex_lock(&inf->mutex);
1645         nvi_pm(inf, NVI_PM_ON);
1646         err = nvi_aux_bypass_release(inf);
1647         nvi_pm(inf, NVI_PM_AUTO);
1648         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_bypass_release err: ", err);
1649         return err;
1650 }
1651 EXPORT_SYMBOL(nvi_mpu_bypass_release);
1652
1653
1654 static int nvi_gyro_enable(struct inv_gyro_state_s *inf,
1655                            unsigned char enable, unsigned char fifo_enable)
1656 {
1657         unsigned char enable_old;
1658         unsigned char fifo_enable_old;
1659         int err;
1660         int err_t;
1661
1662         enable_old = inf->chip_config.gyro_enable;
1663         fifo_enable_old = inf->chip_config.gyro_fifo_enable;
1664         inf->chip_config.gyro_fifo_enable = fifo_enable;
1665         inf->chip_config.gyro_enable = enable;
1666         err_t = nvi_pm(inf, NVI_PM_ON_FULL);
1667         if (enable != enable_old) {
1668                 if (enable) {
1669                         err = nvi_gyro_config_wr(inf,
1670                                                  inf->chip_config.gyro_fsr);
1671                         if (err < 0)
1672                                 err_t |= err;
1673                         nvi_motion_detect_enable(inf, 0);
1674                 }
1675                 nvi_global_delay(inf);
1676         }
1677         if (fifo_enable_old != fifo_enable)
1678                 err_t = nvi_reset(inf, true, false);
1679         if (err_t) {
1680                 inf->chip_config.gyro_enable = enable_old;
1681                 inf->chip_config.gyro_fifo_enable = fifo_enable_old;
1682         }
1683         err_t |= nvi_pm(inf, NVI_PM_AUTO);
1684         return err_t;
1685 }
1686
1687 static int nvi_accl_enable(struct inv_gyro_state_s *inf,
1688                            unsigned char enable, unsigned char fifo_enable)
1689 {
1690         unsigned char enable_old;
1691         unsigned char fifo_enable_old;
1692         int err;
1693         int err_t;
1694
1695         enable_old = inf->chip_config.accl_enable;
1696         fifo_enable_old = inf->chip_config.accl_fifo_enable;
1697         inf->chip_config.accl_fifo_enable = fifo_enable;
1698         inf->chip_config.accl_enable = enable;
1699         err_t = nvi_pm(inf, NVI_PM_ON);
1700         if (enable != enable_old) {
1701                 if (enable) {
1702                         err = nvi_accel_config_wr(inf,
1703                                                  inf->chip_config.accl_fsr, 0);
1704                         if (err < 0)
1705                                 err_t |= err;
1706                 }
1707                 nvi_global_delay(inf);
1708         }
1709         if (fifo_enable_old != fifo_enable)
1710                 err_t = nvi_reset(inf, true, false);
1711         if (err_t) {
1712                 inf->chip_config.accl_enable = enable_old;
1713                 inf->chip_config.accl_fifo_enable = fifo_enable_old;
1714         }
1715         err_t |= nvi_pm(inf, NVI_PM_AUTO);
1716         return err_t;
1717 }
1718
1719
1720 static ssize_t nvi_gyro_enable_store(struct device *dev,
1721                                      struct device_attribute *attr,
1722                                      const char *buf, size_t count)
1723 {
1724         struct inv_gyro_state_s *inf;
1725         unsigned char enable;
1726         unsigned char fifo_enable;
1727         int err;
1728
1729         inf = dev_get_drvdata(dev);
1730         err = kstrtou8(buf, 10, &enable);
1731         if (err)
1732                 return -EINVAL;
1733
1734         if (enable > 7)
1735                 return -EINVAL;
1736
1737         mutex_lock(&inf->mutex);
1738         dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
1739         if (enable != inf->chip_config.gyro_enable) {
1740                 if (enable)
1741                         fifo_enable = inf->chip_config.gyro_fifo_enable;
1742                 else
1743                         fifo_enable = 0;
1744                 err = nvi_gyro_enable(inf, enable, fifo_enable);
1745         }
1746         mutex_unlock(&inf->mutex);
1747         if (err) {
1748                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
1749                         __func__, enable, err);
1750                 return err;
1751         }
1752
1753         return count;
1754 }
1755
1756 ssize_t nvi_gyro_enable_show(struct device *dev,
1757                              struct device_attribute *attr, char *buf)
1758 {
1759         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
1760
1761         return sprintf(buf, "%u\n", inf->chip_config.gyro_enable);
1762 }
1763
1764 ssize_t nvi_gyro_fifo_enable_store(struct device *dev,
1765                                    struct device_attribute *attr,
1766                                    const char *buf, size_t count)
1767 {
1768         struct inv_gyro_state_s *inf;
1769         unsigned char fifo_enable;
1770         unsigned char enable;
1771         int err;
1772
1773         inf = dev_get_drvdata(dev);
1774         err = kstrtou8(buf, 10, &fifo_enable);
1775         if (err)
1776                 return -EINVAL;
1777
1778         mutex_lock(&inf->mutex);
1779         dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
1780         enable = inf->chip_config.gyro_enable;
1781         if (fifo_enable) {
1782                 fifo_enable = 1;
1783                 if (!enable)
1784                         enable = 7;
1785         }
1786         if (fifo_enable != inf->chip_config.gyro_fifo_enable)
1787                 err = nvi_gyro_enable(inf, enable, fifo_enable);
1788         mutex_unlock(&inf->mutex);
1789         if (err) {
1790                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
1791                         __func__, fifo_enable, err);
1792                 return err;
1793         }
1794
1795         return count;
1796 }
1797
1798 ssize_t nvi_gyro_fifo_enable_show(struct device *dev,
1799                                   struct device_attribute *attr, char *buf)
1800 {
1801         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
1802
1803         return sprintf(buf, "%u\n", inf->chip_config.gyro_fifo_enable);
1804 }
1805
1806 static ssize_t inv_gyro_delay_store(struct device *dev,
1807                                     struct device_attribute *attr,
1808                                     const char *buf, size_t count)
1809 {
1810         struct inv_gyro_state_s *inf;
1811         unsigned long gyro_delay_us;
1812         unsigned long gyro_delay_us_old;
1813         int err;
1814
1815         inf = dev_get_drvdata(dev);
1816         err = kstrtoul(buf, 10, &gyro_delay_us);
1817         if (err)
1818                 return err;
1819
1820         mutex_lock(&inf->mutex);
1821         dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, gyro_delay_us);
1822         if (gyro_delay_us < NVI_INPUT_GYRO_DELAY_US_MIN)
1823                 gyro_delay_us = NVI_INPUT_GYRO_DELAY_US_MIN;
1824         if (gyro_delay_us != inf->chip_config.gyro_delay_us) {
1825                 gyro_delay_us_old = inf->chip_config.gyro_delay_us;
1826                 inf->chip_config.gyro_delay_us = gyro_delay_us;
1827                 if (inf->chip_config.gyro_enable) {
1828                         err = nvi_global_delay(inf);
1829                         if (err)
1830                                 inf->chip_config.gyro_delay_us =
1831                                                              gyro_delay_us_old;
1832                 }
1833         }
1834         mutex_unlock(&inf->mutex);
1835         if (err) {
1836                 dev_err(&inf->i2c->dev, "%s: %lu ERR=%d\n",
1837                         __func__, gyro_delay_us, err);
1838                 return err;
1839         }
1840
1841         return count;
1842 }
1843
1844 static ssize_t nvi_gyro_resolution_store(struct device *dev,
1845                                          struct device_attribute *attr,
1846                                          const char *buf, size_t count)
1847 {
1848         struct inv_gyro_state_s *inf;
1849         unsigned int resolution;
1850
1851         inf = dev_get_drvdata(dev);
1852         if (kstrtouint(buf, 10, &resolution))
1853                 return -EINVAL;
1854
1855         dev_dbg(&inf->i2c->dev, "%s %u", __func__, resolution);
1856         inf->chip_config.gyro_resolution = resolution;
1857         return count;
1858 }
1859
1860 static ssize_t nvi_gyro_resolution_show(struct device *dev,
1861                                         struct device_attribute *attr,
1862                                         char *buf)
1863 {
1864         struct inv_gyro_state_s *inf;
1865         unsigned int resolution;
1866
1867         inf = dev_get_drvdata(dev);
1868         if (inf->chip_config.gyro_enable)
1869                 resolution = inf->chip_config.gyro_resolution;
1870         else
1871                 resolution = GYRO_INPUT_RESOLUTION;
1872         return sprintf(buf, "%u\n", resolution);
1873 }
1874
1875 static ssize_t nvi_gyro_delay_show(struct device *dev,
1876                                    struct device_attribute *attr, char *buf)
1877 {
1878         struct inv_gyro_state_s *inf;
1879
1880         inf = dev_get_drvdata(dev);
1881         if (inf->chip_config.gyro_enable)
1882                 return sprintf(buf, "%lu\n", inf->sample_delay_us);
1883
1884         return sprintf(buf, "%d\n", NVI_INPUT_GYRO_DELAY_US_MIN);
1885 }
1886
1887 static ssize_t nvi_gyro_max_range_store(struct device *dev,
1888                                         struct device_attribute *attr,
1889                                         const char *buf, size_t count)
1890 {
1891         struct inv_gyro_state_s *inf;
1892         unsigned char fsr;
1893         int err;
1894
1895         inf = dev_get_drvdata(dev);
1896         err = kstrtou8(buf, 10, &fsr);
1897         if (err)
1898                 return -EINVAL;
1899
1900         if (fsr > 3)
1901                 return -EINVAL;
1902
1903         mutex_lock(&inf->mutex);
1904         dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
1905         if (fsr != inf->chip_config.gyro_fsr) {
1906                 if (inf->chip_config.gyro_enable) {
1907                         err = nvi_gyro_config_wr(inf, fsr);
1908                         if (err > 0)
1909                                 /* reset fifo to purge old data */
1910                                 nvi_reset(inf, true, false);
1911                 }
1912                 if (err >= 0)
1913                         inf->chip_config.gyro_fsr = fsr;
1914         }
1915         mutex_unlock(&inf->mutex);
1916         if (err < 0) {
1917                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", __func__, fsr, err);
1918                 return err;
1919         }
1920
1921         return count;
1922 }
1923
1924 ssize_t nvi_gyro_max_range_show(struct device *dev,
1925                                        struct device_attribute *attr, char *buf)
1926 {
1927         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
1928         unsigned int range;
1929
1930         if (inf->chip_config.gyro_enable)
1931                 range = inf->chip_config.gyro_fsr;
1932         else
1933                 range = (1 << inf->chip_config.gyro_fsr) * 250;
1934         return sprintf(buf, "%u\n", range);
1935 }
1936
1937 static ssize_t nvi_accl_enable_store(struct device *dev,
1938                                      struct device_attribute *attr,
1939                                      const char *buf, size_t count)
1940 {
1941         struct inv_gyro_state_s *inf;
1942         unsigned char enable;
1943         unsigned char fifo_enable;
1944         int err;
1945
1946         inf = dev_get_drvdata(dev);
1947         err = kstrtou8(buf, 10, &enable);
1948         if (err)
1949                 return -EINVAL;
1950
1951         if (enable > 7)
1952                 return -EINVAL;
1953
1954         mutex_lock(&inf->mutex);
1955         dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
1956         if (enable != inf->chip_config.accl_enable) {
1957                 if (enable)
1958                         fifo_enable = inf->chip_config.accl_fifo_enable;
1959                 else
1960                         fifo_enable = 0;
1961                 err = nvi_accl_enable(inf, enable, fifo_enable);
1962         }
1963         mutex_unlock(&inf->mutex);
1964         if (err) {
1965                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
1966                         __func__, enable, err);
1967                 return err;
1968         }
1969
1970         return count;
1971 }
1972
1973 ssize_t nvi_accl_enable_show(struct device *dev,
1974                              struct device_attribute *attr, char *buf)
1975 {
1976         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
1977
1978         return sprintf(buf, "%u\n", inf->chip_config.accl_enable);
1979 }
1980
1981 static ssize_t nvi_accl_fifo_enable_store(struct device *dev,
1982                                           struct device_attribute *attr,
1983                                           const char *buf, size_t count)
1984 {
1985         struct inv_gyro_state_s *inf;
1986         unsigned char fifo_enable;
1987         unsigned char enable;
1988         int err;
1989
1990         inf = dev_get_drvdata(dev);
1991         err = kstrtou8(buf, 10, &fifo_enable);
1992         if (err)
1993                 return -EINVAL;
1994
1995         mutex_lock(&inf->mutex);
1996         dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
1997         enable = inf->chip_config.accl_enable;
1998         if (fifo_enable) {
1999                 fifo_enable = 1;
2000                 if (!enable)
2001                         enable = 7;
2002         }
2003         if (fifo_enable != inf->chip_config.accl_fifo_enable)
2004                 err = nvi_accl_enable(inf, enable, fifo_enable);
2005         mutex_unlock(&inf->mutex);
2006         if (err) {
2007                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
2008                         __func__, fifo_enable, err);
2009                 return err;
2010         }
2011
2012         return count;
2013 }
2014
2015 ssize_t nvi_accl_fifo_enable_show(struct device *dev,
2016                                   struct device_attribute *attr, char *buf)
2017 {
2018         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2019
2020         return sprintf(buf, "%u\n", inf->chip_config.accl_fifo_enable);
2021 }
2022
2023 static ssize_t nvi_accl_delay_store(struct device *dev,
2024                                     struct device_attribute *attr,
2025                                     const char *buf, size_t count)
2026 {
2027         struct inv_gyro_state_s *inf;
2028         unsigned long accl_delay_us;
2029         unsigned long accl_delay_us_old;
2030         int err;
2031
2032         inf = dev_get_drvdata(dev);
2033         err = kstrtoul(buf, 10, &accl_delay_us);
2034         if (err)
2035                 return err;
2036
2037         mutex_lock(&inf->mutex);
2038         dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, accl_delay_us);
2039         if (accl_delay_us < NVI_INPUT_ACCL_DELAY_US_MIN)
2040                 accl_delay_us = NVI_INPUT_ACCL_DELAY_US_MIN;
2041         if (accl_delay_us != inf->chip_config.accl_delay_us) {
2042                 accl_delay_us_old = inf->chip_config.accl_delay_us;
2043                 inf->chip_config.accl_delay_us = accl_delay_us;
2044                 if (inf->chip_config.accl_enable) {
2045                         err = nvi_global_delay(inf);
2046                         if (err)
2047                                 inf->chip_config.accl_delay_us =
2048                                                              accl_delay_us_old;
2049                         else
2050                                 nvi_pm(inf, NVI_PM_AUTO);
2051                 }
2052         }
2053         mutex_unlock(&inf->mutex);
2054         if (err) {
2055                 dev_err(&inf->i2c->dev, "%s: %lu ERR=%d\n",
2056                         __func__, accl_delay_us, err);
2057                 return err;
2058         }
2059
2060         return count;
2061 }
2062
2063 ssize_t nvi_accl_delay_show(struct device *dev,
2064                             struct device_attribute *attr, char *buf)
2065 {
2066         struct inv_gyro_state_s *inf;
2067
2068         inf = dev_get_drvdata(dev);
2069         if (inf->chip_config.accl_enable)
2070                 return sprintf(buf, "%lu\n", inf->sample_delay_us);
2071
2072         return sprintf(buf, "%d\n", NVI_INPUT_ACCL_DELAY_US_MIN);
2073 }
2074
2075 static ssize_t nvi_accl_resolution_store(struct device *dev,
2076                                          struct device_attribute *attr,
2077                                          const char *buf, size_t count)
2078 {
2079         struct inv_gyro_state_s *inf;
2080         unsigned int resolution;
2081
2082         inf = dev_get_drvdata(dev);
2083         if (kstrtouint(buf, 10, &resolution))
2084                 return -EINVAL;
2085
2086         dev_dbg(&inf->i2c->dev, "%s %u", __func__, resolution);
2087         inf->chip_config.accl_resolution = resolution;
2088         return count;
2089 }
2090
2091 static ssize_t nvi_accl_resolution_show(struct device *dev,
2092                                         struct device_attribute *attr,
2093                                         char *buf)
2094 {
2095         struct inv_gyro_state_s *inf;
2096         unsigned int resolution;
2097
2098         inf = dev_get_drvdata(dev);
2099         if (inf->chip_config.accl_enable)
2100                 resolution = inf->chip_config.accl_resolution;
2101         else
2102                 resolution = ACCL_INPUT_RESOLUTION;
2103         return sprintf(buf, "%u\n", resolution);
2104 }
2105
2106 static ssize_t nvi_accl_max_range_store(struct device *dev,
2107                                         struct device_attribute *attr,
2108                                         const char *buf, size_t count)
2109 {
2110         struct inv_gyro_state_s *inf;
2111         unsigned char fsr;
2112         int err;
2113
2114         inf = dev_get_drvdata(dev);
2115         err = kstrtou8(buf, 10, &fsr);
2116         if (err)
2117                 return -EINVAL;
2118
2119         if (fsr > 3)
2120                 return -EINVAL;
2121
2122         mutex_lock(&inf->mutex);
2123         dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
2124         if (fsr != inf->chip_config.accl_fsr) {
2125                 if (inf->chip_config.accl_enable) {
2126                         err = nvi_accel_config_wr(inf, fsr, 0);
2127                         if (err > 0)
2128                                 /* reset fifo to purge old data */
2129                                 nvi_reset(inf, true, false);
2130                         nvi_pm(inf, NVI_PM_AUTO);
2131                 }
2132                 if (err >= 0)
2133                         inf->chip_config.accl_fsr = fsr;
2134         }
2135         mutex_unlock(&inf->mutex);
2136         if (err < 0) {
2137                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", __func__, fsr, err);
2138                 return err;
2139         }
2140
2141         return count;
2142 }
2143
2144 ssize_t nvi_accl_max_range_show(struct device *dev,
2145                                 struct device_attribute *attr, char *buf)
2146 {
2147         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2148         unsigned int range;
2149
2150         if (inf->chip_config.accl_enable)
2151                 range = inf->chip_config.accl_fsr;
2152         else
2153                 range = 0x4000 >> inf->chip_config.accl_fsr;
2154         return sprintf(buf, "%u\n", range);
2155 }
2156
2157 static ssize_t nvi_lpa_delay_enable_store(struct device *dev,
2158                                           struct device_attribute *attr,
2159                                           const char *buf, size_t count)
2160 {
2161         struct inv_gyro_state_s *inf;
2162         unsigned long lpa_delay_us;
2163         int err;
2164
2165         inf = dev_get_drvdata(dev);
2166         err = kstrtoul(buf, 10, &lpa_delay_us);
2167         if (err)
2168                 return err;
2169
2170         mutex_lock(&inf->mutex);
2171         dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, lpa_delay_us);
2172         inf->chip_config.lpa_delay_us = lpa_delay_us;
2173         err = nvi_pm(inf, NVI_PM_AUTO);
2174         mutex_unlock(&inf->mutex);
2175         if (err)
2176                 dev_err(&inf->i2c->dev, "%s: %lu ERR=%d\n",
2177                         __func__, lpa_delay_us, err);
2178         return count;
2179 }
2180
2181 static ssize_t nvi_lpa_delay_enable_show(struct device *dev,
2182                                          struct device_attribute *attr,
2183                                          char *buf)
2184 {
2185         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2186
2187         return sprintf(buf, "%lu\n", inf->chip_config.lpa_delay_us);
2188 }
2189
2190 static ssize_t nvi_motion_thr_store(struct device *dev,
2191                                     struct device_attribute *attr,
2192                                     const char *buf, size_t count)
2193 {
2194         struct inv_gyro_state_s *inf;
2195         unsigned char mot_thr;
2196         int err;
2197
2198         inf = dev_get_drvdata(dev);
2199         err = kstrtou8(buf, 10, &mot_thr);
2200         if (err)
2201                 return -EINVAL;
2202
2203         mutex_lock(&inf->mutex);
2204         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_thr);
2205         nvi_pm(inf, NVI_PM_ON);
2206         err = nvi_motion_detect_enable(inf, mot_thr);
2207         err |= nvi_pm(inf, NVI_PM_AUTO);
2208         mutex_unlock(&inf->mutex);
2209         if (err) {
2210                 dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
2211                         __func__, mot_thr, err);
2212                 return err;
2213         }
2214
2215         return count;
2216 }
2217
2218 static ssize_t nvi_motion_thr_show(struct device *dev,
2219                                    struct device_attribute *attr, char *buf)
2220 {
2221         struct inv_gyro_state_s *inf;
2222         unsigned char mot_thr;
2223
2224         inf = dev_get_drvdata(dev);
2225         if (inf->mot_enable)
2226                 mot_thr = inf->hw.mot_thr;
2227         else
2228                 mot_thr = 0;
2229         return sprintf(buf, "%u\n", mot_thr);
2230 }
2231
2232 static ssize_t nvi_motion_dur_store(struct device *dev,
2233                                     struct device_attribute *attr,
2234                                     const char *buf, size_t count)
2235 {
2236         struct inv_gyro_state_s *inf;
2237         unsigned char mot_dur;
2238         int err;
2239
2240         inf = dev_get_drvdata(dev);
2241         err = kstrtou8(buf, 10, &mot_dur);
2242         if (err)
2243                 return -EINVAL;
2244
2245         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_dur);
2246         inf->chip_config.mot_dur = mot_dur;
2247         return count;
2248 }
2249
2250 static ssize_t nvi_motion_dur_show(struct device *dev,
2251                                    struct device_attribute *attr, char *buf)
2252 {
2253         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2254
2255         return sprintf(buf, "%u\n", inf->chip_config.mot_dur);
2256 }
2257
2258 static ssize_t nvi_motion_ctrl_store(struct device *dev,
2259                                      struct device_attribute *attr,
2260                                      const char *buf, size_t count)
2261 {
2262         struct inv_gyro_state_s *inf;
2263         unsigned char mot_ctrl;
2264         int err;
2265
2266         inf = dev_get_drvdata(dev);
2267         err = kstrtou8(buf, 10, &mot_ctrl);
2268         if (err)
2269                 return -EINVAL;
2270
2271         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_ctrl);
2272         inf->chip_config.mot_ctrl = mot_ctrl;
2273         return count;
2274 }
2275
2276 static ssize_t nvi_motion_ctrl_show(struct device *dev,
2277                                     struct device_attribute *attr, char *buf)
2278 {
2279         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2280
2281         return sprintf(buf, "%u\n", inf->chip_config.mot_ctrl);
2282 }
2283
2284 static ssize_t nvi_motion_count_store(struct device *dev,
2285                                       struct device_attribute *attr,
2286                                       const char *buf, size_t count)
2287 {
2288         struct inv_gyro_state_s *inf;
2289         unsigned int mot_cnt;
2290         int err;
2291
2292         inf = dev_get_drvdata(dev);
2293         err = kstrtouint(buf, 10, &mot_cnt);
2294         if (err)
2295                 return -EINVAL;
2296
2297         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_cnt);
2298         inf->chip_config.mot_cnt = mot_cnt;
2299         return count;
2300 }
2301
2302 static ssize_t nvi_motion_count_show(struct device *dev,
2303                                      struct device_attribute *attr, char *buf)
2304 {
2305         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2306
2307         return sprintf(buf, "%u\n", inf->chip_config.mot_cnt);
2308 }
2309
2310 static ssize_t nvi_enable_store(struct device *dev,
2311                                 struct device_attribute *attr,
2312                                 const char *buf, size_t count)
2313 {
2314         struct inv_gyro_state_s *inf;
2315         unsigned char enable;
2316         int err;
2317
2318         inf = dev_get_drvdata(dev);
2319         err = kstrtou8(buf, 10, &enable);
2320         if (err)
2321                 return -EINVAL;
2322
2323         mutex_lock(&inf->mutex);
2324         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, enable);
2325         if (enable)
2326                 enable = 1;
2327         if (enable != inf->chip_config.enable) {
2328                 inf->chip_config.enable = enable;
2329                 err = nvi_pm(inf, NVI_PM_AUTO);
2330         }
2331         mutex_unlock(&inf->mutex);
2332         if (err)
2333                 dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
2334                         __func__, enable, err);
2335         return count;
2336 }
2337
2338 ssize_t nvi_enable_show(struct device *dev,
2339                         struct device_attribute *attr, char *buf)
2340 {
2341         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2342
2343         return sprintf(buf, "%u\n", inf->chip_config.enable);
2344 }
2345
2346 /**
2347  *  inv_raw_gyro_show() - Read gyro data directly from registers.
2348  */
2349 static ssize_t inv_raw_gyro_show(struct device *dev,
2350                                  struct device_attribute *attr, char *buf)
2351 {
2352         struct inv_gyro_state_s *st;
2353         struct inv_reg_map_s *reg;
2354         int result;
2355         unsigned char data[6];
2356
2357         st = dev_get_drvdata(dev);
2358         reg = st->reg;
2359         if (0 == st->chip_config.gyro_enable)
2360                 return -EPERM;
2361
2362         result = inv_i2c_read(st, reg->raw_gyro, 6, data);
2363         if (result) {
2364                 printk(KERN_ERR "Could not read raw registers.\n");
2365                 return result;
2366         }
2367
2368         return sprintf(buf, "%d %d %d %lld\n",
2369                 (signed short)(be16_to_cpup((short *)&data[0])),
2370                 (signed short)(be16_to_cpup((short *)&data[2])),
2371                 (signed short)(be16_to_cpup((short *)&data[4])),
2372                 get_time_ns());
2373 }
2374
2375 /**
2376  *  inv_raw_accl_show() - Read accel data directly from registers.
2377  */
2378 static ssize_t inv_raw_accl_show(struct device *dev,
2379                                  struct device_attribute *attr, char *buf)
2380 {
2381         struct inv_gyro_state_s *st;
2382         struct inv_reg_map_s *reg;
2383         int result;
2384         unsigned char data[6];
2385
2386         st = dev_get_drvdata(dev);
2387         reg = st->reg;
2388         if (0 == st->chip_config.accl_enable)
2389                 return -EPERM;
2390
2391         result = inv_i2c_read(st, reg->raw_accl, 6, data);
2392         if (result) {
2393                 printk(KERN_ERR "Could not read raw registers.\n");
2394                 return result;
2395         }
2396
2397         return sprintf(buf, "%d %d %d %lld\n",
2398                 ((signed short)(be16_to_cpup((short *)&data[0]))*
2399                                 st->chip_info.multi),
2400                 ((signed short)(be16_to_cpup((short *)&data[2]))*
2401                                 st->chip_info.multi),
2402                 ((signed short)(be16_to_cpup((short *)&data[4]))*
2403                                 st->chip_info.multi),
2404                 get_time_ns());
2405 }
2406
2407 /**
2408  *  inv_temp_scale_show() - Get the temperature scale factor in LSBs/degree C.
2409  */
2410 static ssize_t inv_temp_scale_show(struct device *dev,
2411                                    struct device_attribute *attr, char *buf)
2412 {
2413         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2414
2415         if (INV_MPU3050 == st->chip_type)
2416                 return sprintf(buf, "280\n");
2417         else
2418                 return sprintf(buf, "340\n");
2419 }
2420
2421 /**
2422  *  inv_temp_offset_show() - Get the temperature offset in LSBs/degree C.
2423  */
2424 static ssize_t inv_temp_offset_show(struct device *dev,
2425                                     struct device_attribute *attr, char *buf)
2426 {
2427         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2428
2429         if (INV_MPU3050 == st->chip_type)
2430                 return sprintf(buf, "-13200\n");
2431         else
2432                 return sprintf(buf, "-521\n");
2433 }
2434
2435 static ssize_t inv_temperature_show(struct device *dev,
2436                                     struct device_attribute *attr, char *buf)
2437 {
2438         struct inv_gyro_state_s *inf;
2439         ssize_t rtn;
2440
2441         inf = dev_get_drvdata(dev);
2442         mutex_lock(&inf->mutex_temp);
2443         rtn = sprintf(buf, "%d %lld\n", inf->temp_val, inf->temp_ts);
2444         mutex_unlock(&inf->mutex_temp);
2445         return rtn;
2446 }
2447
2448 /**
2449  * inv_key_store() -  calling this function will store authenticate key
2450  */
2451 static ssize_t inv_key_store(struct device *dev,
2452                              struct device_attribute *attr,
2453                              const char *buf, size_t count)
2454 {
2455         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2456         unsigned int result, data, out;
2457         unsigned char *p, d[4];
2458
2459         if (st->chip_config.enable)
2460                 return -EPERM;
2461
2462         result = kstrtoul(buf, 10, (long unsigned int *)&data);
2463         if (result)
2464                 return result;
2465
2466         out = cpu_to_be32p(&data);
2467         p = (unsigned char *)&out;
2468         result = mem_w(D_AUTH_IN, 4, p);
2469         if (result)
2470                 return result;
2471
2472         result = mpu_memory_read(st->sl_handle, st->i2c_addr,
2473                 D_AUTH_IN, 4, d);
2474         return count;
2475 }
2476
2477 /**
2478  *  inv_reg_dump_show() - Register dump for testing.
2479  *  TODO: Only for testing.
2480  */
2481 static ssize_t inv_reg_dump_show(struct device *dev,
2482                                  struct device_attribute *attr, char *buf)
2483 {
2484         int ii;
2485         char data;
2486         ssize_t bytes_printed = 0;
2487         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2488
2489         for (ii = 0; ii < st->hw_s->num_reg; ii++) {
2490                 /* don't read fifo r/w register */
2491                 if (ii == st->reg->fifo_r_w)
2492                         data = 0;
2493                 else
2494                         inv_i2c_read(st, ii, 1, &data);
2495                 bytes_printed += sprintf(buf + bytes_printed, "%#2x: %#2x\n",
2496                                          ii, data);
2497         }
2498         return bytes_printed;
2499 }
2500
2501 /**
2502  * inv_gyro_orientation_show() - show orientation matrix
2503  */
2504 static ssize_t inv_gyro_orientation_show(struct device *dev,
2505                                          struct device_attribute *attr,
2506                                          char *buf)
2507 {
2508         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2509         signed char *m;
2510
2511         m = st->plat_data.orientation;
2512         return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
2513                        m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
2514 }
2515
2516 /**
2517  * inv_accl_matrix_show() - show orientation matrix
2518  */
2519 ssize_t inv_accl_matrix_show(struct device *dev,
2520                              struct device_attribute *attr, char *buf)
2521 {
2522         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2523         signed char *m;
2524
2525         if (st->plat_data.sec_slave_type == SECONDARY_SLAVE_TYPE_ACCEL)
2526                 m = st->plat_data.secondary_orientation;
2527         else
2528                 m = st->plat_data.orientation;
2529         return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
2530                        m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
2531 }
2532
2533 /**
2534  * inv_key_show() -  calling this function will show the key
2535  *
2536  */
2537 static ssize_t inv_key_show(struct device *dev, struct device_attribute *attr,
2538                             char *buf)
2539 {
2540         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2541         unsigned char *key;
2542         key = st->plat_data.key;
2543
2544         return sprintf(buf,
2545         "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
2546                 key[0], key[1], key[2], key[3], key[4], key[5], key[6],
2547                 key[7], key[8], key[9], key[10], key[11], key[12],
2548                 key[13], key[14], key[15]);
2549 }
2550
2551 #if DEBUG_SYSFS_INTERFACE
2552 static ssize_t nvi_dbg_i2c_addr_store(struct device *dev,
2553                                       struct device_attribute *attr,
2554                                       const char *buf, size_t count)
2555 {
2556         struct inv_gyro_state_s *inf;
2557         unsigned char dbg_i2c_addr;
2558         int err;
2559
2560         inf = dev_get_drvdata(dev);
2561         err = kstrtou8(buf, 16, &dbg_i2c_addr);
2562         if (err)
2563                 return -EINVAL;
2564
2565         inf->dbg_i2c_addr = dbg_i2c_addr;
2566         return count;
2567 }
2568
2569 static ssize_t nvi_dbg_i2c_addr_show(struct device *dev,
2570                                      struct device_attribute *attr, char *buf)
2571 {
2572         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2573         ssize_t bytes_printed = 0;
2574
2575         bytes_printed += sprintf(buf + bytes_printed,
2576                                  "%#2x\n", st->dbg_i2c_addr);
2577         return bytes_printed;
2578 }
2579
2580 static ssize_t nvi_dbg_reg_store(struct device *dev,
2581                                  struct device_attribute *attr,
2582                                  const char *buf, size_t count)
2583 {
2584         struct inv_gyro_state_s *inf;
2585         unsigned char dbg_reg;
2586         int err;
2587
2588         inf = dev_get_drvdata(dev);
2589         err = kstrtou8(buf, 16, &dbg_reg);
2590         if (err)
2591                 return -EINVAL;
2592
2593         inf->dbg_reg = dbg_reg;
2594         return count;
2595 }
2596
2597 static ssize_t nvi_dbg_reg_show(struct device *dev,
2598                                 struct device_attribute *attr, char *buf)
2599 {
2600         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2601         ssize_t bytes_printed = 0;
2602
2603         bytes_printed += sprintf(buf + bytes_printed, "%#2x\n", st->dbg_reg);
2604         return bytes_printed;
2605 }
2606
2607 static ssize_t nvi_dbg_dat_store(struct device *dev,
2608                                  struct device_attribute *attr,
2609                                  const char *buf, size_t count)
2610 {
2611         struct inv_gyro_state_s *inf;
2612         unsigned short dbg_i2c_addr;
2613         unsigned char dbg_dat;
2614         int err;
2615
2616         inf = dev_get_drvdata(dev);
2617         err = kstrtou8(buf, 16, &dbg_dat);
2618         if (err)
2619                 return -EINVAL;
2620
2621         if (inf->dbg_i2c_addr) {
2622                 err = inv_i2c_single_write_base(inf, inf->dbg_i2c_addr,
2623                                                 inf->dbg_reg, dbg_dat);
2624                 dbg_i2c_addr = inf->dbg_i2c_addr;
2625         } else {
2626                 err = inv_i2c_single_write(inf, inf->dbg_reg, dbg_dat);
2627                 dbg_i2c_addr = inf->i2c->addr;
2628         }
2629         pr_info("%s dev=%x reg=%x data=%x err=%d\n",
2630                 __func__, dbg_i2c_addr, inf->dbg_reg, dbg_dat, err);
2631         return count;
2632 }
2633
2634 static ssize_t nvi_dbg_dat_show(struct device *dev,
2635                                 struct device_attribute *attr, char *buf)
2636 {
2637         struct inv_gyro_state_s *inf;
2638         unsigned short dbg_i2c_addr;
2639         unsigned char data;
2640         ssize_t bytes_printed = 0;
2641
2642         inf = dev_get_drvdata(dev);
2643         if (inf->dbg_i2c_addr) {
2644                 inv_i2c_read_base(inf, inf->dbg_i2c_addr,
2645                                   inf->dbg_reg, 1, &data);
2646                 dbg_i2c_addr = inf->dbg_i2c_addr;
2647         } else {
2648                 inv_i2c_read(inf, inf->dbg_reg, 1, &data);
2649                 dbg_i2c_addr = inf->i2c->addr;
2650         }
2651         bytes_printed += sprintf(buf + bytes_printed, "%#2x:%#2x=%#2x\n",
2652                                  dbg_i2c_addr, inf->dbg_reg, data);
2653         return bytes_printed;
2654 }
2655
2656 static ssize_t nvi_aux_dbg_store(struct device *dev,
2657                                  struct device_attribute *attr,
2658                                  const char *buf, size_t count)
2659 {
2660         struct inv_gyro_state_s *inf;
2661         unsigned int enable;
2662         int err;
2663
2664         inf = dev_get_drvdata(dev);
2665         err = kstrtouint(buf, 10, &enable);
2666         if (err)
2667                 return err;
2668
2669         inf->aux.dbg = true;
2670         nvi_aux_dbg(inf, "SNAPSHOT", 0);
2671         if (enable)
2672                 inf->aux.dbg = true;
2673         else
2674                 inf->aux.dbg = false;
2675         return count;
2676 }
2677
2678 static ssize_t nvi_aux_dbg_show(struct device *dev,
2679                                 struct device_attribute *attr, char *buf)
2680 {
2681         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2682
2683         return sprintf(buf, "%x\n", inf->aux.dbg);
2684 }
2685
2686 static ssize_t nvi_mot_dbg_store(struct device *dev,
2687                                  struct device_attribute *attr,
2688                                  const char *buf, size_t count)
2689 {
2690         struct inv_gyro_state_s *inf;
2691         unsigned int mot_dbg;
2692         int err;
2693
2694         inf = dev_get_drvdata(dev);
2695         err = kstrtouint(buf, 10, &mot_dbg);
2696         if (err)
2697                 return err;
2698
2699         if (mot_dbg)
2700                 inf->mot_dbg = true;
2701         else
2702                 inf->mot_dbg = false;
2703         return count;
2704 }
2705
2706 static ssize_t nvi_mot_dbg_show(struct device *dev,
2707                                 struct device_attribute *attr, char *buf)
2708 {
2709         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2710
2711         return sprintf(buf, "%x\n", inf->mot_dbg);
2712 }
2713 #endif /* DEBUG_SYSFS_INTERFACE */
2714
2715 static ssize_t nvi_min_delay_us_store(struct device *dev,
2716                                       struct device_attribute *attr,
2717                                       const char *buf, size_t count)
2718 {
2719         struct inv_gyro_state_s *inf;
2720         unsigned long min_delay_us;
2721         int err;
2722
2723         inf = dev_get_drvdata(dev);
2724         err = kstrtoul(buf, 10, &min_delay_us);
2725         if (err)
2726                 return -EINVAL;
2727
2728         dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, min_delay_us);
2729         inf->chip_config.min_delay_us = min_delay_us;
2730         return count;
2731 }
2732
2733 static ssize_t nvi_min_delay_us_show(struct device *dev,
2734                                      struct device_attribute *attr, char *buf)
2735 {
2736         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2737
2738         return sprintf(buf, "%lu\n", inf->chip_config.min_delay_us);
2739 }
2740
2741
2742 static void inv_report_gyro_accl(struct inv_gyro_state_s *st, s64 t,
2743                                  unsigned char *data)
2744 {
2745         short x, y, z;
2746         int ind;
2747         struct inv_chip_config_s *conf;
2748
2749         conf = &st->chip_config;
2750         ind = 0;
2751         if (conf->accl_fifo_enable) {
2752                 x = ((data[ind] << 8)|data[ind + 1])*st->chip_info.multi;
2753                 y = ((data[ind + 2] << 8)|data[ind + 3])*st->chip_info.multi;
2754                 z = ((data[ind + 4] << 8)|data[ind + 5])*st->chip_info.multi;
2755                 if (conf->accl_fifo_enable) {
2756                         /*it is possible that accl disabled when dmp is on*/
2757                         input_report_rel(st->idev, REL_RX,  x);
2758                         input_report_rel(st->idev, REL_RY,  y);
2759                         input_report_rel(st->idev, REL_RZ,  z);
2760                 }
2761                 ind += 6;
2762         }
2763         if (conf->gyro_fifo_enable) {
2764                 x = (data[ind] << 8)     | data[ind + 1];
2765                 y = (data[ind + 2] << 8) | data[ind + 3];
2766                 z = (data[ind + 4] << 8) | data[ind + 5];
2767                 if (conf->gyro_fifo_enable) {
2768                         /*it is possible that gyro disabled when dmp is on*/
2769                         input_report_rel(st->idev, REL_X,  x);
2770                         input_report_rel(st->idev, REL_Y,  y);
2771                         input_report_rel(st->idev, REL_Z,  z);
2772                 }
2773                 ind += 6;
2774         }
2775         if (conf->accl_fifo_enable | conf->gyro_fifo_enable) {
2776                 input_report_rel(st->idev, REL_MISC, (unsigned int)(t >> 32));
2777                 input_report_rel(st->idev, REL_WHEEL,
2778                         (unsigned int)(t & 0xffffffff));
2779                 input_sync(st->idev);
2780         }
2781 }
2782
2783 /**
2784  *  inv_read_fifo() - Transfer data from FIFO to ring buffer.
2785  */
2786 static irqreturn_t inv_read_fifo(int irq, void *dev_id)
2787 {
2788         struct inv_gyro_state_s *st;
2789         unsigned char bytes_per_datum;
2790         const unsigned short fifo_thresh = 500;
2791         int result;
2792         unsigned char data[16];
2793         unsigned short fifo_count;
2794         unsigned int copied;
2795         s64 timestamp;
2796         struct inv_reg_map_s *reg;
2797
2798         st = (struct inv_gyro_state_s *)dev_id;
2799         reg = st->reg;
2800
2801         timestamp = get_time_ns();
2802         if ((!(st->hw.fifo_en & BIT_TEMP_FIFO_EN)) &&
2803                                                  st->chip_config.gyro_enable) {
2804                 result = inv_i2c_read(st, st->reg->temperature, 2, data);
2805                 if (!result) {
2806                         mutex_lock(&st->mutex_temp);
2807                         st->temp_val = (data[0] << 8) | data[1];
2808                         st->temp_ts = timestamp;
2809                         mutex_unlock(&st->mutex_temp);
2810                 }
2811         }
2812         if (st->mot_cnt)
2813                 st->mot_cnt--;
2814         if (st->lpa_enable || (st->hw.int_enable & BIT_MOT_EN)) {
2815                 if (st->hw.int_enable & BIT_MOT_EN) {
2816                         st->mot_cnt = st->chip_config.mot_cnt;
2817                         st->mot_enable = false;
2818                         nvi_int_enable_wr(st, true);
2819                         if (st->mot_dbg)
2820                                 pr_info("%s motion detect off", __func__);
2821                 }
2822                 result = inv_i2c_read(st, reg->raw_accl, 6, data);
2823                 if (result)
2824                         goto end_session;
2825
2826                 inv_report_gyro_accl(st, timestamp, data);
2827                 if (st->mot_enable && st->mot_dbg)
2828                         pr_info("%s SENDING MOTION DETECT DATA", __func__);
2829                 if (st->mot_enable && (!st->mot_cnt))
2830                         nvi_int_enable_wr(st, true);
2831                 goto end_session;
2832         }
2833
2834         bytes_per_datum = (st->chip_config.accl_fifo_enable +
2835                            st->chip_config.gyro_fifo_enable)*BYTES_PER_SENSOR;
2836         fifo_count = 0;
2837         if (bytes_per_datum != 0) {
2838                 result = inv_i2c_read(st, reg->fifo_count_h, 2, data);
2839                 if (result)
2840                         goto end_session;
2841
2842                 fifo_count = (data[0] << 8) + data[1];
2843                 if (fifo_count < bytes_per_datum)
2844                         goto end_session;
2845
2846                 if (fifo_count%2)
2847                         goto flush_fifo;
2848
2849                 if (fifo_count > fifo_thresh)
2850                         goto flush_fifo;
2851
2852                 /* Timestamp mismatch. */
2853                 if (kfifo_len(&st->trigger.timestamps) < (fifo_count /
2854                                                           bytes_per_datum))
2855                         goto flush_fifo;
2856
2857                 if (kfifo_len(&st->trigger.timestamps) > (fifo_count /
2858                                                           bytes_per_datum +
2859                                                           TIME_STAMP_TOR))
2860                         goto flush_fifo;
2861         }
2862
2863         if (bytes_per_datum == 0) {
2864                 result = kfifo_to_user(&st->trigger.timestamps,
2865                         &timestamp, sizeof(timestamp), &copied);
2866                 if (result)
2867                         goto flush_fifo;
2868         }
2869
2870         while ((bytes_per_datum != 0) && (fifo_count >= bytes_per_datum)) {
2871                 result = inv_i2c_read(st, reg->fifo_r_w, bytes_per_datum,
2872                         data);
2873                 if (result)
2874                         goto flush_fifo;
2875
2876                 result = kfifo_to_user(&st->trigger.timestamps,
2877                         &timestamp, sizeof(timestamp), &copied);
2878                 if (result)
2879                         goto flush_fifo;
2880
2881                 inv_report_gyro_accl(st, timestamp, data);
2882                 fifo_count -= bytes_per_datum;
2883         }
2884
2885         nvi_aux_read(st);
2886
2887 end_session:
2888         return IRQ_HANDLED;
2889
2890 flush_fifo:
2891         /* Flush HW and SW FIFOs. */
2892         nvi_reset(st, true, false);
2893         return IRQ_HANDLED;
2894 }
2895
2896 /**
2897  *  inv_irq_handler() - Cache a timestamp at each data ready interrupt.
2898  */
2899 static irqreturn_t inv_irq_handler(int irq, void *dev_id)
2900 {
2901         struct inv_gyro_state_s *st;
2902         long long timestamp;
2903         int result, catch_up;
2904         unsigned int time_since_last_irq;
2905
2906         st = (struct inv_gyro_state_s *)dev_id;
2907         timestamp = get_time_ns();
2908         time_since_last_irq = ((unsigned int)(timestamp - st->last_isr_time)) /
2909                               ONE_K_HZ;
2910         spin_lock(&st->time_stamp_lock);
2911         catch_up = 0;
2912         while ((time_since_last_irq > st->irq_dur_us*2) &&
2913                (catch_up < MAX_CATCH_UP) && (!st->lpa_enable)) {
2914                 st->last_isr_time += st->irq_dur_us * ONE_K_HZ;
2915                 result = kfifo_in(&st->trigger.timestamps,
2916                                   &st->last_isr_time, 1);
2917                 time_since_last_irq = ((unsigned int)(timestamp -
2918                                         st->last_isr_time)) / ONE_K_HZ;
2919                 catch_up++;
2920         }
2921         result = kfifo_in(&st->trigger.timestamps, &timestamp, 1);
2922         st->last_isr_time = timestamp;
2923         spin_unlock(&st->time_stamp_lock);
2924         return IRQ_WAKE_THREAD;
2925 }
2926
2927 static int inv_pm(struct inv_gyro_state_s *inf, int pm_req)
2928 {
2929         int err;
2930
2931         if (inf->nvi) {
2932                 err = nvi_pm(inf, pm_req);
2933         } else {
2934                 if ((pm_req > NVI_PM_OFF) || (pm_req == NVI_PM_AUTO))
2935                         err = set_power_mpu3050(inf, 1);
2936                 else
2937                         err = set_power_mpu3050(inf, 0);
2938         }
2939         return err;
2940 }
2941
2942 #ifdef CONFIG_PM
2943 static int inv_suspend(struct device *dev)
2944 {
2945         int result;
2946         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2947
2948         result = inv_pm(st, NVI_PM_OFF_FORCE);
2949         if (result)
2950                 dev_err(dev, "%s ERR\n", __func__);
2951         return result;
2952 }
2953
2954 static int inv_resume(struct device *dev)
2955 {
2956         int result;
2957         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2958
2959         result = inv_pm(st, NVI_PM_AUTO);
2960         if (result)
2961                 dev_err(dev, "%s ERR\n", __func__);
2962         return result;
2963 }
2964
2965 static const struct dev_pm_ops inv_pm_ops = {
2966         .suspend = inv_suspend,
2967         .resume = inv_resume,
2968 };
2969 #endif
2970
2971 static DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR | S_IWOTH,
2972                    nvi_gyro_enable_show, nvi_gyro_enable_store);
2973 static DEVICE_ATTR(gyro_fifo_enable, S_IRUGO | S_IWUSR | S_IWOTH,
2974                    nvi_gyro_fifo_enable_show, nvi_gyro_fifo_enable_store);
2975 static DEVICE_ATTR(gyro_delay, S_IRUGO | S_IWUSR | S_IWOTH,
2976                    nvi_gyro_delay_show, inv_gyro_delay_store);
2977 static DEVICE_ATTR(gyro_resolution, S_IRUGO | S_IWUSR | S_IWOTH,
2978                    nvi_gyro_resolution_show, nvi_gyro_resolution_store);
2979 static DEVICE_ATTR(gyro_max_range, S_IRUGO | S_IWUSR | S_IWOTH,
2980                    nvi_gyro_max_range_show, nvi_gyro_max_range_store);
2981 static DEVICE_ATTR(gyro_orientation, S_IRUGO,
2982                    inv_gyro_orientation_show, NULL);
2983 static DEVICE_ATTR(raw_gyro, S_IRUGO,
2984                    inv_raw_gyro_show, NULL);
2985 static DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR | S_IWOTH,
2986                    nvi_accl_enable_show, nvi_accl_enable_store);
2987 static DEVICE_ATTR(accl_fifo_enable, S_IRUGO | S_IWUSR,
2988                    nvi_accl_fifo_enable_show, nvi_accl_fifo_enable_store);
2989 static DEVICE_ATTR(accl_delay, S_IRUGO | S_IWUSR | S_IWOTH,
2990                    nvi_accl_delay_show, nvi_accl_delay_store);
2991 static DEVICE_ATTR(accl_resolution, S_IRUGO | S_IWUSR | S_IWOTH,
2992                    nvi_accl_resolution_show, nvi_accl_resolution_store);
2993 static DEVICE_ATTR(accl_max_range, S_IRUGO | S_IWUSR | S_IWOTH,
2994                    nvi_accl_max_range_show, nvi_accl_max_range_store);
2995 static DEVICE_ATTR(accl_orientation, S_IRUGO,
2996                    inv_accl_matrix_show, NULL);
2997 static DEVICE_ATTR(raw_accl, S_IRUGO,
2998                    inv_raw_accl_show, NULL);
2999 static DEVICE_ATTR(lpa_delay, S_IRUGO | S_IWUSR | S_IWOTH,
3000                    nvi_lpa_delay_enable_show, nvi_lpa_delay_enable_store);
3001 static DEVICE_ATTR(motion_threshold, S_IRUGO | S_IWUSR | S_IWOTH,
3002                    nvi_motion_thr_show, nvi_motion_thr_store);
3003 static DEVICE_ATTR(motion_duration, S_IRUGO | S_IWUSR | S_IWOTH,
3004                    nvi_motion_dur_show, nvi_motion_dur_store);
3005 static DEVICE_ATTR(motion_count, S_IRUGO | S_IWUSR | S_IWOTH,
3006                    nvi_motion_count_show, nvi_motion_count_store);
3007 static DEVICE_ATTR(motion_control, S_IRUGO | S_IWUSR | S_IWOTH,
3008                    nvi_motion_ctrl_show, nvi_motion_ctrl_store);
3009 static DEVICE_ATTR(temp_scale, S_IRUGO,
3010                    inv_temp_scale_show, NULL);
3011 static DEVICE_ATTR(temp_offset, S_IRUGO,
3012                    inv_temp_offset_show, NULL);
3013 static DEVICE_ATTR(temperature, S_IRUGO,
3014                    inv_temperature_show, NULL);
3015 static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWOTH,
3016                    nvi_enable_show, nvi_enable_store);
3017 static DEVICE_ATTR(dbg_reg, S_IRUGO | S_IWUSR | S_IWOTH,
3018                    nvi_dbg_reg_show, nvi_dbg_reg_store);
3019 static DEVICE_ATTR(dbg_dat, S_IRUGO | S_IWUSR | S_IWOTH,
3020                    nvi_dbg_dat_show, nvi_dbg_dat_store);
3021 static DEVICE_ATTR(dbg_i2c_addr, S_IRUGO | S_IWUSR | S_IWOTH,
3022                    nvi_dbg_i2c_addr_show, nvi_dbg_i2c_addr_store);
3023 static DEVICE_ATTR(aux_dbg, S_IRUGO | S_IWUSR | S_IWOTH,
3024                    nvi_aux_dbg_show, nvi_aux_dbg_store);
3025 static DEVICE_ATTR(mot_dbg, S_IRUGO | S_IWUSR | S_IWOTH,
3026                    nvi_mot_dbg_show, nvi_mot_dbg_store);
3027 static DEVICE_ATTR(key, S_IRUGO | S_IWUSR,
3028                    inv_key_show, inv_key_store);
3029 static DEVICE_ATTR(reg_dump, S_IRUGO,
3030                    inv_reg_dump_show, NULL);
3031 static DEVICE_ATTR(min_delay_us, S_IRUGO | S_IWUSR | S_IWOTH,
3032                    nvi_min_delay_us_show, nvi_min_delay_us_store);
3033
3034 static struct device_attribute *inv_attributes[] = {
3035         &dev_attr_gyro_delay,
3036         &dev_attr_gyro_orientation,
3037         &dev_attr_raw_gyro,
3038         &dev_attr_temp_scale,
3039         &dev_attr_temp_offset,
3040         &dev_attr_temperature,
3041         &dev_attr_reg_dump,
3042         &dev_attr_key,
3043 #if DEBUG_SYSFS_INTERFACE
3044         &dev_attr_dbg_reg,
3045         &dev_attr_dbg_dat,
3046         &dev_attr_dbg_i2c_addr,
3047 #endif /* DEBUG_SYSFS_INTERFACE */
3048         NULL
3049 };
3050
3051 static struct device_attribute *inv_mpu6050_attributes[] = {
3052         &dev_attr_gyro_enable,
3053         &dev_attr_gyro_fifo_enable,
3054         &dev_attr_gyro_max_range,
3055         &dev_attr_gyro_resolution,
3056         &dev_attr_accl_enable,
3057         &dev_attr_accl_fifo_enable,
3058         &dev_attr_accl_delay,
3059         &dev_attr_accl_max_range,
3060         &dev_attr_accl_resolution,
3061         &dev_attr_accl_orientation,
3062         &dev_attr_raw_accl,
3063         &dev_attr_lpa_delay,
3064         &dev_attr_motion_threshold,
3065         &dev_attr_motion_duration,
3066         &dev_attr_motion_count,
3067         &dev_attr_motion_control,
3068         &dev_attr_enable,
3069 #if DEBUG_SYSFS_INTERFACE
3070         &dev_attr_min_delay_us,
3071         &dev_attr_aux_dbg,
3072         &dev_attr_mot_dbg,
3073 #endif /* DEBUG_SYSFS_INTERFACE */
3074         NULL
3075 };
3076
3077 static int inv_check_chip_type(struct inv_gyro_state_s *st,
3078                                const struct i2c_device_id *id)
3079 {
3080         struct inv_reg_map_s *reg;
3081         int result = 0;
3082
3083         reg = st->reg;
3084         st->mpu_slave = NULL;
3085         if (!strcmp(id->name, "itg3500")) {
3086                 st->chip_type = INV_ITG3500;
3087                 st->nvi = false;
3088         } else if (!strcmp(id->name, "mpu3050")) {
3089                 st->chip_type = INV_MPU3050;
3090                 inv_setup_reg_mpu3050(reg);
3091                 st->nvi = false;
3092         } else if (!strcmp(id->name, "mpu6050")) {
3093                 st->chip_type = INV_MPU6050;
3094                 st->nvi = true;
3095         } else if (!strcmp(id->name, "mpu9150")) {
3096                 st->chip_type = INV_MPU9150;
3097                 st->nvi = true;
3098         }
3099         if (SECONDARY_SLAVE_TYPE_ACCEL == st->plat_data.sec_slave_type) {
3100                 if (st->plat_data.sec_slave_id == ACCEL_ID_KXTF9)
3101                         inv_register_kxtf9_slave(st);
3102         }
3103         if (st->nvi) {
3104                 nvi_pm_init(st);
3105         } else {
3106                 nvi_vreg_init(st);
3107                 /*reset register to power up default*/
3108                 result = inv_i2c_single_write(st, reg->pwr_mgmt_1, 0);
3109                 result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_RESET);
3110                 if (!result)
3111                         mdelay(POWER_UP_TIME);
3112         }
3113         return result;
3114 }
3115
3116 /**
3117  *  inv_init_config() - Initialize hardware, disable FIFO.
3118  *  @st:        Device driver instance.
3119  *  Initial configuration:
3120  *  FSR: +/- 2000DPS
3121  *  DLPF: 42Hz
3122  *  FIFO rate: 50Hz
3123  *  Clock source: Gyro PLL
3124  */
3125 static int inv_init_config(struct inv_gyro_state_s *st)
3126 {
3127         st->chip_config.min_delay_us = MIN_FIFO_RATE;
3128         st->chip_config.lpf = INV_FILTER_42HZ;
3129         st->chip_config.gyro_enable = 0;
3130         st->chip_config.gyro_fifo_enable = 0;
3131         st->chip_config.gyro_fsr = INV_FSR_2000DPS;
3132         st->chip_config.accl_enable = 0;
3133         st->chip_config.accl_fifo_enable = 0;
3134         st->chip_config.accl_fsr = INV_FS_02G;
3135         st->chip_config.mot_dur = 1;
3136         st->chip_config.mot_ctrl = 1;
3137         st->chip_config.mot_cnt = 10;
3138         st->irq_dur_us = 20 * ONE_K_HZ;
3139         st->chip_config.fifo_rate = 50;
3140         st->chip_config.enable = 0;
3141         st->chip_config.dmp_on = 0;
3142         st->chip_config.firmware_loaded = 0;
3143         st->chip_config.prog_start_addr = DMP_START_ADDR;
3144         return 0;
3145 }
3146
3147 static void inv_input_close(struct input_dev *d)
3148 {
3149         struct inv_gyro_state_s *st;
3150
3151         st = input_get_drvdata(d);
3152         inv_pm(st, NVI_PM_OFF_FORCE);
3153         nvi_vreg_exit(st);
3154 }
3155
3156 /**
3157  *  inv_setup_input() - internal setup input device.
3158  *  @st:        Device driver instance.
3159  *  @**idev_in  pointer to input device
3160  *  @*client    i2c client
3161  *  @*name      name of the input device.
3162  */
3163 static int inv_setup_input(struct inv_gyro_state_s *st,
3164                            struct input_dev **idev_in,
3165                            struct i2c_client *client, unsigned char *name)
3166 {
3167         int result;
3168         struct input_dev *idev;
3169
3170         idev = input_allocate_device();
3171         if (!idev) {
3172                 result = -ENOMEM;
3173                 return result;
3174         }
3175
3176         /* Setup input device. */
3177         idev->name = name;
3178         idev->id.bustype = BUS_I2C;
3179         idev->id.product = 'S';
3180         idev->id.vendor     = ('I'<<8) | 'S';
3181         idev->id.version    = 1;
3182         idev->dev.parent = &client->dev;
3183         /* Open and close method. */
3184         if (strcmp(name, "INV_DMP"))
3185                 idev->close = inv_input_close;
3186         input_set_capability(idev, EV_REL, REL_X);
3187         input_set_capability(idev, EV_REL, REL_Y);
3188         input_set_capability(idev, EV_REL, REL_Z);
3189         input_set_capability(idev, EV_REL, REL_RX);
3190         input_set_capability(idev, EV_REL, REL_RY);
3191         input_set_capability(idev, EV_REL, REL_RZ);
3192         input_set_capability(idev, EV_REL, REL_MISC);
3193         input_set_capability(idev, EV_REL, REL_WHEEL);
3194         input_set_drvdata(idev, st);
3195         result = input_register_device(idev);
3196         if (result)
3197                 input_free_device(idev);
3198         *idev_in = idev;
3199         return result;
3200 }
3201
3202 static int inv_create_input(struct inv_gyro_state_s *st,
3203                 struct i2c_client *client){
3204         struct input_dev *idev;
3205         int result;
3206
3207         idev = NULL;
3208         result = inv_setup_input(st, &idev, client, st->hw_s->name);
3209         if (result)
3210                 return result;
3211
3212         st->idev = idev;
3213         if (INV_ITG3500 == st->chip_type)
3214                 return 0;
3215
3216         result = inv_setup_input(st, &idev, client, "INV_DMP");
3217         if (result)
3218                 input_unregister_device(st->idev);
3219         else
3220                 st->idev_dmp = idev;
3221         return 0;
3222 }
3223
3224 int create_device_attributes(struct device *dev,
3225         struct device_attribute **attrs)
3226 {
3227         int i;
3228         int err = 0;
3229
3230         for (i = 0 ; NULL != attrs[i] ; ++i) {
3231                 err = sysfs_create_file(&dev->kobj, &attrs[i]->attr);
3232                 if (0 != err)
3233                         break;
3234         }
3235         if (0 != err) {
3236                 for (; i >= 0 ; --i)
3237                         sysfs_remove_file(&dev->kobj, &attrs[i]->attr);
3238         }
3239         return err;
3240 }
3241
3242 void remove_device_attributes(struct device *dev,
3243         struct device_attribute **attrs)
3244 {
3245         int i;
3246
3247         for (i = 0 ; NULL != attrs[i] ; ++i)
3248                 device_remove_file(dev, attrs[i]);
3249 }
3250
3251 static char const *const inv_class_name = "invensense";
3252 static char const *const inv_device_name = "mpu";
3253 static dev_t const inv_device_dev_t = MKDEV(MISC_MAJOR, MISC_DYNAMIC_MINOR);
3254 static struct bin_attribute dmp_firmware = {
3255         .attr = {
3256                 .name = "dmp_firmware",
3257                 .mode = S_IRUGO | S_IWUSR
3258         },
3259         .size = 4096,
3260         .read = inv_dmp_firmware_read,
3261         .write = inv_dmp_firmware_write,
3262 };
3263
3264 static int create_sysfs_interfaces(struct inv_gyro_state_s *st)
3265 {
3266         int result;
3267         result = 0;
3268
3269         st->inv_class = class_create(THIS_MODULE, inv_class_name);
3270         if (IS_ERR(st->inv_class)) {
3271                 result = PTR_ERR(st->inv_class);
3272                 goto exit_nullify_class;
3273         }
3274
3275         st->inv_dev = device_create(st->inv_class, &st->i2c->dev,
3276                         inv_device_dev_t, st, inv_device_name);
3277         if (IS_ERR(st->inv_dev)) {
3278                 result = PTR_ERR(st->inv_dev);
3279                 goto exit_destroy_class;
3280         }
3281
3282         result = create_device_attributes(st->inv_dev, inv_attributes);
3283         if (result < 0)
3284                 goto exit_destroy_device;
3285
3286         if (INV_ITG3500 == st->chip_type)
3287                 return 0;
3288
3289         result = sysfs_create_bin_file(&st->inv_dev->kobj, &dmp_firmware);
3290         if (result < 0)
3291                 goto exit_remove_device_attributes;
3292
3293         if (INV_MPU3050 == st->chip_type) {
3294                 result = inv_mpu3050_create_sysfs(st);
3295                 if (result)
3296                         goto exit_remove_bin_file;
3297
3298                 return 0;
3299         }
3300
3301         result = create_device_attributes(st->inv_dev, inv_mpu6050_attributes);
3302         if (result < 0)
3303                 goto exit_remove_bin_file;
3304
3305         return 0;
3306
3307 exit_remove_bin_file:
3308         sysfs_remove_bin_file(&st->inv_dev->kobj, &dmp_firmware);
3309 exit_remove_device_attributes:
3310         remove_device_attributes(st->inv_dev, inv_attributes);
3311 exit_destroy_device:
3312         device_destroy(st->inv_class, inv_device_dev_t);
3313 exit_destroy_class:
3314         st->inv_dev = NULL;
3315         class_destroy(st->inv_class);
3316 exit_nullify_class:
3317         st->inv_class = NULL;
3318         return result;
3319 }
3320
3321 static void remove_sysfs_interfaces(struct inv_gyro_state_s *st)
3322 {
3323         remove_device_attributes(st->inv_dev, inv_attributes);
3324         if (INV_ITG3500 != st->chip_type)
3325                 sysfs_remove_bin_file(&st->inv_dev->kobj, &dmp_firmware);
3326         if ((INV_ITG3500 != st->chip_type) && (INV_MPU3050 != st->chip_type))
3327                 remove_device_attributes(st->inv_dev, inv_mpu6050_attributes);
3328         if (INV_MPU3050 == st->chip_type)
3329                 inv_mpu3050_remove_sysfs(st);
3330         device_destroy(st->inv_class, inv_device_dev_t);
3331         class_destroy(st->inv_class);
3332         st->inv_dev = NULL;
3333         st->inv_class = NULL;
3334 }
3335
3336 static void nvi_shutdown(struct i2c_client *client)
3337 {
3338         struct inv_gyro_state_s *inf;
3339         int i;
3340
3341         inf = i2c_get_clientdata(client);
3342         if (inf != NULL) {
3343                 if (inf->nvi) {
3344                         for (i = 0; i < AUX_PORT_SPECIAL; i++) {
3345                                 if (inf->aux.port[i].nmp.shutdown_bypass) {
3346                                         nvi_aux_bypass_enable(inf, true);
3347                                         break;
3348                                 }
3349                         }
3350                 }
3351                 inf->shutdown = true;
3352                 if (inf->inv_dev)
3353                         remove_sysfs_interfaces(inf);
3354                 kfifo_free(&inf->trigger.timestamps);
3355                 free_irq(client->irq, inf);
3356                 if (inf->idev)
3357                         input_unregister_device(inf->idev);
3358                 if ((INV_ITG3500 != inf->chip_type) && (inf->idev_dmp))
3359                         input_unregister_device(inf->idev_dmp);
3360         }
3361 }
3362
3363 static int nvi_remove(struct i2c_client *client)
3364 {
3365         struct inv_gyro_state_s *inf;
3366
3367         nvi_shutdown(client);
3368         inf = i2c_get_clientdata(client);
3369         if (inf != NULL) {
3370                 if (inf->nvi) {
3371                         nvi_pm_exit(inf);
3372                 } else {
3373                         inv_pm(inf, NVI_PM_OFF_FORCE);
3374                         nvi_vreg_exit(inf);
3375                 }
3376                 kfree(inf);
3377         }
3378         dev_info(&client->dev, "Gyro module removed.\n");
3379         return 0;
3380 }
3381
3382 static int nvi_probe(struct i2c_client *client,
3383                      const struct i2c_device_id *id)
3384 {
3385         struct inv_gyro_state_s *st;
3386         int result;
3387
3388         pr_info("%s: Probe name %s\n", __func__, id->name);
3389         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
3390                 result = -ENODEV;
3391                 goto out_no_free;
3392         }
3393
3394         st = kzalloc(sizeof(*st), GFP_KERNEL);
3395         if (!st) {
3396                 result = -ENOMEM;
3397                 goto out_no_free;
3398         }
3399
3400         /* Make state variables available to all _show and _store functions. */
3401         i2c_set_clientdata(client, st);
3402         st->i2c = client;
3403         st->sl_handle = client->adapter;
3404         st->reg = (struct inv_reg_map_s *)&chip_reg ;
3405         st->hw_s = (struct inv_hw_s *)hw_info;
3406         st->i2c_addr = client->addr;
3407         st->plat_data =
3408                    *(struct mpu_platform_data *)dev_get_platdata(&client->dev);
3409
3410         /* power is turned on inside check chip type*/
3411         result = inv_check_chip_type(st, id);
3412         if (result)
3413                 goto out_free;
3414
3415         st->hw_s = (struct inv_hw_s *)(hw_info  + st->chip_type);
3416         if (INV_MPU3050 == st->chip_type)
3417                 result = inv_init_config_mpu3050(st);
3418         else
3419                 result = inv_init_config(st);
3420         if (result) {
3421                 dev_err(&client->adapter->dev,
3422                         "Could not initialize device.\n");
3423                 goto out_free;
3424         }
3425
3426         if (INV_ITG3500 != st->chip_type && INV_MPU3050 != st->chip_type) {
3427                 result = inv_get_silicon_rev_mpu6050(st);
3428                 if (result) {
3429                         dev_err(&client->adapter->dev,
3430                                 "%s get silicon error.\n", st->hw_s->name);
3431                         goto out_free;
3432                 }
3433         }
3434
3435         result = inv_pm(st, NVI_PM_OFF);
3436         if (result) {
3437                 dev_err(&client->adapter->dev,
3438                         "%s could not be turned off.\n", st->hw_s->name);
3439                 goto out_free;
3440         }
3441
3442         mutex_init(&st->mutex);
3443         mutex_init(&st->mutex_temp);
3444         INIT_KFIFO(st->trigger.timestamps);
3445         result = create_sysfs_interfaces(st);
3446         if (result)
3447                 goto out_free_kfifo;
3448
3449         if (!client->irq) {
3450                 dev_err(&client->adapter->dev, "IRQ not assigned.\n");
3451                 result = -EPERM;
3452                 goto out_close_sysfs;
3453         }
3454
3455         st->trigger.irq = client->irq;
3456         if (INV_MPU3050 == st->chip_type)
3457                 result = request_threaded_irq(client->irq, inv_irq_handler,
3458                                               inv_read_fifo_mpu3050,
3459                                               IRQF_TRIGGER_RISING |
3460                                               IRQF_SHARED, "inv_irq", st);
3461         else
3462                 result = request_threaded_irq(client->irq, inv_irq_handler,
3463                                               inv_read_fifo,
3464                                               IRQF_TRIGGER_RISING |
3465                                               IRQF_SHARED, "inv_irq", st);
3466         if (result)
3467                 goto out_close_sysfs;
3468
3469         spin_lock_init(&st->time_stamp_lock);
3470         result = inv_create_input(st, client);
3471         if (result) {
3472                 free_irq(client->irq, st);
3473                 goto out_close_sysfs;
3474         }
3475
3476         inf_local = st;
3477         dev_info(&client->adapter->dev, "%s is ready to go!\n", st->hw_s->name);
3478         return 0;
3479
3480 out_close_sysfs:
3481         remove_sysfs_interfaces(st);
3482 out_free_kfifo:
3483         kfifo_free(&st->trigger.timestamps);
3484 out_free:
3485         if (st->nvi) {
3486                 nvi_pm_exit(st);
3487         } else {
3488                 inv_pm(st, NVI_PM_OFF_FORCE);
3489                 nvi_vreg_exit(st);
3490         }
3491         kfree(st);
3492 out_no_free:
3493         dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
3494         return -EIO;
3495 }
3496
3497 static unsigned short normal_i2c[] = { I2C_CLIENT_END };
3498
3499 /* device id table is used to identify what device can be
3500  * supported by this driver
3501  */
3502 static struct i2c_device_id inv_mod_id[] = {
3503         {"itg3500", 0},
3504         {"mpu3050", 0},
3505         {"mpu6050", 0},
3506         {"mpu9150", 0},
3507         {}
3508 };
3509
3510 MODULE_DEVICE_TABLE(i2c, inv_mod_id);
3511
3512 static struct i2c_driver inv_mod_driver = {
3513         .class = I2C_CLASS_HWMON,
3514         .probe          =       nvi_probe,
3515         .remove         =       nvi_remove,
3516         .id_table       =       inv_mod_id,
3517         .driver = {
3518                 .owner  =       THIS_MODULE,
3519                 .name   =       "inv_dev",
3520 #ifdef CONFIG_PM
3521                 .pm     =       &inv_pm_ops,
3522 #endif
3523         },
3524         .address_list = normal_i2c,
3525         .shutdown       =       nvi_shutdown,
3526 };
3527
3528 static int __init inv_mod_init(void)
3529 {
3530         int result = i2c_add_driver(&inv_mod_driver);
3531
3532         if (result) {
3533                 pr_err("%s failed\n", __func__);
3534                 return result;
3535         }
3536
3537         return 0;
3538 }
3539
3540 static void __exit inv_mod_exit(void)
3541 {
3542         i2c_del_driver(&inv_mod_driver);
3543 }
3544
3545 module_init(inv_mod_init);
3546 module_exit(inv_mod_exit);
3547
3548 MODULE_AUTHOR("Invensense Corporation");
3549 MODULE_DESCRIPTION("Invensense device driver");
3550 MODULE_LICENSE("GPL");
3551 MODULE_ALIAS("inv_dev");
3552