input: misc: mpu: fix nvi_pwr_mgmt_1_war
[linux-2.6.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 #define NVI_VERSION                     (25)
49
50 /* regulator names in order of powering on */
51 static char *nvi_vregs[] = {
52         "vdd",
53         "vlogic",
54 };
55
56 static struct inv_reg_map_s chip_reg = {
57         .who_am_i               = 0x75,
58         .sample_rate_div        = 0x19,
59         .lpf                    = 0x1A,
60         .product_id             = 0x0C,
61         .bank_sel               = 0x6D,
62         .user_ctrl              = 0x6A,
63         .fifo_en                = 0x23,
64         .gyro_config            = 0x1B,
65         .accl_config            = 0x1C,
66         .fifo_count_h           = 0x72,
67         .fifo_r_w               = 0x74,
68         .raw_gyro               = 0x43,
69         .raw_accl               = 0x3B,
70         .temperature            = 0x41,
71         .int_enable             = 0x38,
72         .int_status             = 0x3A,
73         .pwr_mgmt_1             = 0x6B,
74         .pwr_mgmt_2             = 0x6C,
75         .mem_start_addr         = 0x6E,
76         .mem_r_w                = 0x6F,
77         .prgm_strt_addrh        = 0x70,
78
79         .accl_fifo_en           = BIT_ACCEL_OUT,
80         .fifo_reset             = BIT_FIFO_RST,
81         .i2c_mst_reset          = BIT_I2C_MST_RST,
82         .cycle                  = BIT_CYCLE,
83         .temp_dis               = BIT_TEMP_DIS
84 };
85
86 static const struct inv_hw_s hw_info[INV_NUM_PARTS] = {
87         {119, "ITG3500"},
88         { 63, "MPU3050"},
89         {117, "MPU6050"},
90         {118, "MPU9150"},
91         {119, "MPU6500"},
92         {118, "MPU9250"},
93 };
94
95 static unsigned long nvi_lpf_us_tbl[] = {
96         0, /* WAR: disabled 3906, 256Hz */
97         5319,   /* 188Hz */
98         10204,  /* 98Hz */
99         23810,  /* 42Hz */
100         50000,  /* 20Hz */
101         100000, /* 10Hz */
102         /* 200000, 5Hz */
103 };
104
105 static unsigned long nvi_lpa_delay_us_tbl_6050[] = {
106         800000, /* 800ms */
107         200000, /* 200ms */
108         50000,  /* 50ms */
109         /* 25000, 25ms */
110 };
111
112 static unsigned long nvi_lpa_delay_us_tbl_6500[] = {
113         4096000,/* 4096ms */
114         2048000,/* 2048ms */
115         1024000,/* 1024ms */
116         512000, /* 512ms */
117         256000, /* 256ms */
118         128000, /* 128ms */
119         64000,  /* 64ms */
120         32000,  /* 32ms */
121         16000,  /* 16ms */
122         8000,   /* 8ms */
123         4000,   /* 4ms */
124         /* 2000, 2ms */
125 };
126
127 static struct inv_gyro_state_s *inf_local;
128
129 s64 nvi_ts_ns(void)
130 {
131         struct timespec ts;
132         ktime_get_ts(&ts);
133         return timespec_to_ns(&ts);
134 }
135
136 /**
137  *  inv_i2c_read_base() - Read one or more bytes from the device registers.
138  *  @st:        Device driver instance.
139  *  @reg:       First device register to be read from.
140  *  @length:    Number of bytes to read.
141  *  @data:      Data read from device.
142  *  NOTE: The slave register will not increment when reading from the FIFO.
143  */
144 int inv_i2c_read_base(struct inv_gyro_state_s *st, unsigned short i2c_addr,
145                       unsigned char reg, unsigned short length,
146                       unsigned char *data)
147 {
148         struct i2c_msg msgs[2];
149         int res;
150
151         msgs[0].addr = i2c_addr;
152         msgs[0].flags = 0;      /* write */
153         msgs[0].buf = &reg;
154         msgs[0].len = 1;
155
156         msgs[1].addr = i2c_addr;
157         msgs[1].flags = I2C_M_RD;
158         msgs[1].buf = data;
159         msgs[1].len = length;
160
161         res = i2c_transfer(st->sl_handle, msgs, 2);
162         pr_debug("%s RD%02X%02X%02X res=%d\n",
163                  st->hw_s->name, i2c_addr, reg, length, res);
164         if (res < 2) {
165                 if (res >= 0)
166                         res = -EIO;
167                 return res;
168         }
169
170         return 0;
171 }
172
173 /**
174  *  inv_i2c_single_write_base() - Write a byte to a device register.
175  *  @st:        Device driver instance.
176  *  @reg:       Device register to be written to.
177  *  @data:      Byte to write to device.
178  */
179 int inv_i2c_single_write_base(struct inv_gyro_state_s *st,
180                               unsigned short i2c_addr, unsigned char reg,
181                               unsigned char data)
182 {
183         unsigned char tmp[2];
184         struct i2c_msg msg;
185         int res;
186
187         tmp[0] = reg;
188         tmp[1] = data;
189
190         msg.addr = i2c_addr;
191         msg.flags = 0;  /* write */
192         msg.buf = tmp;
193         msg.len = 2;
194
195         res = i2c_transfer(st->sl_handle, &msg, 1);
196         pr_debug("%s WR%02X%02X%02X res=%d\n",
197                  st->hw_s->name, i2c_addr, reg, data, res);
198         if (res < 1) {
199                 if (res == 0)
200                         res = -EIO;
201                 return res;
202         }
203
204         return 0;
205 }
206
207
208 /* Register SMPLRT_DIV (0x19) */
209 int nvi_smplrt_div_wr(struct inv_gyro_state_s *inf, u8 smplrt_div)
210 {
211         int err = 0;
212
213         if (smplrt_div != inf->hw.smplrt_div) {
214                 err = inv_i2c_single_write(inf, inf->reg->sample_rate_div,
215                                            smplrt_div);
216                 if (!err)
217                         inf->hw.smplrt_div = smplrt_div;
218         }
219         return err;
220 }
221
222 /* Register CONFIG (0x1A) */
223 int nvi_config_wr(struct inv_gyro_state_s *inf, u8 val)
224 {
225         int err = 0;
226
227         if (val != inf->hw.config) {
228                 err = inv_i2c_single_write(inf, inf->reg->lpf, val);
229                 if (!err) {
230                         inf->hw.config = val;
231                         err = 1; /* flag change made */
232                 }
233         }
234         return err;
235 }
236
237 /* Register GYRO_CONFIG (0x1B) */
238 int nvi_gyro_config_wr(struct inv_gyro_state_s *inf, u8 test, u8 fsr)
239 {
240         u8 val;
241         int err = 0;
242
243         if (inf->chip_type == INV_MPU3050) {
244                 val = inf->hw.config;
245                 val &= 0xE7;
246                 val |= fsr << 3;
247                 return nvi_config_wr(inf, val);
248         }
249
250         val = (test << 5) | (fsr << 3);
251         if (val != inf->hw.gyro_config) {
252                 err = inv_i2c_single_write(inf, inf->reg->gyro_config, val);
253                 if (!err) {
254                         inf->hw.gyro_config = val;
255                         err = 1; /* flag change made */
256                 }
257         }
258         return err;
259 }
260
261 /* Register ACCEL_CONFIG2 (0x1D) */
262 static int nvi_accel_config2_wr(struct inv_gyro_state_s *inf, u8 val)
263 {
264         int err = 0;
265
266         if (val != inf->hw.accl_config2) {
267                 err = inv_i2c_single_write(inf, 0x1D, val);
268                 if (!err) {
269                         inf->hw.accl_config2 = val;
270                         err = 1; /* flag change made */
271                 }
272         }
273         return err;
274 }
275
276 /* Register ACCEL_CONFIG (0x1C) */
277 int nvi_accel_config_wr(struct inv_gyro_state_s *inf, u8 test, u8 fsr, u8 hpf)
278 {
279         u8 val;
280         int err;
281         int err_t = 0;
282
283         if (inf->chip_type == INV_MPU3050) {
284                 if (inf->mpu_slave != NULL)
285                         err_t = inf->mpu_slave->set_fs(inf, fsr);
286                 return err_t;
287         }
288
289         val = (test << 5) | (fsr << 3);
290         if (inf->chip_type == INV_MPU6500)
291                 err_t = nvi_accel_config2_wr(inf, hpf);
292         else
293                 val |= hpf;
294         if (val != inf->hw.accl_config) {
295                 err = inv_i2c_single_write(inf, inf->reg->accl_config, val);
296                 if (!err) {
297                         inf->hw.accl_config = val;
298                         err_t |= 1; /* flag change made */
299                 } else {
300                         err_t |= err;
301                 }
302         }
303         return err_t;
304 }
305
306 /* Register LP_ACCEL_ODR (0x1E) */
307 static int nvi_lp_accel_odr_wr(struct inv_gyro_state_s *inf, u8 lposc_clksel)
308 {
309         int err = 0;
310
311         if (lposc_clksel != inf->hw.lposc_clksel) {
312                 err = inv_i2c_single_write(inf, REG_6500_LP_ACCEL_ODR,
313                                            lposc_clksel);
314                 if (!err)
315                         inf->hw.lposc_clksel = lposc_clksel;
316         }
317         return err;
318 }
319
320 /* Register MOT_THR (0x1F) */
321 static int nvi_mot_thr_wr(struct inv_gyro_state_s *inf, u8 mot_thr)
322 {
323         int err = 0;
324
325         if (mot_thr != inf->hw.mot_thr) {
326                 err = inv_i2c_single_write(inf, REG_MOT_THR, mot_thr);
327                 if (!err)
328                         inf->hw.mot_thr = mot_thr;
329         }
330         return err;
331 }
332
333 /* Register MOT_DUR (0x20) */
334 static int nvi_mot_dur_wr(struct inv_gyro_state_s *inf, u8 mot_dur)
335 {
336         int err = 0;
337
338         if (mot_dur != inf->hw.mot_dur) {
339                 err = inv_i2c_single_write(inf, REG_MOT_DUR, mot_dur);
340                 if (!err)
341                         inf->hw.mot_dur = mot_dur;
342         }
343         return err;
344 }
345
346 /* Register FIFO_EN (0x23) */
347 int nvi_fifo_en_wr(struct inv_gyro_state_s *inf, u8 fifo_en)
348 {
349         int err = 0;
350
351         if (fifo_en != inf->hw.fifo_en) {
352                 err = inv_i2c_single_write(inf, inf->reg->fifo_en, fifo_en);
353                 if (!err)
354                         inf->hw.fifo_en = fifo_en;
355         }
356         return err;
357 }
358
359 /* Register I2C_MST_CTRL (0x24) */
360 static int nvi_i2c_mst_ctrl_wr(struct inv_gyro_state_s *inf,
361                                bool port3_fifo_en)
362 {
363         u8 val;
364         int err = 0;
365
366         if (inf->chip_type == INV_MPU3050)
367                 return 0;
368
369         val = inf->aux.clock_i2c;
370         val |= BIT_WAIT_FOR_ES;
371         if (port3_fifo_en)
372                 val |= BIT_SLV3_FIFO_EN;
373         if (val != inf->hw.i2c_mst_ctrl) {
374                 err = inv_i2c_single_write(inf, REG_I2C_MST_CTRL, val);
375                 if (!err)
376                         inf->hw.i2c_mst_ctrl = val;
377         }
378         return err;
379 }
380
381 /* Register I2C_SLV0_CTRL (0x25) */
382 /* Register I2C_SLV1_CTRL (0x28) */
383 /* Register I2C_SLV2_CTRL (0x2B) */
384 /* Register I2C_SLV3_CTRL (0x2E) */
385 /* Register I2C_SLV4_CTRL (0x31) */
386 static int nvi_i2c_slv_addr_wr(struct inv_gyro_state_s *inf, int port, u8 addr)
387 {
388         u8 reg;
389         int err = 0;
390
391         reg = (REG_I2C_SLV0_ADDR + (port * 3));
392         if (addr != inf->hw.i2c_slv_addr[port]) {
393                 err = inv_i2c_single_write(inf, reg, addr);
394                 if (!err)
395                         inf->hw.i2c_slv_addr[port] = addr;
396         }
397         return err;
398 }
399
400 /* Register I2C_SLV0_CTRL (0x26) */
401 /* Register I2C_SLV1_CTRL (0x29) */
402 /* Register I2C_SLV2_CTRL (0x2C) */
403 /* Register I2C_SLV3_CTRL (0x2F) */
404 /* Register I2C_SLV4_CTRL (0x32) */
405 static int nvi_i2c_slv_reg_wr(struct inv_gyro_state_s *inf, int port, u8 val)
406 {
407         u8 reg;
408         int err = 0;
409
410         reg = (REG_I2C_SLV0_REG + (port * 3));
411         if (val != inf->hw.i2c_slv_reg[port]) {
412                 err = inv_i2c_single_write(inf, reg, val);
413                 if (!err)
414                         inf->hw.i2c_slv_reg[port] = val;
415         }
416         return err;
417 }
418
419 /* Register I2C_SLV0_CTRL (0x27) */
420 /* Register I2C_SLV1_CTRL (0x2A) */
421 /* Register I2C_SLV2_CTRL (0x2D) */
422 /* Register I2C_SLV3_CTRL (0x30) */
423 static int nvi_i2c_slv_ctrl_wr(struct inv_gyro_state_s *inf, int port, u8 val)
424 {
425         u8 reg;
426         int err = 0;
427
428         reg = (REG_I2C_SLV0_CTRL + (port * 3));
429         if (val != inf->hw.i2c_slv_ctrl[port]) {
430                 err = inv_i2c_single_write(inf, reg, val);
431                 if (!err) {
432                         inf->hw.i2c_slv_ctrl[port] = val;
433                         err = 1; /* flag change made */
434                 }
435         }
436         return err;
437 }
438
439 /* Register I2C_SLV4_CTRL (0x34) */
440 static int nvi_i2c_slv4_ctrl_wr(struct inv_gyro_state_s *inf, bool slv4_en)
441 {
442         u8 val;
443         int err = 0;
444
445         val = inf->aux.delay_hw;
446         val |= (inf->aux.port[AUX_PORT_SPECIAL].nmp.ctrl &
447                                                          BITS_I2C_SLV_REG_DIS);
448         if (slv4_en)
449                 val |= BIT_SLV_EN;
450         if (val != inf->hw.i2c_slv4_ctrl) {
451                 err = inv_i2c_single_write(inf, REG_I2C_SLV4_CTRL, val);
452                 if (!err) {
453                         inf->hw.i2c_slv4_ctrl = val;
454                         err = 1; /* flag change made */
455                 }
456         }
457         return err;
458 }
459
460 /* Register INT_PIN_CFG (0x37) */
461 static int nvi_int_pin_cfg_wr(struct inv_gyro_state_s *inf, u8 val)
462 {
463         int err = 0;
464
465         if (val != inf->hw.int_pin_cfg) {
466                 err = inv_i2c_single_write(inf, REG_INT_PIN_CFG, val);
467                 if (!err)
468                         inf->hw.int_pin_cfg = val;
469         }
470         return err;
471 }
472
473 /* Register INT_ENABLE (0x38) */
474 int nvi_int_enable_wr(struct inv_gyro_state_s *inf, bool enable)
475 {
476         u8 int_enable = 0;
477         int err = 0;
478
479         if (enable && (!(inf->hw.pwr_mgmt_1 & BIT_SLEEP))) {
480                 if ((inf->hw.user_ctrl & BIT_I2C_MST_EN) ||
481                                  ((~inf->hw.pwr_mgmt_2) & BIT_PWR_GYRO_STBY)) {
482                         int_enable = BIT_DATA_RDY_EN;
483                 } else if (inf->chip_type > INV_MPU3050) {
484                         if ((~inf->hw.pwr_mgmt_2) & BIT_PWR_ACCL_STBY) {
485                                 if (inf->mot_det_en) {
486                                         int_enable = BIT_MOT_EN;
487                                         if (inf->chip_config.mot_enable ==
488                                                                    NVI_MOT_DBG)
489                                                 pr_info("%s HW motion on\n",
490                                                         __func__);
491                                 } else {
492                                         int_enable = BIT_DATA_RDY_EN;
493                                 }
494                         } else if (!(inf->hw.pwr_mgmt_1 & BIT_TEMP_DIS)) {
495                                 int_enable = BIT_DATA_RDY_EN;
496                         }
497                 }
498                 if ((inf->hw.user_ctrl & BIT_FIFO_EN) &&
499                                                   (!inf->chip_config.fifo_thr))
500                         int_enable = BIT_FIFO_OVERFLOW;
501         }
502         if ((int_enable != inf->hw.int_enable) && (inf->pm > NVI_PM_OFF)) {
503                 err = inv_i2c_single_write(inf,
504                                            inf->reg->int_enable, int_enable);
505                 if (!err) {
506                         inf->hw.int_enable = int_enable;
507                         if (inf->dbg & NVI_DBG_SPEW_MSG)
508                                 dev_info(&inf->i2c->dev, "%s: %x\n",
509                                          __func__, int_enable);
510                 }
511         }
512         return err;
513 }
514
515 /* Register I2C_SLV0_CTRL (0x63) */
516 /* Register I2C_SLV1_CTRL (0x64) */
517 /* Register I2C_SLV2_CTRL (0x65) */
518 /* Register I2C_SLV3_CTRL (0x66) */
519 /* Register I2C_SLV4_CTRL (0x33) */
520 static int nvi_i2c_slv_do_wr(struct inv_gyro_state_s *inf,
521                              int port, u8 data_out)
522 {
523         u8 *hw;
524         u8 reg;
525         int err = 0;
526
527         if (port == AUX_PORT_SPECIAL) {
528                 hw = &inf->hw.i2c_slv4_do;
529                 reg = REG_I2C_SLV4_DO;
530         } else {
531                 hw = &inf->hw.i2c_slv_do[port];
532                 reg = REG_I2C_SLV0_DO + port;
533         }
534         if (data_out != *hw) {
535                 err = inv_i2c_single_write(inf, reg, data_out);
536                 if (!err)
537                         *hw = data_out;
538         }
539         return err;
540 }
541
542 /* Register I2C_MST_DELAY_CTRL (0x67) */
543 static int nvi_i2c_mst_delay_ctrl_wr(struct inv_gyro_state_s *inf,
544                                      u8 i2c_mst_delay_ctrl)
545 {
546         int err = 0;
547
548         if (i2c_mst_delay_ctrl != inf->hw.i2c_mst_delay_ctrl) {
549                 err = inv_i2c_single_write(inf, REG_I2C_MST_DELAY_CTRL,
550                                            i2c_mst_delay_ctrl);
551                 if (!err)
552                         inf->hw.i2c_mst_delay_ctrl = i2c_mst_delay_ctrl;
553         }
554         return err;
555 }
556
557 /* Register MOT_DETECT_CTRL (0x69) */
558 static int nvi_mot_detect_ctrl_wr(struct inv_gyro_state_s *inf, u8 val)
559 {
560         int err = 0;
561
562         if (val != inf->hw.mot_detect_ctrl) {
563                 err = inv_i2c_single_write(inf, REG_MOT_DETECT_CTRL, val);
564                 if (!err)
565                         inf->hw.mot_detect_ctrl = val;
566         }
567         return err;
568 }
569
570 /* Register USER_CTRL (0x6A) */
571 int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf, u8 val)
572 {
573         int i;
574         int err;
575         int err_t;
576
577         err_t =  inv_i2c_single_write(inf, inf->reg->user_ctrl, val);
578         for (i = 0; i < POWER_UP_TIME; i++) {
579                 val = -1;
580                 err = inv_i2c_read(inf, inf->reg->user_ctrl, 1, &val);
581                 if (!(val & (inf->reg->fifo_reset | inf->reg->i2c_mst_reset)))
582                         break;
583
584                 mdelay(1);
585         }
586         err_t |= err;
587         inf->hw.user_ctrl = val;
588         return err_t;
589 }
590
591 /* Register USER_CTRL (0x6A) */
592 int nvi_user_ctrl_en_wr(struct inv_gyro_state_s *inf, u8 val)
593 {
594         int err = 0;
595
596         if (val != inf->hw.user_ctrl) {
597                 err = inv_i2c_single_write(inf, inf->reg->user_ctrl, val);
598                 if (!err) {
599                         inf->hw.user_ctrl = val;
600                         dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, val);
601                 } else {
602                         dev_err(&inf->i2c->dev, "%s: %x=>%x ERR\n",
603                                 __func__, inf->hw.user_ctrl, val);
604                 }
605         }
606         return err;
607 }
608
609 int nvi_user_ctrl_en(struct inv_gyro_state_s *inf,
610                      bool fifo_enable, bool i2c_enable)
611 {
612         u8 val;
613         u16 fifo_sample_size;
614         bool en;
615         int i;
616         int err;
617
618         dev_dbg(&inf->i2c->dev, "%s: FIFO=%x I2C=%x\n",
619                 __func__, fifo_enable, i2c_enable);
620         val = 0;
621         fifo_sample_size = 0;
622         inf->fifo_sample_size = 0;
623         en = false;
624         if (fifo_enable) {
625                 if (inf->chip_type == INV_MPU3050) {
626                         val |= BIT_3050_FIFO_FOOTER;
627                         fifo_sample_size += 2;
628                 }
629                 if (inf->chip_config.accl_fifo_enable) {
630                         val |= inf->reg->accl_fifo_en;
631                         fifo_sample_size += 6;
632                 }
633                 if (inf->chip_config.temp_enable &&
634                                            inf->chip_config.temp_fifo_enable) {
635                         val |= BIT_TEMP_FIFO_EN;
636                         fifo_sample_size += 2;
637                 }
638                 if (inf->chip_config.gyro_fifo_enable) {
639                         val |= (inf->chip_config.gyro_fifo_enable << 4);
640                         if (val & BIT_GYRO_XOUT)
641                                 fifo_sample_size += 2;
642                         if (val & BIT_GYRO_YOUT)
643                                 fifo_sample_size += 2;
644                         if (val & BIT_GYRO_ZOUT)
645                                 fifo_sample_size += 2;
646                 }
647                 for (i = 0; i < AUX_PORT_SPECIAL; i++) {
648                         if (inf->aux.port[i].fifo_en &&
649                                   (inf->aux.port[i].nmp.addr & BIT_I2C_READ) &&
650                                       (inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN)) {
651                                 if (i == 3)
652                                         en = true;
653                                 else
654                                         val |= (1 << i);
655                                 fifo_sample_size += inf->aux.port[i].nmp.ctrl &
656                                                     BITS_I2C_SLV_CTRL_LEN;
657                         }
658                 }
659                 err = nvi_i2c_mst_ctrl_wr(inf, en);
660                 if (val || en)
661                         en = true;
662                 else
663                         en = false;
664                 inf->fifo_sample_size = fifo_sample_size;
665         } else {
666                 err = nvi_i2c_mst_ctrl_wr(inf, false);
667         }
668         err |= nvi_fifo_en_wr(inf, val);
669         val = 0;
670         if (fifo_enable && en)
671                 val |= BIT_FIFO_EN;
672         if (i2c_enable && (inf->aux.enable || inf->aux.en3050))
673                 val |= BIT_I2C_MST_EN;
674         err |= nvi_user_ctrl_en_wr(inf, val);
675         return err;
676 }
677
678 /* Register PWR_MGMT_1 (0x6B) */
679 static int nvi_pwr_mgmt_1_war(struct inv_gyro_state_s *inf)
680 {
681         u8 val;
682         int i;
683         int err;
684
685         for (i = 0; i < POWER_UP_TIME; i++) {
686                 err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, 0);
687                 if (err > 0) {
688                         val = -1;
689                         err = inv_i2c_read(inf, inf->reg->pwr_mgmt_1, 1, &val);
690                         if (err > 0 && !val)
691                                 break;
692                 }
693                 mdelay(1);
694         }
695         inf->hw.pwr_mgmt_1 = val;
696         return err;
697 }
698
699 /* Register PWR_MGMT_1 (0x6B) */
700 static int nvi_pwr_mgmt_1_wr(struct inv_gyro_state_s *inf, u8 pwr_mgmt_1)
701 {
702         int err = 0;
703
704         if (pwr_mgmt_1 != inf->hw.pwr_mgmt_1) {
705                 err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1,
706                                            pwr_mgmt_1);
707                 if (!err)
708                         inf->hw.pwr_mgmt_1 = pwr_mgmt_1;
709         }
710         return err;
711 }
712
713 /* Register PWR_MGMT_2 (0x6C) */
714 static int nvi_pwr_mgmt_2_wr(struct inv_gyro_state_s *inf, u8 pwr_mgmt_2)
715 {
716         int err = 0;
717
718         if (pwr_mgmt_2 != inf->hw.pwr_mgmt_2) {
719                 err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_2,
720                                            pwr_mgmt_2);
721                 if (!err)
722                         inf->hw.pwr_mgmt_2 = pwr_mgmt_2;
723         }
724         return err;
725 }
726
727 static int nvi_motion_detect_enable(struct inv_gyro_state_s *inf, u8 mot_thr)
728 {
729         int err;
730         int err_t = 0;
731
732         if (inf->chip_type != INV_MPU6050)
733                 return 0;
734
735         if (mot_thr) {
736                 err = nvi_accel_config_wr(inf, 0,
737                                           inf->chip_config.accl_fsr, 0);
738                 if (err < 0)
739                         err_t |= err;
740                 err = nvi_config_wr(inf, 0);
741                 if (err < 0)
742                         err_t |= err;
743                 err_t |= nvi_mot_dur_wr(inf, inf->chip_config.mot_dur);
744                 err_t |= nvi_mot_detect_ctrl_wr(inf,
745                                                 inf->chip_config.mot_ctrl);
746                 err_t |= nvi_mot_thr_wr(inf, mot_thr);
747                 mdelay(5);
748                 err = nvi_accel_config_wr(inf, 0,
749                                           inf->chip_config.accl_fsr, 7);
750                 if (err < 0)
751                         err_t |= err;
752                 if (err_t)
753                         nvi_accel_config_wr(inf, 0,
754                                             inf->chip_config.accl_fsr, 0);
755         } else {
756                 err = nvi_accel_config_wr(inf, 0,
757                                           inf->chip_config.accl_fsr, 0);
758                 if (err < 0)
759                         err_t |= err;
760         }
761         return err_t;
762 }
763
764 static int nvi_vreg_dis(struct inv_gyro_state_s *inf, unsigned int i)
765 {
766         int err = 0;
767
768         if (inf->vreg[i].ret && (inf->vreg[i].consumer != NULL)) {
769                 err = regulator_disable(inf->vreg[i].consumer);
770                 if (!err) {
771                         inf->vreg[i].ret = 0;
772                         dev_dbg(&inf->i2c->dev, "%s %s\n",
773                                 __func__, inf->vreg[i].supply);
774                 } else {
775                         dev_err(&inf->i2c->dev, "%s %s ERR\n",
776                                 __func__, inf->vreg[i].supply);
777                 }
778         }
779         return err;
780 }
781
782 static int nvi_vreg_dis_all(struct inv_gyro_state_s *inf)
783 {
784         unsigned int i;
785         int err = 0;
786
787         for (i = ARRAY_SIZE(nvi_vregs); i > 0; i--)
788                 err |= nvi_vreg_dis(inf, (i - 1));
789         return err;
790 }
791
792 static int nvi_vreg_en(struct inv_gyro_state_s *inf, unsigned int i)
793 {
794         int err = 0;
795
796         if ((!inf->vreg[i].ret) && (inf->vreg[i].consumer != NULL)) {
797                 err = regulator_enable(inf->vreg[i].consumer);
798                 if (!err) {
799                         inf->vreg[i].ret = 1;
800                         dev_dbg(&inf->i2c->dev, "%s %s\n",
801                                 __func__, inf->vreg[i].supply);
802                         err = 1; /* flag regulator state change */
803                 } else {
804                         dev_err(&inf->i2c->dev, "%s %s ERR\n",
805                                 __func__, inf->vreg[i].supply);
806                 }
807         }
808         return err;
809 }
810
811 static int nvi_vreg_en_all(struct inv_gyro_state_s *inf)
812 {
813         unsigned i;
814         int err = 0;
815
816         for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++)
817                 err |= nvi_vreg_en(inf, i);
818         return err;
819 }
820
821 static void nvi_vreg_exit(struct inv_gyro_state_s *inf)
822 {
823         int i;
824
825         for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++) {
826                 if (inf->vreg[i].consumer != NULL) {
827                         devm_regulator_put(inf->vreg[i].consumer);
828                         inf->vreg[i].consumer = NULL;
829                         dev_dbg(&inf->i2c->dev, "%s %s\n",
830                                 __func__, inf->vreg[i].supply);
831                 }
832         }
833 }
834
835 static int nvi_vreg_init(struct inv_gyro_state_s *inf)
836 {
837         unsigned int i;
838         int err = 0;
839
840         for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++) {
841                 inf->vreg[i].supply = nvi_vregs[i];
842                 inf->vreg[i].ret = 0;
843                 inf->vreg[i].consumer = devm_regulator_get(&inf->i2c->dev,
844                                                           inf->vreg[i].supply);
845                 if (IS_ERR(inf->vreg[i].consumer)) {
846                         err = PTR_ERR(inf->vreg[i].consumer);
847                         dev_err(&inf->i2c->dev, "%s ERR %d for %s\n",
848                                 __func__, err, inf->vreg[i].supply);
849                         inf->vreg[i].consumer = NULL;
850                 } else {
851                         dev_dbg(&inf->i2c->dev, "%s %s\n",
852                                 __func__, inf->vreg[i].supply);
853                 }
854         }
855         return err;
856 }
857
858 int nvi_pm_wr(struct inv_gyro_state_s *inf,
859               u8 pwr_mgmt_1, u8 pwr_mgmt_2, u8 lpa)
860 {
861         u8 val;
862         int i;
863         int err;
864         int err_t = 0;
865
866         err = nvi_vreg_en_all(inf);
867         if (err) {
868                 err_t |= nvi_pwr_mgmt_1_war(inf);
869                 err_t |= inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1,
870                                               BIT_RESET);
871                 for (i = 0; i < POWER_UP_TIME; i++) {
872                         val = -1;
873                         err = inv_i2c_read(inf, inf->reg->pwr_mgmt_1, 1, &val);
874                         if (!(val & BIT_RESET))
875                                 break;
876
877                         mdelay(1);
878                 }
879                 err_t |= err;
880                 err_t |= nvi_pwr_mgmt_1_war(inf);
881                 err_t |= nvi_user_ctrl_reset_wr(inf, (inf->reg->fifo_reset |
882                                                      inf->reg->i2c_mst_reset));
883                 memset(&inf->hw, 0, sizeof(struct nvi_hw));
884                 inf->sample_delay_us = 0;
885         } else {
886                 err_t |= nvi_pwr_mgmt_1_war(inf);
887         }
888         switch (inf->chip_type) {
889         case INV_MPU3050:
890                 pwr_mgmt_1 &= (BIT_SLEEP | INV_CLK_PLL);
891                 if (pwr_mgmt_1 & INV_CLK_PLL) {
892                         err = nvi_pwr_mgmt_1_wr(inf, (BITS_3050_POWER1 |
893                                                       INV_CLK_PLL));
894                         err |= nvi_pwr_mgmt_1_wr(inf, (BITS_3050_POWER2 |
895                                                        INV_CLK_PLL));
896                         err |= nvi_pwr_mgmt_1_wr(inf, INV_CLK_PLL);
897                 } else {
898                         pwr_mgmt_1 |= (pwr_mgmt_2 & 0x07) << 3;
899                         err = nvi_pwr_mgmt_1_wr(inf, pwr_mgmt_1);
900                 }
901                 if (err)
902                         err_t |= err;
903                 else
904                         inf->hw.pwr_mgmt_2 = pwr_mgmt_2;
905                 break;
906
907         case INV_MPU6050:
908                 pwr_mgmt_2 |= lpa << 6;
909                 err = nvi_pwr_mgmt_2_wr(inf, pwr_mgmt_2);
910                 if (err)
911                         err_t |= err;
912                 else
913                         inf->hw.lposc_clksel = lpa;
914                 err_t |= nvi_pwr_mgmt_1_wr(inf, pwr_mgmt_1);
915                 break;
916
917         default: /* INV_MPU6500 */
918                 err_t |= nvi_lp_accel_odr_wr(inf, lpa);
919                 err_t |= nvi_pwr_mgmt_2_wr(inf, pwr_mgmt_2);
920                 err_t |= nvi_pwr_mgmt_1_wr(inf, pwr_mgmt_1);
921                 break;
922         }
923
924         return err_t;
925 }
926
927 /**
928  * @param inf
929  * @param pm_req: call with one of the following:
930  *      NVI_PM_OFF_FORCE = force off state
931  *      NVI_PM_ON = minimum power for device access
932  *      NVI_PM_ON_FULL = power for gyro
933  *      NVI_PM_AUTO = automatically sets power for configuration
934  *      Typical use is to set needed power for configuration and
935  *      then call with NVI_PM_AUTO when done.
936  *      All other NVI_PM_ levels are handled automatically and
937  *      are for internal use.
938  * @return int: returns 0 for success or error code
939  */
940 static int nvi_pm(struct inv_gyro_state_s *inf, int pm_req)
941 {
942         bool irq;
943         u8 pwr_mgmt_1;
944         u8 pwr_mgmt_2;
945         u8 lpa;
946         int pm;
947         int err = 0;
948
949         lpa = inf->hw.lposc_clksel;
950         if ((pm_req == NVI_PM_OFF_FORCE) || inf->suspend) {
951                 pwr_mgmt_1 = BIT_SLEEP | inf->reg->temp_dis;
952                 pwr_mgmt_2 = (BIT_PWR_ACCL_STBY | BIT_PWR_GYRO_STBY);
953                 pm = NVI_PM_OFF;
954         } else {
955                 pwr_mgmt_2 = (((~inf->chip_config.accl_enable) << 3) &
956                               BIT_PWR_ACCL_STBY);
957                 pwr_mgmt_2 |= ((~inf->chip_config.gyro_enable) &
958                                BIT_PWR_GYRO_STBY);
959                 if (inf->chip_config.gyro_enable ||
960                                 (inf->chip_config.temp_enable & NVI_TEMP_EN) ||
961                                         (inf->hw.user_ctrl & BIT_I2C_MST_EN)) {
962                         if (inf->chip_config.gyro_enable)
963                                 pm = NVI_PM_ON_FULL;
964                         else
965                                 pm = NVI_PM_ON;
966                 } else if (inf->chip_config.accl_enable) {
967                         if (inf->mot_det_en ||
968                                               (inf->chip_config.lpa_delay_us &&
969                                                inf->hal.lpa_tbl_n &&
970                                              (inf->chip_config.accl_delay_us >=
971                                              inf->chip_config.lpa_delay_us))) {
972                                 for (lpa = 0; lpa < inf->hal.lpa_tbl_n;
973                                                                        lpa++) {
974                                         if (inf->chip_config.accl_delay_us >=
975                                                          inf->hal.lpa_tbl[lpa])
976                                                 break;
977                                 }
978                                 pm = NVI_PM_ON_CYCLE;
979                         } else {
980                                 pm = NVI_PM_ON;
981                         }
982                 } else if (inf->chip_config.enable || inf->aux.bypass_lock) {
983                         pm = NVI_PM_STDBY;
984                 } else {
985                         pm = NVI_PM_OFF;
986                 }
987                 if (pm_req > pm)
988                         pm = pm_req;
989                 switch (pm) {
990                 case NVI_PM_OFF:
991                 case NVI_PM_STDBY:
992                         pwr_mgmt_1 = BIT_SLEEP;
993                         break;
994
995                 case NVI_PM_ON_CYCLE:
996                         pwr_mgmt_1 = inf->reg->cycle;
997                         break;
998
999                 case NVI_PM_ON:
1000                         pwr_mgmt_1 = INV_CLK_INTERNAL;
1001                         break;
1002
1003                 case NVI_PM_ON_FULL:
1004                         pwr_mgmt_1 = INV_CLK_PLL;
1005                         /* gyro must be turned on before going to PLL clock */
1006                         pwr_mgmt_2 &= ~BIT_PWR_GYRO_STBY;
1007                         break;
1008
1009                 default:
1010                         dev_err(&inf->i2c->dev, "%s %d=>%d ERR=EINVAL\n",
1011                                 __func__, inf->pm, pm);
1012                         return -EINVAL;
1013                 }
1014
1015                 if (!inf->chip_config.temp_enable)
1016                         pwr_mgmt_1 |= inf->reg->temp_dis;
1017         }
1018
1019         if ((pm != inf->pm) || (lpa != inf->hw.lposc_clksel) ||
1020                    (pwr_mgmt_1 != inf->hw.pwr_mgmt_1) ||
1021                    (pwr_mgmt_2 != (inf->hw.pwr_mgmt_2 &
1022                                    (BIT_PWR_ACCL_STBY | BIT_PWR_GYRO_STBY)))) {
1023                 nvi_int_enable_wr(inf, false);
1024                 inf->gyro_start_ts = 0;
1025                 if ((!(inf->hw.pwr_mgmt_1 & (BIT_SLEEP | inf->reg->cycle))) &&
1026                              (pm < NVI_PM_ON) && (inf->pm > NVI_PM_ON_CYCLE)) {
1027                         /* tasks that need access before low power state */
1028                         if (pm_req == NVI_PM_AUTO)
1029                                 /* turn off FIFO and I2C */
1030                                 nvi_user_ctrl_en(inf, false, false);
1031                 }
1032                 err |= nvi_pm_wr(inf, pwr_mgmt_1, pwr_mgmt_2, lpa);
1033                 if (pm == NVI_PM_OFF)
1034                         err |= nvi_vreg_dis_all(inf);
1035                 if (err < 0) {
1036                         dev_err(&inf->i2c->dev, "%s %d=>%d ERR=%d\n",
1037                                 __func__, inf->pm, pm, err);
1038                         pm = NVI_PM_ERR;
1039                 }
1040                 if (inf->dbg & NVI_DBG_SPEW_MSG)
1041                         dev_info(&inf->i2c->dev, "%s %d=>%d PM2=%x LPA=%x\n",
1042                                  __func__, inf->pm, pm, pwr_mgmt_2, lpa);
1043                 inf->pm = pm;
1044                 if (err > 0)
1045                         err = 0;
1046         }
1047         if (pm_req == NVI_PM_AUTO) {
1048                 if (pm > NVI_PM_STDBY)
1049                         irq = true;
1050                 if (pm > NVI_PM_ON_CYCLE)
1051                         nvi_user_ctrl_en(inf, true, true);
1052                 if ((pm == NVI_PM_ON_FULL) && (!inf->gyro_start_ts))
1053                         inf->gyro_start_ts = nvi_ts_ns() +
1054                                           inf->chip_config.gyro_start_delay_ns;
1055         } else {
1056                 irq = false;
1057         }
1058         nvi_int_enable_wr(inf, irq);
1059         return err;
1060 }
1061
1062 static void nvi_pm_exit(struct inv_gyro_state_s *inf)
1063 {
1064         nvi_pm(inf, NVI_PM_OFF_FORCE);
1065         nvi_vreg_exit(inf);
1066 }
1067
1068 static int nvi_pm_init(struct inv_gyro_state_s *inf)
1069 {
1070         int err = 0;
1071
1072         nvi_vreg_init(inf);
1073         inf->pm = NVI_PM_ERR;
1074         err = nvi_pm(inf, NVI_PM_ON);
1075         return err;
1076 }
1077
1078 static int nvi_aux_delay(struct inv_gyro_state_s *inf,
1079                          int port, unsigned int delay_ms)
1080 {
1081         struct aux_port *ap;
1082         u8 val;
1083         u8 i;
1084         unsigned int delay_new;
1085         int delay_rtn;
1086
1087         if (port != AUX_PORT_BYPASS)
1088                 inf->aux.port[port].nmp.delay_ms = delay_ms;
1089         /* determine valid delays by ports enabled */
1090         delay_new = 0;
1091         delay_rtn = 0;
1092         for (i = 0; i < AUX_PORT_SPECIAL; i++) {
1093                 ap = &inf->aux.port[i];
1094                 if (delay_rtn < ap->nmp.delay_ms)
1095                         delay_rtn = ap->nmp.delay_ms;
1096                 if (inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) {
1097                         if (delay_new < ap->nmp.delay_ms)
1098                                 delay_new = ap->nmp.delay_ms;
1099                 }
1100         }
1101         ap = &inf->aux.port[AUX_PORT_SPECIAL];
1102         if (delay_rtn < ap->nmp.delay_ms)
1103                 delay_rtn = ap->nmp.delay_ms;
1104         if (inf->hw.i2c_slv4_ctrl & BIT_SLV_EN) {
1105                 if (delay_new < ap->nmp.delay_ms)
1106                         delay_new = ap->nmp.delay_ms;
1107         }
1108         if (!(inf->hw.user_ctrl & BIT_I2C_MST_EN)) {
1109                 /* delay will execute when re-enabled */
1110                 if (delay_ms)
1111                         return delay_rtn;
1112                 else
1113                         return 0;
1114         }
1115
1116         /* HW global delay */
1117         delay_new *= 1000;
1118         delay_new /= inf->sample_delay_us;
1119         delay_new++;
1120         inf->aux.delay_hw = (u8)delay_new;
1121         nvi_i2c_slv4_ctrl_wr(inf, (bool)(inf->hw.i2c_slv4_ctrl & BIT_SLV_EN));
1122         /* HW port delay enable */
1123         val = BIT_DELAY_ES_SHADOW;
1124         for (i = 0; i < AUX_PORT_MAX; i++) {
1125                 ap = &inf->aux.port[i];
1126                 if (ap->nmp.delay_ms)
1127                         val |= (1 << i);
1128         }
1129         nvi_i2c_mst_delay_ctrl_wr(inf, val);
1130         if (delay_ms)
1131                 return delay_rtn;
1132         else
1133                 return 0;
1134 }
1135
1136 static int nvi_global_delay(struct inv_gyro_state_s *inf)
1137 {
1138         unsigned long delay_us;
1139         unsigned long delay_us_old;
1140         unsigned long fs_hz;
1141         u8 dlpf;
1142         u8 smplrt_div;
1143         int i;
1144         int err;
1145         int err_t = 0;
1146
1147         /* find the fastest polling of all the devices */
1148         delay_us = -1;
1149         for (i = 0; i < AUX_PORT_MAX; i++) {
1150                 if (inf->aux.port[i].enable && inf->aux.port[i].nmp.delay_us) {
1151                         if (inf->aux.port[i].nmp.delay_us < delay_us)
1152                                 delay_us = inf->aux.port[i].nmp.delay_us;
1153                 }
1154         }
1155         if (inf->chip_config.gyro_enable && inf->chip_config.gyro_delay_us) {
1156                 if (inf->chip_config.gyro_delay_us < delay_us)
1157                         delay_us = inf->chip_config.gyro_delay_us;
1158         }
1159         if (inf->chip_config.accl_enable && inf->chip_config.accl_delay_us) {
1160                 if (inf->chip_config.accl_delay_us < delay_us)
1161                         delay_us = inf->chip_config.accl_delay_us;
1162         }
1163         if (delay_us == -1)
1164                 delay_us = NVI_DELAY_DEFAULT; /* default if nothing found */
1165         /* set the limits */
1166         if (delay_us < inf->chip_config.min_delay_us)
1167                 delay_us = inf->chip_config.min_delay_us;
1168         if (delay_us > NVI_DELAY_US_MAX)
1169                 delay_us = NVI_DELAY_US_MAX;
1170         delay_us_old = inf->sample_delay_us;
1171         inf->sample_delay_us = delay_us;
1172         delay_us <<= 1;
1173         for (dlpf = 0; dlpf < ARRAY_SIZE(nvi_lpf_us_tbl); dlpf++) {
1174                 if (delay_us < nvi_lpf_us_tbl[dlpf])
1175                         break;
1176         }
1177         if (dlpf)
1178                 fs_hz = 1000;
1179         else
1180                 fs_hz = 8000;
1181         smplrt_div = inf->sample_delay_us / fs_hz - 1;
1182         dlpf |= (inf->hw.config & 0xF8);
1183         fs_hz = 1000000 / inf->sample_delay_us;
1184         if ((smplrt_div != inf->hw.smplrt_div) || (dlpf != inf->hw.config)) {
1185                 if (inf->dbg)
1186                         dev_info(&inf->i2c->dev, "%s %lu\n",
1187                                  __func__, delay_us);
1188                 if (inf->sample_delay_us < delay_us_old) {
1189                         /* go faster */
1190                         if (inf->chip_type == INV_MPU3050) {
1191                                 if (inf->mpu_slave != NULL)
1192                                         inf->mpu_slave->set_lpf(inf, fs_hz);
1193                                 dlpf |= (inf->hw.config & 0xE7);
1194                         } else {
1195                                 nvi_aux_delay(inf, AUX_PORT_BYPASS, 0);
1196                         }
1197                         err = nvi_config_wr(inf, dlpf);
1198                         if (err < 0)
1199                                 err_t |= err;
1200                         err_t |= nvi_smplrt_div_wr(inf, smplrt_div);
1201                 } else {
1202                         /* go slower */
1203                         err_t |= nvi_smplrt_div_wr(inf, smplrt_div);
1204                         if (inf->chip_type == INV_MPU3050) {
1205                                 if (inf->mpu_slave != NULL)
1206                                         inf->mpu_slave->set_lpf(inf, fs_hz);
1207                                 dlpf |= (inf->hw.config & 0xE7);
1208                                 err = nvi_config_wr(inf, dlpf);
1209                         } else {
1210                                 err = nvi_config_wr(inf, dlpf);
1211                                 nvi_aux_delay(inf, AUX_PORT_BYPASS, 0);
1212                         }
1213                         if (err < 0)
1214                                 err_t |= err;
1215                 }
1216         }
1217         return err_t;
1218 }
1219
1220 static void nvi_aux_dbg(struct inv_gyro_state_s *inf, char *tag, int val)
1221 {
1222         struct nvi_mpu_port *n;
1223         struct aux_port *p;
1224         struct aux_ports *a;
1225         u8 data[4];
1226         int i;
1227
1228         if (!(inf->dbg & NVI_DBG_SPEW_AUX))
1229                 return;
1230
1231         dev_info(&inf->i2c->dev, "%s %s %d\n", __func__, tag, val);
1232         for (i = 0; i < AUX_PORT_MAX; i++) {
1233                 inv_i2c_read(inf, (REG_I2C_SLV0_ADDR + (i * 3)), 3, data);
1234                 inv_i2c_read(inf, (REG_I2C_SLV0_DO + i), 1, &data[3]);
1235                 /* HW = hardware */
1236                 pr_info("HW: P%d AD=%x RG=%x CL=%x DO=%x\n",
1237                         i, data[0], data[1], data[2], data[3]);
1238                 n = &inf->aux.port[i].nmp;
1239                 /* NS = nmp structure */
1240                 pr_info("NS: P%d AD=%x RG=%x CL=%x DO=%x MS=%u US=%lu SB=%x\n",
1241                         i, n->addr, n->reg, n->ctrl, n->data_out, n->delay_ms,
1242                         n->delay_us, n->shutdown_bypass);
1243                 p = &inf->aux.port[i];
1244                 /* PS = port structure */
1245                 pr_info("PS: P%d OF=%u EN=%x FE=%x HD=%x\n", i,
1246                         p->ext_data_offset, p->enable, p->fifo_en, p->hw_do);
1247         }
1248         a = &inf->aux;
1249         pr_info("AUX: EN=%x GE=%x MD=%x GD=%lu DN=%u BE=%x BL=%d MX=%d\n",
1250                 a->enable, (bool)(inf->hw.user_ctrl & BIT_I2C_MST_EN),
1251                 (inf->hw.i2c_slv4_ctrl & BITS_I2C_MST_DLY),
1252                 inf->sample_delay_us, a->ext_data_n,
1253                 (inf->hw.int_pin_cfg & BIT_BYPASS_EN), a->bypass_lock,
1254                 atomic_read(&inf->mutex.count));
1255 }
1256
1257 static void nvi_aux_read(struct inv_gyro_state_s *inf)
1258 {
1259         struct aux_port *ap;
1260         long long timestamp1;
1261         long long timestamp2;
1262         unsigned int i;
1263         unsigned int len;
1264         u8 *p;
1265         int err;
1266
1267         if ((inf->aux.ext_data_n == 0) ||
1268                                        (!(inf->hw.user_ctrl & BIT_I2C_MST_EN)))
1269                 return;
1270
1271         timestamp1 = nvi_ts_ns();
1272         err = inv_i2c_read(inf, REG_EXT_SENS_DATA_00,
1273                            inf->aux.ext_data_n,
1274                            (unsigned char *)&inf->aux.ext_data);
1275         if (err)
1276                 return;
1277
1278         timestamp2 = nvi_ts_ns();
1279         timestamp1 = timestamp1 + ((timestamp2 - timestamp1) / 2);
1280         for (i = 0; i < AUX_PORT_SPECIAL; i++) {
1281                 ap = &inf->aux.port[i];
1282                 if ((inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) && (!ap->fifo_en) &&
1283                                                (ap->nmp.addr & BIT_I2C_READ) &&
1284                                                    (ap->nmp.handler != NULL)) {
1285                         p = &inf->aux.ext_data[ap->ext_data_offset];
1286                         len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
1287                         ap->nmp.handler(p, len, timestamp1,
1288                                         ap->nmp.ext_driver);
1289                 }
1290         }
1291 }
1292
1293 static void nvi_aux_ext_data_offset(struct inv_gyro_state_s *inf)
1294 {
1295         int i;
1296         unsigned short offset;
1297
1298         offset = 0;
1299         for (i = 0; i < AUX_PORT_SPECIAL; i++) {
1300                 if ((inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) &&
1301                                   (inf->aux.port[i].nmp.addr & BIT_I2C_READ)) {
1302                         inf->aux.port[i].ext_data_offset = offset;
1303                         offset += (inf->aux.port[i].nmp.ctrl &
1304                                    BITS_I2C_SLV_CTRL_LEN);
1305                 }
1306         }
1307         if (offset > AUX_EXT_DATA_REG_MAX) {
1308                 offset = AUX_EXT_DATA_REG_MAX;
1309                 dev_err(&inf->i2c->dev,
1310                         "%s ERR MPU slaves exceed data storage\n", __func__);
1311         }
1312         inf->aux.ext_data_n = offset;
1313         return;
1314 }
1315
1316 static int nvi_aux_port_data_out(struct inv_gyro_state_s *inf,
1317                                  int port, u8 data_out)
1318 {
1319         int err;
1320
1321         err = nvi_i2c_slv_do_wr(inf, port, data_out);
1322         if (!err) {
1323                 inf->aux.port[port].nmp.data_out = data_out;
1324                 inf->aux.port[port].hw_do = true;
1325         } else {
1326                 inf->aux.port[port].hw_do = false;
1327         }
1328         return err;
1329 }
1330
1331 static int nvi_aux_port_wr(struct inv_gyro_state_s *inf, int port)
1332 {
1333         struct aux_port *ap;
1334         int err;
1335
1336         ap = &inf->aux.port[port];
1337         err = nvi_i2c_slv_addr_wr(inf, port, ap->nmp.addr);
1338         err |= nvi_i2c_slv_reg_wr(inf, port, ap->nmp.reg);
1339         err |= nvi_i2c_slv_do_wr(inf, port, ap->nmp.data_out);
1340         return err;
1341 }
1342
1343 static int nvi_aux_port_en(struct inv_gyro_state_s *inf,
1344                            int port, bool en)
1345 {
1346         struct aux_port *ap;
1347         u8 val;
1348         int err = 0;
1349
1350         inf->aux.ext_data_n = 0;
1351         ap = &inf->aux.port[port];
1352         if ((!(inf->hw.i2c_slv_addr[port])) && en) {
1353                 err = nvi_aux_port_wr(inf, port);
1354                 if (!err)
1355                         ap->hw_do = true;
1356         }
1357         if ((!ap->hw_do) && en)
1358                 nvi_aux_port_data_out(inf, port, ap->nmp.data_out);
1359         if (port == AUX_PORT_SPECIAL) {
1360                 err = nvi_i2c_slv4_ctrl_wr(inf, en);
1361         } else {
1362                 if (en)
1363                         val = (ap->nmp.ctrl | BIT_SLV_EN);
1364                 else
1365                         val = 0;
1366                 err = nvi_i2c_slv_ctrl_wr(inf, port, val);
1367         }
1368         if (err > 0) {
1369                 nvi_aux_ext_data_offset(inf);
1370                 err = 0;
1371         }
1372         return err;
1373 }
1374
1375 static int nvi_reset(struct inv_gyro_state_s *inf,
1376                      bool reset_fifo, bool reset_i2c);
1377
1378 static int nvi_aux_enable(struct inv_gyro_state_s *inf, bool enable)
1379 {
1380         bool en;
1381         unsigned int i;
1382         int err = 0;
1383
1384         if (inf->hw.int_pin_cfg & BIT_BYPASS_EN)
1385                 enable = false;
1386         en = false;
1387         if (enable) {
1388                 /* global enable is honored only if a port is enabled */
1389                 for (i = 0; i < AUX_PORT_MAX; i++) {
1390                         if (inf->aux.port[i].enable) {
1391                                 en = true;
1392                                 break;
1393                         }
1394                 }
1395                 if (en == (bool)(inf->hw.user_ctrl & BIT_I2C_MST_EN))
1396                         /* if already on then just update delays */
1397                         nvi_global_delay(inf);
1398         }
1399         inf->aux.enable = en;
1400         if ((bool)(inf->hw.user_ctrl & BIT_I2C_MST_EN) == en) {
1401                 if (inf->aux.reset_fifo)
1402                         nvi_reset(inf, true, false);
1403                 return 0;
1404         }
1405
1406         if (en) {
1407                 for (i = 0; i < AUX_PORT_MAX; i++) {
1408                         if (inf->aux.port[i].enable)
1409                                 err |= nvi_aux_port_en(inf, i, true);
1410                 }
1411         } else {
1412                 for (i = 0; i < AUX_PORT_MAX; i++) {
1413                         if (inf->hw.i2c_slv_addr[i])
1414                                 nvi_aux_port_en(inf, i, false);
1415                 }
1416         }
1417         err |= nvi_global_delay(inf);
1418         if (inf->aux.reset_fifo)
1419                 err |= nvi_reset(inf, true, false);
1420         else
1421                 err |= nvi_user_ctrl_en(inf, true, en);
1422         return err;
1423 }
1424
1425 static int nvi_aux_port_enable(struct inv_gyro_state_s *inf,
1426                                int port, bool enable, bool fifo_enable)
1427 {
1428         struct aux_port *ap;
1429         int err;
1430
1431         ap = &inf->aux.port[port];
1432         ap->enable = enable;
1433         if ((!enable) || (!(ap->nmp.addr & BIT_I2C_READ)))
1434                 fifo_enable = false;
1435         if (ap->fifo_en != fifo_enable)
1436                 inf->aux.reset_fifo = true;
1437         ap->fifo_en = fifo_enable;
1438         if (enable && (inf->hw.int_pin_cfg & BIT_BYPASS_EN))
1439                 return 0;
1440
1441         err = nvi_aux_port_en(inf, port, enable);
1442         err |= nvi_aux_enable(inf, true);
1443         return err;
1444 }
1445
1446 static int nvi_reset(struct inv_gyro_state_s *inf,
1447                      bool reset_fifo, bool reset_i2c)
1448 {
1449         u8 val;
1450         unsigned long flags;
1451         int err;
1452
1453         dev_dbg(&inf->i2c->dev, "%s FIFO=%x I2C=%x\n",
1454                 __func__, reset_fifo, reset_i2c);
1455         err = nvi_int_enable_wr(inf, false);
1456         val = 0;
1457         if (reset_i2c) {
1458                 inf->aux.reset_i2c = false;
1459                 /* nvi_aux_bypass_enable(inf, false)? */
1460                 err |= nvi_aux_enable(inf, false);
1461                 val |= inf->reg->i2c_mst_reset;
1462         }
1463         if (reset_fifo)
1464                 val |= inf->reg->fifo_reset;
1465         err |= nvi_user_ctrl_en(inf, !reset_fifo, !reset_i2c);
1466         val |= inf->hw.user_ctrl;
1467         err |= nvi_user_ctrl_reset_wr(inf, val);
1468         if (reset_i2c)
1469                 err |= nvi_aux_enable(inf, true);
1470         err |= nvi_user_ctrl_en(inf, true, true);
1471         if (reset_fifo && (inf->hw.user_ctrl & BIT_FIFO_EN)) {
1472                 spin_lock_irqsave(&inf->time_stamp_lock, flags);
1473                 kfifo_reset(&inf->trigger.timestamps);
1474                 spin_unlock_irqrestore(&inf->time_stamp_lock, flags);
1475                 inf->fifo_ts = nvi_ts_ns();
1476                 inf->fifo_reset_3050 = true;
1477         }
1478         err |= nvi_int_enable_wr(inf, true);
1479         return err;
1480 }
1481
1482 static int nvi_aux_port_free(struct inv_gyro_state_s *inf, int port)
1483 {
1484         memset(&inf->aux.port[port], 0, sizeof(struct aux_port));
1485         if (inf->hw.i2c_slv_addr[port]) {
1486                 nvi_aux_port_wr(inf, port);
1487                 nvi_aux_port_en(inf, port, false);
1488                 nvi_aux_enable(inf, false);
1489                 nvi_aux_enable(inf, true);
1490                 if (port != AUX_PORT_SPECIAL)
1491                         inf->aux.reset_i2c = true;
1492         }
1493         return 0;
1494 }
1495
1496 static int nvi_aux_port_alloc(struct inv_gyro_state_s *inf,
1497                               struct nvi_mpu_port *nmp, int port)
1498 {
1499         int i;
1500
1501         if (inf->aux.reset_i2c)
1502                 nvi_reset(inf, false, true);
1503         if (port < 0) {
1504                 for (i = 0; i < AUX_PORT_SPECIAL; i++) {
1505                         if (inf->aux.port[i].nmp.addr == 0)
1506                                 break;
1507                 }
1508                 if (i == AUX_PORT_SPECIAL)
1509                         return -ENODEV;
1510         } else {
1511                 if (inf->aux.port[port].nmp.addr == 0)
1512                         i = port;
1513                 else
1514                         return -ENODEV;
1515         }
1516
1517         memset(&inf->aux.port[i], 0, sizeof(struct aux_port));
1518         memcpy(&inf->aux.port[i].nmp, nmp, sizeof(struct nvi_mpu_port));
1519         return i;
1520 }
1521
1522 static int nvi_aux_bypass_enable(struct inv_gyro_state_s *inf, bool enable)
1523 {
1524         u8 val;
1525         int err;
1526
1527         if ((bool)(inf->hw.int_pin_cfg & BIT_BYPASS_EN) == enable)
1528                 return 0;
1529
1530         val = inf->hw.int_pin_cfg;
1531         if (enable) {
1532                 err = nvi_aux_enable(inf, false);
1533                 if (!err) {
1534                         val |= BIT_BYPASS_EN;
1535                         err = nvi_int_pin_cfg_wr(inf, val);
1536                 }
1537         } else {
1538                 val &= ~BIT_BYPASS_EN;
1539                 err = nvi_int_pin_cfg_wr(inf, val);
1540                 if (!err)
1541                         nvi_aux_enable(inf, true);
1542         }
1543         return err;
1544 }
1545
1546 static int nvi_aux_bypass_request(struct inv_gyro_state_s *inf, bool enable)
1547 {
1548         s64 ns;
1549         s64 to;
1550         int err = 0;
1551
1552         if ((bool)(inf->hw.int_pin_cfg & BIT_BYPASS_EN) == enable) {
1553                 inf->aux.bypass_timeout_ns = nvi_ts_ns();
1554                 inf->aux.bypass_lock++;
1555                 if (!inf->aux.bypass_lock)
1556                         dev_err(&inf->i2c->dev, "%s rollover ERR\n", __func__);
1557         } else {
1558                 if (inf->aux.bypass_lock) {
1559                         ns = nvi_ts_ns() - inf->aux.bypass_timeout_ns;
1560                         to = inf->chip_config.bypass_timeout_ms * 1000000;
1561                         if (ns > to)
1562                                 inf->aux.bypass_lock = 0;
1563                         else
1564                                 err = -EBUSY;
1565                 }
1566                 if (!inf->aux.bypass_lock) {
1567                         err = nvi_aux_bypass_enable(inf, enable);
1568                         if (err)
1569                                 dev_err(&inf->i2c->dev, "%s ERR=%d\n",
1570                                         __func__, err);
1571                         else
1572                                 inf->aux.bypass_lock++;
1573                 }
1574         }
1575         return err;
1576 }
1577
1578 static int nvi_aux_bypass_release(struct inv_gyro_state_s *inf)
1579 {
1580         int err = 0;
1581
1582         if (inf->aux.bypass_lock)
1583                 inf->aux.bypass_lock--;
1584         if (!inf->aux.bypass_lock) {
1585                 err = nvi_aux_bypass_enable(inf, false);
1586                 if (err)
1587                         dev_err(&inf->i2c->dev, "%s ERR=%d\n", __func__, err);
1588         }
1589         return err;
1590 }
1591
1592 static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf,
1593                              struct nvi_mpu_port *nmp, u8 *data)
1594 {
1595         unsigned char val;
1596         int i;
1597         int err;
1598
1599         /* turn off bypass */
1600         err = nvi_aux_bypass_request(inf, false);
1601         if (err)
1602                 return -EBUSY;
1603
1604         /* grab the special port */
1605         err = nvi_aux_port_alloc(inf, nmp, AUX_PORT_SPECIAL);
1606         if (err != AUX_PORT_SPECIAL) {
1607                 nvi_aux_bypass_release(inf);
1608                 return -EBUSY;
1609         }
1610
1611         /* enable it */
1612         inf->aux.port[AUX_PORT_SPECIAL].nmp.delay_ms = 0;
1613         inf->aux.port[AUX_PORT_SPECIAL].nmp.delay_us = NVI_DELAY_US_MIN;
1614         err = nvi_aux_port_enable(inf, AUX_PORT_SPECIAL, true, false);
1615         if (err) {
1616                 nvi_aux_port_free(inf, AUX_PORT_SPECIAL);
1617                 nvi_aux_bypass_release(inf);
1618                 return -EBUSY;
1619         }
1620
1621         /* now turn off all the other ports for fastest response */
1622         for (i = 0; i < AUX_PORT_SPECIAL; i++) {
1623                 if (inf->hw.i2c_slv_addr[i])
1624                         nvi_aux_port_en(inf, i, false);
1625         }
1626         /* start reading the results */
1627         for (i = 0; i < AUX_DEV_VALID_READ_LOOP_MAX; i++) {
1628                 mdelay(AUX_DEV_VALID_READ_DELAY_MS);
1629                 val = 0;
1630                 err = inv_i2c_read(inf, REG_I2C_MST_STATUS, 1, &val);
1631                 if (err)
1632                         continue;
1633
1634                 if (val & 0x50)
1635                         break;
1636         }
1637         /* these will restore all previously disabled ports */
1638         nvi_aux_bypass_release(inf);
1639         nvi_aux_port_free(inf, AUX_PORT_SPECIAL);
1640         if (i == AUX_DEV_VALID_READ_LOOP_MAX)
1641                 return -ENODEV;
1642
1643         if (val & 0x10) /* NACK */
1644                 return -EIO;
1645
1646         if (nmp->addr & BIT_I2C_READ) {
1647                 err = inv_i2c_read(inf, REG_I2C_SLV4_DI, 1, &val);
1648                 if (err)
1649                         return -EBUSY;
1650
1651                 *data = (u8)val;
1652                 dev_info(&inf->i2c->dev, "%s MPU read 0x%x from device 0x%x\n",
1653                         __func__, val, (nmp->addr & ~BIT_I2C_READ));
1654         } else {
1655                 dev_info(&inf->i2c->dev, "%s MPU found device 0x%x\n",
1656                         __func__, (nmp->addr & ~BIT_I2C_READ));
1657         }
1658         return 0;
1659 }
1660
1661 static int nvi_aux_mpu_call_pre(struct inv_gyro_state_s *inf, int port)
1662 {
1663         if ((port < 0) || (port >= AUX_PORT_SPECIAL))
1664                 return -EINVAL;
1665
1666         if (inf->shutdown || inf->suspend)
1667                 return -EPERM;
1668
1669         if (!inf->aux.port[port].nmp.addr)
1670                 return -EINVAL;
1671
1672         mutex_lock(&inf->mutex);
1673         return 0;
1674 }
1675
1676 static int nvi_aux_mpu_call_post(struct inv_gyro_state_s *inf,
1677                                  char *tag, int err)
1678 {
1679         if (err < 0)
1680                 err = -EBUSY;
1681         mutex_unlock(&inf->mutex);
1682         nvi_aux_dbg(inf, tag, err);
1683         return err;
1684 }
1685
1686 /* See the mpu.h file for details on the nvi_mpu_ calls.
1687  */
1688 int nvi_mpu_dev_valid(struct nvi_mpu_port *nmp, u8 *data)
1689 {
1690         struct inv_gyro_state_s *inf;
1691         int err;
1692
1693         inf = inf_local;
1694         if (inf != NULL) {
1695                 if (inf->dbg & NVI_DBG_SPEW_AUX)
1696                         pr_info("%s\n", __func__);
1697         } else {
1698                 pr_debug("%s ERR -EAGAIN\n", __func__);
1699                 return -EAGAIN;
1700         }
1701
1702         if (nmp == NULL)
1703                 return -EINVAL;
1704
1705         if ((nmp->addr & BIT_I2C_READ) && (data == NULL))
1706                 return -EINVAL;
1707
1708         if (inf->shutdown || inf->suspend)
1709                 return -EPERM;
1710
1711         mutex_lock(&inf->mutex);
1712         nvi_pm(inf, NVI_PM_ON);
1713         err = nvi_aux_dev_valid(inf, nmp, data);
1714         nvi_pm(inf, NVI_PM_AUTO);
1715         mutex_unlock(&inf->mutex);
1716         nvi_aux_dbg(inf, "nvi_mpu_dev_valid err: ", err);
1717         return err;
1718 }
1719 EXPORT_SYMBOL(nvi_mpu_dev_valid);
1720
1721 int nvi_mpu_port_alloc(struct nvi_mpu_port *nmp)
1722 {
1723         struct inv_gyro_state_s *inf;
1724         int err;
1725
1726         inf = inf_local;
1727         if (inf != NULL) {
1728                 if (inf->dbg & NVI_DBG_SPEW_AUX)
1729                         pr_info("%s\n", __func__);
1730         } else {
1731                 pr_debug("%s ERR -EAGAIN\n", __func__);
1732                 return -EAGAIN;
1733         }
1734
1735         if (nmp == NULL)
1736                 return -EINVAL;
1737
1738         if (!(nmp->ctrl & BITS_I2C_SLV_CTRL_LEN))
1739                 return -EINVAL;
1740
1741         if (inf->shutdown || inf->suspend)
1742                 return -EPERM;
1743
1744         mutex_lock(&inf->mutex);
1745         nvi_pm(inf, NVI_PM_ON);
1746         err = nvi_aux_port_alloc(inf, nmp, -1);
1747         nvi_pm(inf, NVI_PM_AUTO);
1748         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_port_alloc err/port: ", err);
1749         return err;
1750 }
1751 EXPORT_SYMBOL(nvi_mpu_port_alloc);
1752
1753 int nvi_mpu_port_free(int port)
1754 {
1755         struct inv_gyro_state_s *inf;
1756         int err;
1757
1758         inf = inf_local;
1759         if (inf != NULL) {
1760                 if (inf->dbg & NVI_DBG_SPEW_AUX)
1761                         pr_info("%s port %d\n", __func__, port);
1762         } else {
1763                 pr_debug("%s port %d ERR -EAGAIN\n", __func__, port);
1764                 return -EAGAIN;
1765         }
1766
1767         err = nvi_aux_mpu_call_pre(inf, port);
1768         if (err)
1769                 return err;
1770
1771         nvi_pm(inf, NVI_PM_ON);
1772         err = nvi_aux_port_free(inf, port);
1773         nvi_pm(inf, NVI_PM_AUTO);
1774         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_port_free err: ", err);
1775         return err;
1776 }
1777 EXPORT_SYMBOL(nvi_mpu_port_free);
1778
1779 int nvi_mpu_enable(int port, bool enable, bool fifo_enable)
1780 {
1781         struct inv_gyro_state_s *inf;
1782         int err;
1783
1784         inf = inf_local;
1785         if (inf != NULL) {
1786                 if (inf->dbg & NVI_DBG_SPEW_AUX)
1787                         pr_info("%s port %d: %x\n", __func__, port, enable);
1788         } else {
1789                 pr_debug("%s port %d: %x ERR -EAGAIN\n",
1790                          __func__, port, enable);
1791                 return -EAGAIN;
1792         }
1793
1794         err = nvi_aux_mpu_call_pre(inf, port);
1795         if (err)
1796                 return err;
1797
1798         nvi_pm(inf, NVI_PM_ON);
1799         err = nvi_aux_port_enable(inf, port, enable, fifo_enable);
1800         nvi_pm(inf, NVI_PM_AUTO);
1801         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_enable err: ", err);
1802         return err;
1803 }
1804 EXPORT_SYMBOL(nvi_mpu_enable);
1805
1806 int nvi_mpu_delay_ms(int port, u8 delay_ms)
1807 {
1808         struct inv_gyro_state_s *inf;
1809         int err;
1810
1811         inf = inf_local;
1812         if (inf != NULL) {
1813                 if (inf->dbg & NVI_DBG_SPEW_AUX)
1814                         pr_info("%s port %d: %u\n", __func__, port, delay_ms);
1815         } else {
1816                 pr_debug("%s port %d: %u ERR -EAGAIN\n",
1817                          __func__, port, delay_ms);
1818                 return -EAGAIN;
1819         }
1820
1821         err = nvi_aux_mpu_call_pre(inf, port);
1822         if (err)
1823                 return err;
1824
1825         if (inf->hw.i2c_slv_ctrl[port] & BIT_SLV_EN) {
1826                 err = nvi_aux_delay(inf, port, delay_ms);
1827                 nvi_global_delay(inf);
1828         } else {
1829                 inf->aux.port[port].nmp.delay_ms = delay_ms;
1830         }
1831         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_delay_ms err: ", err);
1832         return err;
1833 }
1834 EXPORT_SYMBOL(nvi_mpu_delay_ms);
1835
1836 int nvi_mpu_delay_us(int port, unsigned long delay_us)
1837 {
1838         struct inv_gyro_state_s *inf;
1839         int err;
1840
1841         inf = inf_local;
1842         if (inf != NULL) {
1843                 if (inf->dbg & NVI_DBG_SPEW_AUX)
1844                         pr_info("%s port %d: %lu\n", __func__, port, delay_us);
1845         } else {
1846                 pr_debug("%s port %d: %lu ERR -EAGAIN\n",
1847                         __func__, port, delay_us);
1848                 return -EAGAIN;
1849         }
1850
1851         err = nvi_aux_mpu_call_pre(inf, port);
1852         if (err)
1853                 return err;
1854
1855         inf->aux.port[port].nmp.delay_us = delay_us;
1856         if (inf->hw.i2c_slv_ctrl[port] & BIT_SLV_EN)
1857                 err = nvi_global_delay(inf);
1858         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_delay_us err: ", err);
1859         return err;
1860 }
1861 EXPORT_SYMBOL(nvi_mpu_delay_us);
1862
1863 int nvi_mpu_data_out(int port, u8 data_out)
1864 {
1865         struct inv_gyro_state_s *inf;
1866         int err;
1867
1868         inf = inf_local;
1869         if (inf == NULL)
1870                 return -EAGAIN;
1871
1872         err = nvi_aux_mpu_call_pre(inf, port);
1873         if (err)
1874                 return err;
1875
1876         if (inf->hw.i2c_slv_ctrl[port] & BIT_SLV_EN) {
1877                 err = nvi_aux_port_data_out(inf, port, data_out);
1878         } else {
1879                 inf->aux.port[port].nmp.data_out = data_out;
1880                 inf->aux.port[port].hw_do = false;
1881         }
1882         if (err < 0)
1883                 err = -EBUSY;
1884         mutex_unlock(&inf->mutex);
1885         return err;
1886 }
1887 EXPORT_SYMBOL(nvi_mpu_data_out);
1888
1889 int nvi_mpu_bypass_request(bool enable)
1890 {
1891         struct inv_gyro_state_s *inf;
1892         int err;
1893
1894         inf = inf_local;
1895         if (inf != NULL) {
1896                 if (inf->dbg & NVI_DBG_SPEW_AUX)
1897                         pr_info("%s\n", __func__);
1898         } else {
1899                 pr_debug("%s ERR -EAGAIN\n", __func__);
1900                 return -EAGAIN;
1901         }
1902
1903         if (inf->shutdown || inf->suspend)
1904                 return -EPERM;
1905
1906         mutex_lock(&inf->mutex);
1907         nvi_pm(inf, NVI_PM_ON);
1908         err = nvi_aux_bypass_request(inf, enable);
1909         nvi_pm(inf, NVI_PM_AUTO);
1910         err = nvi_aux_mpu_call_post(inf, "nvi_mpu_bypass_request err: ", err);
1911         return err;
1912 }
1913 EXPORT_SYMBOL(nvi_mpu_bypass_request);
1914
1915 int nvi_mpu_bypass_release(void)
1916 {
1917         struct inv_gyro_state_s *inf;
1918
1919         inf = inf_local;
1920         if (inf != NULL) {
1921                 if (inf->dbg & NVI_DBG_SPEW_AUX)
1922                         pr_info("%s\n", __func__);
1923         } else {
1924                 pr_debug("%s\n", __func__);
1925                 return 0;
1926         }
1927
1928         if (inf->shutdown || inf->suspend)
1929                 return 0;
1930
1931         mutex_lock(&inf->mutex);
1932         nvi_pm(inf, NVI_PM_ON);
1933         nvi_aux_bypass_release(inf);
1934         nvi_pm(inf, NVI_PM_AUTO);
1935         nvi_aux_mpu_call_post(inf, "nvi_mpu_bypass_release", 0);
1936         return 0;
1937 }
1938 EXPORT_SYMBOL(nvi_mpu_bypass_release);
1939
1940
1941 int nvi_gyro_enable(struct inv_gyro_state_s *inf,
1942                     unsigned char enable, unsigned char fifo_enable)
1943 {
1944         unsigned char enable_old;
1945         unsigned char fifo_enable_old;
1946         int err;
1947         int err_t;
1948
1949         enable_old = inf->chip_config.gyro_enable;
1950         fifo_enable_old = inf->chip_config.gyro_fifo_enable;
1951         inf->chip_config.gyro_fifo_enable = fifo_enable;
1952         inf->chip_config.gyro_enable = enable;
1953         err_t = nvi_pm(inf, NVI_PM_ON_FULL);
1954         if (enable != enable_old) {
1955                 if (enable) {
1956                         err = nvi_gyro_config_wr(inf, 0,
1957                                                  inf->chip_config.gyro_fsr);
1958                         if (err < 0)
1959                                 err_t |= err;
1960                 }
1961                 nvi_global_delay(inf);
1962         }
1963         if (fifo_enable_old != fifo_enable)
1964                 err_t = nvi_reset(inf, true, false);
1965         if (err_t) {
1966                 inf->chip_config.gyro_enable = enable_old;
1967                 inf->chip_config.gyro_fifo_enable = fifo_enable_old;
1968         }
1969         if (inf->chip_config.gyro_enable)
1970                 inf->chip_config.temp_enable |= NVI_TEMP_GYRO;
1971         else
1972                 inf->chip_config.temp_enable &= ~NVI_TEMP_GYRO;
1973         err_t |= nvi_pm(inf, NVI_PM_AUTO);
1974         return err_t;
1975 }
1976
1977 int nvi_accl_enable(struct inv_gyro_state_s *inf,
1978                     unsigned char enable, unsigned char fifo_enable)
1979 {
1980         unsigned char enable_old;
1981         unsigned char fifo_enable_old;
1982         int err;
1983         int err_t;
1984
1985         enable_old = inf->chip_config.accl_enable;
1986         fifo_enable_old = inf->chip_config.accl_fifo_enable;
1987         inf->chip_config.accl_fifo_enable = fifo_enable;
1988         inf->chip_config.accl_enable = enable;
1989         err_t = nvi_pm(inf, NVI_PM_ON);
1990         if (enable != enable_old) {
1991                 if (inf->chip_type == INV_MPU3050) {
1992                         if (inf->mpu_slave != NULL) {
1993                                 if (enable) {
1994                                         inf->mpu_slave->resume(inf);
1995                                         err_t |= nvi_accel_config_wr(inf, 0,
1996                                                  inf->chip_config.accl_fsr, 0);
1997                                 } else {
1998                                         inf->mpu_slave->suspend(inf);
1999                                 }
2000                         }
2001                 } else {
2002                         if (enable) {
2003                                 err = nvi_accel_config_wr(inf, 0,
2004                                                  inf->chip_config.accl_fsr, 0);
2005                                 if (err < 0)
2006                                         err_t |= err;
2007                         }
2008                 }
2009                 nvi_global_delay(inf);
2010         }
2011         if (fifo_enable_old != fifo_enable)
2012                 err_t |= nvi_reset(inf, true, false);
2013         if (err_t) {
2014                 inf->chip_config.accl_enable = enable_old;
2015                 inf->chip_config.accl_fifo_enable = fifo_enable_old;
2016         }
2017         if (inf->chip_config.accl_enable)
2018                 inf->chip_config.temp_enable |= NVI_TEMP_ACCL;
2019         else
2020                 inf->chip_config.temp_enable &= ~NVI_TEMP_ACCL;
2021         err_t |= nvi_pm(inf, NVI_PM_AUTO);
2022         return err_t;
2023 }
2024
2025
2026 static ssize_t nvi_gyro_enable_store(struct device *dev,
2027                                      struct device_attribute *attr,
2028                                      const char *buf, size_t count)
2029 {
2030         struct inv_gyro_state_s *inf;
2031         unsigned char enable;
2032         unsigned char fifo_enable;
2033         int err;
2034
2035         inf = dev_get_drvdata(dev);
2036         err = kstrtou8(buf, 10, &enable);
2037         if (err)
2038                 return -EINVAL;
2039
2040         if (enable > 7)
2041                 enable = 7;
2042         mutex_lock(&inf->mutex);
2043         if (enable != inf->chip_config.gyro_enable) {
2044                 dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
2045                 fifo_enable = inf->chip_config.gyro_fifo_enable & enable;
2046                 err = nvi_gyro_enable(inf, enable, fifo_enable);
2047         }
2048         mutex_unlock(&inf->mutex);
2049         if (err) {
2050                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
2051                         __func__, enable, err);
2052                 return err;
2053         }
2054
2055         return count;
2056 }
2057
2058 static ssize_t nvi_gyro_enable_show(struct device *dev,
2059                                     struct device_attribute *attr, char *buf)
2060 {
2061         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2062
2063         return sprintf(buf, "%u\n", inf->chip_config.gyro_enable);
2064 }
2065
2066 static ssize_t nvi_gyro_fifo_enable_store(struct device *dev,
2067                                           struct device_attribute *attr,
2068                                           const char *buf, size_t count)
2069 {
2070         struct inv_gyro_state_s *inf;
2071         unsigned char fifo_enable;
2072         unsigned char enable;
2073         int err;
2074
2075         inf = dev_get_drvdata(dev);
2076         err = kstrtou8(buf, 10, &fifo_enable);
2077         if (err)
2078                 return -EINVAL;
2079
2080         if (fifo_enable > 7)
2081                 fifo_enable = 7;
2082         mutex_lock(&inf->mutex);
2083         if (fifo_enable != inf->chip_config.gyro_fifo_enable) {
2084                 dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
2085                 enable = inf->chip_config.gyro_enable | fifo_enable;
2086                 err = nvi_gyro_enable(inf, enable, fifo_enable);
2087         }
2088         mutex_unlock(&inf->mutex);
2089         if (err) {
2090                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
2091                         __func__, fifo_enable, err);
2092                 return err;
2093         }
2094
2095         return count;
2096 }
2097
2098 static ssize_t nvi_gyro_fifo_enable_show(struct device *dev,
2099                                          struct device_attribute *attr,
2100                                          char *buf)
2101 {
2102         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2103
2104         return sprintf(buf, "%u\n", inf->chip_config.gyro_fifo_enable);
2105 }
2106
2107 static ssize_t inv_gyro_delay_store(struct device *dev,
2108                                     struct device_attribute *attr,
2109                                     const char *buf, size_t count)
2110 {
2111         struct inv_gyro_state_s *inf;
2112         unsigned long gyro_delay_us;
2113         unsigned long gyro_delay_us_old;
2114         int err;
2115
2116         inf = dev_get_drvdata(dev);
2117         err = kstrtoul(buf, 10, &gyro_delay_us);
2118         if (err)
2119                 return err;
2120
2121         if (gyro_delay_us < NVI_INPUT_GYRO_DELAY_US_MIN)
2122                 gyro_delay_us = NVI_INPUT_GYRO_DELAY_US_MIN;
2123         mutex_lock(&inf->mutex);
2124         if (gyro_delay_us != inf->chip_config.gyro_delay_us) {
2125                 dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, gyro_delay_us);
2126                 gyro_delay_us_old = inf->chip_config.gyro_delay_us;
2127                 inf->chip_config.gyro_delay_us = gyro_delay_us;
2128                 if (inf->chip_config.gyro_enable) {
2129                         err = nvi_global_delay(inf);
2130                         if (err)
2131                                 inf->chip_config.gyro_delay_us =
2132                                                              gyro_delay_us_old;
2133                 }
2134         }
2135         mutex_unlock(&inf->mutex);
2136         if (err) {
2137                 dev_err(&inf->i2c->dev, "%s: %lu ERR=%d\n",
2138                         __func__, gyro_delay_us, err);
2139                 return err;
2140         }
2141
2142         return count;
2143 }
2144
2145 static ssize_t nvi_gyro_delay_show(struct device *dev,
2146                                    struct device_attribute *attr, char *buf)
2147 {
2148         struct inv_gyro_state_s *inf;
2149
2150         inf = dev_get_drvdata(dev);
2151         if (inf->chip_config.gyro_enable)
2152                 return sprintf(buf, "%lu\n", inf->sample_delay_us);
2153
2154         return sprintf(buf, "%d\n", NVI_INPUT_GYRO_DELAY_US_MIN);
2155 }
2156
2157 static ssize_t nvi_gyro_resolution_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 int resolution;
2163
2164         inf = dev_get_drvdata(dev);
2165         if (kstrtouint(buf, 10, &resolution))
2166                 return -EINVAL;
2167
2168         dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, resolution);
2169         inf->chip_config.gyro_resolution = resolution;
2170         return count;
2171 }
2172
2173 static ssize_t nvi_gyro_resolution_show(struct device *dev,
2174                                         struct device_attribute *attr,
2175                                         char *buf)
2176 {
2177         struct inv_gyro_state_s *inf;
2178         unsigned int resolution;
2179
2180         inf = dev_get_drvdata(dev);
2181         if (inf->chip_config.gyro_enable)
2182                 resolution = inf->chip_config.gyro_resolution;
2183         else
2184                 resolution = GYRO_INPUT_RESOLUTION;
2185         return sprintf(buf, "%u\n", resolution);
2186 }
2187
2188 static ssize_t nvi_gyro_max_range_store(struct device *dev,
2189                                         struct device_attribute *attr,
2190                                         const char *buf, size_t count)
2191 {
2192         struct inv_gyro_state_s *inf;
2193         unsigned char fsr;
2194         int err;
2195
2196         inf = dev_get_drvdata(dev);
2197         err = kstrtou8(buf, 10, &fsr);
2198         if (err)
2199                 return -EINVAL;
2200
2201         if (fsr > 3)
2202                 return -EINVAL;
2203
2204         mutex_lock(&inf->mutex);
2205         if (fsr != inf->chip_config.gyro_fsr) {
2206                 dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
2207                 if (inf->chip_config.gyro_enable) {
2208                         err = nvi_gyro_config_wr(inf, 0, fsr);
2209                         if ((err > 0) && (inf->hw.fifo_en & BITS_GYRO_OUT))
2210                                 nvi_reset(inf, true, false);
2211                 }
2212                 if (err >= 0)
2213                         inf->chip_config.gyro_fsr = fsr;
2214         }
2215         mutex_unlock(&inf->mutex);
2216         if (err < 0) {
2217                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", __func__, fsr, err);
2218                 return err;
2219         }
2220
2221         return count;
2222 }
2223
2224 static ssize_t nvi_gyro_max_range_show(struct device *dev,
2225                                        struct device_attribute *attr, char *buf)
2226 {
2227         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2228         unsigned int range;
2229
2230         if (inf->chip_config.gyro_enable)
2231                 range = inf->chip_config.gyro_fsr;
2232         else
2233                 range = (1 << inf->chip_config.gyro_fsr) * 250;
2234         return sprintf(buf, "%u\n", range);
2235 }
2236
2237 static ssize_t nvi_accl_enable_store(struct device *dev,
2238                                      struct device_attribute *attr,
2239                                      const char *buf, size_t count)
2240 {
2241         struct inv_gyro_state_s *inf;
2242         unsigned char enable;
2243         unsigned char fifo_enable;
2244         int err;
2245
2246         inf = dev_get_drvdata(dev);
2247         err = kstrtou8(buf, 10, &enable);
2248         if (err)
2249                 return -EINVAL;
2250
2251         if (enable > 7)
2252                 enable = 7;
2253         mutex_lock(&inf->mutex);
2254         if (enable != inf->chip_config.accl_enable) {
2255                 dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
2256                 fifo_enable = inf->chip_config.accl_fifo_enable & enable;
2257                 err = nvi_accl_enable(inf, enable, fifo_enable);
2258         }
2259         mutex_unlock(&inf->mutex);
2260         if (err) {
2261                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
2262                         __func__, enable, err);
2263                 return err;
2264         }
2265
2266         return count;
2267 }
2268
2269 static ssize_t nvi_accl_enable_show(struct device *dev,
2270                                     struct device_attribute *attr, char *buf)
2271 {
2272         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2273
2274         return sprintf(buf, "%u\n", inf->chip_config.accl_enable);
2275 }
2276
2277 static ssize_t nvi_accl_fifo_enable_store(struct device *dev,
2278                                           struct device_attribute *attr,
2279                                           const char *buf, size_t count)
2280 {
2281         struct inv_gyro_state_s *inf;
2282         unsigned char fifo_enable;
2283         unsigned char enable;
2284         int err;
2285
2286         inf = dev_get_drvdata(dev);
2287         err = kstrtou8(buf, 10, &fifo_enable);
2288         if (err)
2289                 return -EINVAL;
2290
2291         if (fifo_enable > 7)
2292                 fifo_enable = 7;
2293         mutex_lock(&inf->mutex);
2294         if (fifo_enable != inf->chip_config.accl_fifo_enable) {
2295                 dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
2296                 enable = inf->chip_config.accl_enable | fifo_enable;
2297                 err = nvi_accl_enable(inf, enable, fifo_enable);
2298         }
2299         mutex_unlock(&inf->mutex);
2300         if (err) {
2301                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
2302                         __func__, fifo_enable, err);
2303                 return err;
2304         }
2305
2306         return count;
2307 }
2308
2309 static ssize_t nvi_accl_fifo_enable_show(struct device *dev,
2310                                          struct device_attribute *attr,
2311                                          char *buf)
2312 {
2313         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2314
2315         return sprintf(buf, "%u\n", inf->chip_config.accl_fifo_enable);
2316 }
2317
2318 static ssize_t nvi_accl_delay_store(struct device *dev,
2319                                     struct device_attribute *attr,
2320                                     const char *buf, size_t count)
2321 {
2322         struct inv_gyro_state_s *inf;
2323         unsigned long accl_delay_us;
2324         unsigned long accl_delay_us_old;
2325         int err;
2326
2327         inf = dev_get_drvdata(dev);
2328         err = kstrtoul(buf, 10, &accl_delay_us);
2329         if (err)
2330                 return err;
2331
2332         if (accl_delay_us < NVI_INPUT_ACCL_DELAY_US_MIN)
2333                 accl_delay_us = NVI_INPUT_ACCL_DELAY_US_MIN;
2334         mutex_lock(&inf->mutex);
2335         if (accl_delay_us != inf->chip_config.accl_delay_us) {
2336                 dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, accl_delay_us);
2337                 accl_delay_us_old = inf->chip_config.accl_delay_us;
2338                 inf->chip_config.accl_delay_us = accl_delay_us;
2339                 if (inf->chip_config.accl_enable) {
2340                         if (inf->hw.pwr_mgmt_1 & inf->reg->cycle)
2341                                 nvi_pm(inf, NVI_PM_ON);
2342                         err = nvi_global_delay(inf);
2343                         if (err)
2344                                 inf->chip_config.accl_delay_us =
2345                                                              accl_delay_us_old;
2346                         else
2347                                 nvi_pm(inf, NVI_PM_AUTO);
2348                 }
2349         }
2350         mutex_unlock(&inf->mutex);
2351         if (err) {
2352                 dev_err(&inf->i2c->dev, "%s: %lu ERR=%d\n",
2353                         __func__, accl_delay_us, err);
2354                 return err;
2355         }
2356
2357         return count;
2358 }
2359
2360 static ssize_t nvi_accl_delay_show(struct device *dev,
2361                                    struct device_attribute *attr, char *buf)
2362 {
2363         struct inv_gyro_state_s *inf;
2364
2365         inf = dev_get_drvdata(dev);
2366         if (inf->chip_config.accl_enable)
2367                 return sprintf(buf, "%lu\n", inf->sample_delay_us);
2368
2369         return sprintf(buf, "%d\n", NVI_INPUT_ACCL_DELAY_US_MIN);
2370 }
2371
2372 static ssize_t nvi_accl_resolution_store(struct device *dev,
2373                                          struct device_attribute *attr,
2374                                          const char *buf, size_t count)
2375 {
2376         struct inv_gyro_state_s *inf;
2377         unsigned int resolution;
2378
2379         inf = dev_get_drvdata(dev);
2380         if (kstrtouint(buf, 10, &resolution))
2381                 return -EINVAL;
2382
2383         dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, resolution);
2384         inf->chip_config.accl_resolution = resolution;
2385         return count;
2386 }
2387
2388 static ssize_t nvi_accl_resolution_show(struct device *dev,
2389                                         struct device_attribute *attr,
2390                                         char *buf)
2391 {
2392         struct inv_gyro_state_s *inf;
2393         unsigned int resolution;
2394
2395         inf = dev_get_drvdata(dev);
2396         if (inf->chip_config.accl_enable)
2397                 resolution = inf->chip_config.accl_resolution;
2398         else
2399                 resolution = ACCL_INPUT_RESOLUTION;
2400         return sprintf(buf, "%u\n", resolution);
2401 }
2402
2403 static ssize_t nvi_accl_max_range_store(struct device *dev,
2404                                         struct device_attribute *attr,
2405                                         const char *buf, size_t count)
2406 {
2407         struct inv_gyro_state_s *inf;
2408         unsigned char fsr;
2409         int err;
2410
2411         inf = dev_get_drvdata(dev);
2412         err = kstrtou8(buf, 10, &fsr);
2413         if (err)
2414                 return -EINVAL;
2415
2416         if (fsr > 3)
2417                 return -EINVAL;
2418
2419         mutex_lock(&inf->mutex);
2420         if (fsr != inf->chip_config.accl_fsr) {
2421                 dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
2422                 if (inf->chip_config.accl_enable) {
2423                         if (inf->hw.pwr_mgmt_1 & inf->reg->cycle)
2424                                 nvi_pm(inf, NVI_PM_ON);
2425                         if (inf->chip_type == INV_MPU3050) {
2426                                 if (inf->mpu_slave != NULL) {
2427                                         inf->mpu_slave->set_fs(inf, fsr);
2428                                         err = 1;
2429                                 }
2430                         } else {
2431                                 err = nvi_accel_config_wr(inf, 0, fsr, 0);
2432                         }
2433                         if ((err > 0) && (inf->hw.fifo_en &
2434                                           inf->reg->accl_fifo_en))
2435                                 nvi_reset(inf, true, false);
2436                         nvi_pm(inf, NVI_PM_AUTO);
2437                 }
2438                 if (err >= 0)
2439                         inf->chip_config.accl_fsr = fsr;
2440         }
2441         mutex_unlock(&inf->mutex);
2442         if (err < 0) {
2443                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", __func__, fsr, err);
2444                 return err;
2445         }
2446
2447         return count;
2448 }
2449
2450 static ssize_t nvi_accl_max_range_show(struct device *dev,
2451                                        struct device_attribute *attr, char *buf)
2452 {
2453         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2454         unsigned int range;
2455
2456         if (inf->chip_config.accl_enable)
2457                 range = inf->chip_config.accl_fsr;
2458         else
2459                 range = 0x4000 >> inf->chip_config.accl_fsr;
2460         return sprintf(buf, "%u\n", range);
2461 }
2462
2463 static ssize_t nvi_lpa_delay_enable_store(struct device *dev,
2464                                           struct device_attribute *attr,
2465                                           const char *buf, size_t count)
2466 {
2467         struct inv_gyro_state_s *inf;
2468         unsigned long lpa_delay_us;
2469         int err;
2470
2471         inf = dev_get_drvdata(dev);
2472         err = kstrtoul(buf, 10, &lpa_delay_us);
2473         if (err)
2474                 return err;
2475
2476         if (lpa_delay_us > NVI_DELAY_US_MAX)
2477                 lpa_delay_us = NVI_DELAY_US_MAX;
2478         dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, lpa_delay_us);
2479         mutex_lock(&inf->mutex);
2480         inf->chip_config.lpa_delay_us = lpa_delay_us;
2481         err = nvi_pm(inf, NVI_PM_AUTO);
2482         mutex_unlock(&inf->mutex);
2483         if (err)
2484                 dev_err(&inf->i2c->dev, "%s: %lu ERR=%d\n",
2485                         __func__, lpa_delay_us, err);
2486         return count;
2487 }
2488
2489 static ssize_t nvi_lpa_delay_enable_show(struct device *dev,
2490                                          struct device_attribute *attr,
2491                                          char *buf)
2492 {
2493         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2494
2495         return sprintf(buf, "%lu\n", inf->chip_config.lpa_delay_us);
2496 }
2497
2498 static ssize_t nvi_mot_enable_store(struct device *dev,
2499                                     struct device_attribute *attr,
2500                                     const char *buf, size_t count)
2501 {
2502         struct inv_gyro_state_s *inf;
2503         unsigned char mot_enable;
2504         int err;
2505
2506         inf = dev_get_drvdata(dev);
2507         err = kstrtou8(buf, 10, &mot_enable);
2508         if (err)
2509                 return -EINVAL;
2510
2511         if (mot_enable > NVI_MOT_DBG)
2512                 return -EINVAL;
2513
2514         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_enable);
2515         inf->chip_config.mot_enable = mot_enable;
2516         if (!mot_enable) {
2517                 mutex_lock(&inf->mutex);
2518                 nvi_pm(inf, NVI_PM_ON);
2519                 err = nvi_motion_detect_enable(inf, 0);
2520                 err |= nvi_pm(inf, NVI_PM_AUTO);
2521                 mutex_unlock(&inf->mutex);
2522                 if (err) {
2523                         dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
2524                                 __func__, mot_enable, err);
2525                         return err;
2526                 }
2527         }
2528
2529         return count;
2530 }
2531
2532 static ssize_t nvi_mot_enable_show(struct device *dev,
2533                                    struct device_attribute *attr, char *buf)
2534 {
2535         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2536
2537         return sprintf(buf, "%x (0=dis 1=en 2=dbg)\n",
2538                        inf->chip_config.mot_enable);
2539 }
2540
2541 static ssize_t nvi_motion_thr_store(struct device *dev,
2542                                     struct device_attribute *attr,
2543                                     const char *buf, size_t count)
2544 {
2545         struct inv_gyro_state_s *inf;
2546         unsigned char mot_thr;
2547         int err;
2548
2549         inf = dev_get_drvdata(dev);
2550         err = kstrtou8(buf, 10, &mot_thr);
2551         if (err)
2552                 return -EINVAL;
2553
2554         mutex_lock(&inf->mutex);
2555         if (((!inf->chip_config.gyro_enable) && (!inf->aux.enable) &&
2556                                                 inf->chip_config.mot_enable)) {
2557                 dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_thr);
2558                 if (inf->chip_config.mot_enable == NVI_MOT_DBG)
2559                         pr_info("%s: %u\n", __func__, mot_thr);
2560                 nvi_pm(inf, NVI_PM_ON);
2561                 err = nvi_motion_detect_enable(inf, mot_thr);
2562                 err |= nvi_pm(inf, NVI_PM_AUTO);
2563         }
2564         mutex_unlock(&inf->mutex);
2565         if (err) {
2566                 dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
2567                         __func__, mot_thr, err);
2568                 return err;
2569         }
2570
2571         return count;
2572 }
2573
2574 static ssize_t nvi_motion_thr_show(struct device *dev,
2575                                    struct device_attribute *attr, char *buf)
2576 {
2577         struct inv_gyro_state_s *inf;
2578         unsigned char mot_thr;
2579
2580         inf = dev_get_drvdata(dev);
2581         if ((inf->hw.accl_config & 0x07) == 0x07)
2582                 mot_thr = inf->hw.mot_thr;
2583         else
2584                 mot_thr = 0;
2585         return sprintf(buf, "%u\n", mot_thr);
2586 }
2587
2588 static ssize_t nvi_motion_dur_store(struct device *dev,
2589                                     struct device_attribute *attr,
2590                                     const char *buf, size_t count)
2591 {
2592         struct inv_gyro_state_s *inf;
2593         unsigned char mot_dur;
2594         int err;
2595
2596         inf = dev_get_drvdata(dev);
2597         err = kstrtou8(buf, 10, &mot_dur);
2598         if (err)
2599                 return -EINVAL;
2600
2601         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_dur);
2602         inf->chip_config.mot_dur = mot_dur;
2603         return count;
2604 }
2605
2606 static ssize_t nvi_motion_dur_show(struct device *dev,
2607                                    struct device_attribute *attr, char *buf)
2608 {
2609         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2610
2611         return sprintf(buf, "%u\n", inf->chip_config.mot_dur);
2612 }
2613
2614 static ssize_t nvi_motion_ctrl_store(struct device *dev,
2615                                      struct device_attribute *attr,
2616                                      const char *buf, size_t count)
2617 {
2618         struct inv_gyro_state_s *inf;
2619         unsigned char mot_ctrl;
2620         int err;
2621
2622         inf = dev_get_drvdata(dev);
2623         err = kstrtou8(buf, 10, &mot_ctrl);
2624         if (err)
2625                 return -EINVAL;
2626
2627         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_ctrl);
2628         inf->chip_config.mot_ctrl = mot_ctrl;
2629         return count;
2630 }
2631
2632 static ssize_t nvi_motion_ctrl_show(struct device *dev,
2633                                     struct device_attribute *attr, char *buf)
2634 {
2635         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2636
2637         return sprintf(buf, "%u\n", inf->chip_config.mot_ctrl);
2638 }
2639
2640 static ssize_t nvi_motion_count_store(struct device *dev,
2641                                       struct device_attribute *attr,
2642                                       const char *buf, size_t count)
2643 {
2644         struct inv_gyro_state_s *inf;
2645         unsigned int mot_cnt;
2646         int err;
2647
2648         inf = dev_get_drvdata(dev);
2649         err = kstrtouint(buf, 10, &mot_cnt);
2650         if (err)
2651                 return -EINVAL;
2652
2653         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_cnt);
2654         inf->chip_config.mot_cnt = mot_cnt;
2655         return count;
2656 }
2657
2658 static ssize_t nvi_motion_count_show(struct device *dev,
2659                                      struct device_attribute *attr, char *buf)
2660 {
2661         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2662
2663         return sprintf(buf, "%u\n", inf->chip_config.mot_cnt);
2664 }
2665
2666 static ssize_t nvi_bypass_timeout_ms_store(struct device *dev,
2667                                            struct device_attribute *attr,
2668                                            const char *buf, size_t count)
2669 {
2670         struct inv_gyro_state_s *inf;
2671         unsigned int bypass_timeout_ms;
2672         int err;
2673
2674         inf = dev_get_drvdata(dev);
2675         err = kstrtouint(buf, 10, &bypass_timeout_ms);
2676         if (err)
2677                 return -EINVAL;
2678
2679         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, bypass_timeout_ms);
2680         inf->chip_config.bypass_timeout_ms = bypass_timeout_ms;
2681         return count;
2682 }
2683
2684 static ssize_t nvi_bypass_timeout_ms_show(struct device *dev,
2685                                           struct device_attribute *attr,
2686                                           char *buf)
2687 {
2688         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2689
2690         return sprintf(buf, "%u\n", inf->chip_config.bypass_timeout_ms);
2691 }
2692
2693 static ssize_t nvi_min_delay_us_store(struct device *dev,
2694                                       struct device_attribute *attr,
2695                                       const char *buf, size_t count)
2696 {
2697         struct inv_gyro_state_s *inf;
2698         unsigned long min_delay_us;
2699         int err;
2700
2701         inf = dev_get_drvdata(dev);
2702         err = kstrtoul(buf, 10, &min_delay_us);
2703         if (err)
2704                 return -EINVAL;
2705
2706         dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, min_delay_us);
2707         inf->chip_config.min_delay_us = min_delay_us;
2708         return count;
2709 }
2710
2711 static ssize_t nvi_min_delay_us_show(struct device *dev,
2712                                      struct device_attribute *attr, char *buf)
2713 {
2714         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2715
2716         return sprintf(buf, "%lu\n", inf->chip_config.min_delay_us);
2717 }
2718
2719 static ssize_t nvi_fifo_thr_store(struct device *dev,
2720                                   struct device_attribute *attr,
2721                                   const char *buf, size_t count)
2722 {
2723         struct inv_gyro_state_s *inf;
2724         unsigned int fifo_thr;
2725         int err;
2726
2727         inf = dev_get_drvdata(dev);
2728         err = kstrtouint(buf, 10, &fifo_thr);
2729         if (err)
2730                 return -EINVAL;
2731
2732         dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, fifo_thr);
2733         mutex_lock(&inf->mutex);
2734         inf->chip_config.fifo_thr = fifo_thr;
2735         err = nvi_int_enable_wr(inf, true);
2736         mutex_unlock(&inf->mutex);
2737         if (err) {
2738                 dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
2739                         __func__, fifo_thr, err);
2740                 return err;
2741         }
2742
2743         return count;
2744 }
2745
2746 static ssize_t nvi_fifo_thr_show(struct device *dev,
2747                                  struct device_attribute *attr, char *buf)
2748 {
2749         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2750
2751         return sprintf(buf, "%u  0=batch_mode 1=disable %u=limit\n",
2752                        inf->chip_config.fifo_thr, inf->hal.fifo_size);
2753 }
2754
2755 static ssize_t nvi_enable_store(struct device *dev,
2756                                 struct device_attribute *attr,
2757                                 const char *buf, size_t count)
2758 {
2759         struct inv_gyro_state_s *inf;
2760         unsigned char enable;
2761         int err;
2762
2763         inf = dev_get_drvdata(dev);
2764         err = kstrtou8(buf, 10, &enable);
2765         if (err)
2766                 return -EINVAL;
2767
2768         mutex_lock(&inf->mutex);
2769         if (enable)
2770                 enable = 1;
2771         if (enable != inf->chip_config.enable) {
2772                 dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, enable);
2773                 inf->chip_config.enable = enable;
2774                 err = nvi_pm(inf, NVI_PM_AUTO);
2775         }
2776         mutex_unlock(&inf->mutex);
2777         if (err)
2778                 dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
2779                         __func__, enable, err);
2780         return count;
2781 }
2782
2783 static ssize_t nvi_enable_show(struct device *dev,
2784                                struct device_attribute *attr, char *buf)
2785 {
2786         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2787
2788         return sprintf(buf, "%u\n", inf->chip_config.enable);
2789 }
2790
2791 /**
2792  *  inv_raw_gyro_show() - Read gyro data directly from registers.
2793  */
2794 static ssize_t inv_raw_gyro_show(struct device *dev,
2795                                  struct device_attribute *attr, char *buf)
2796 {
2797         struct inv_gyro_state_s *st;
2798         struct inv_reg_map_s *reg;
2799         int result;
2800         unsigned char data[6];
2801
2802         st = dev_get_drvdata(dev);
2803         reg = st->reg;
2804         if (0 == st->chip_config.gyro_enable)
2805                 return -EPERM;
2806
2807         result = inv_i2c_read(st, reg->raw_gyro, 6, data);
2808         if (result) {
2809                 printk(KERN_ERR "Could not read raw registers.\n");
2810                 return result;
2811         }
2812
2813         return sprintf(buf, "%d %d %d %lld\n",
2814                 (signed short)(be16_to_cpup((short *)&data[0])),
2815                 (signed short)(be16_to_cpup((short *)&data[2])),
2816                 (signed short)(be16_to_cpup((short *)&data[4])),
2817                 nvi_ts_ns());
2818 }
2819
2820 /**
2821  *  inv_raw_accl_show() - Read accel data directly from registers.
2822  */
2823 static ssize_t inv_raw_accl_show(struct device *dev,
2824                                  struct device_attribute *attr, char *buf)
2825 {
2826         struct inv_gyro_state_s *st;
2827         struct inv_reg_map_s *reg;
2828         u8 data[6];
2829         s16 out[3];
2830         int result;
2831
2832         st = dev_get_drvdata(dev);
2833         reg = st->reg;
2834         if (0 == st->chip_config.accl_enable)
2835                 return -EPERM;
2836
2837         result = inv_i2c_read(st, reg->raw_accl, 6, data);
2838         if (result)
2839                 return result;
2840
2841         if (st->chip_type == INV_MPU3050) {
2842                 if (st->mpu_slave != NULL) {
2843                         if (0 == st->mpu_slave->get_mode(st))
2844                                 return -EINVAL;
2845
2846                         st->mpu_slave->combine_data(data, out);
2847                 } else {
2848                         memcpy(out, data, sizeof(data));
2849                 }
2850                 return sprintf(buf, "%d %d %d %lld\n",
2851                                out[0], out[1], out[2], nvi_ts_ns());
2852         }
2853
2854         return sprintf(buf, "%d %d %d %lld\n",
2855                        ((signed short)(be16_to_cpup((short *)&data[0])) *
2856                         st->chip_info.multi),
2857                        ((signed short)(be16_to_cpup((short *)&data[2])) *
2858                         st->chip_info.multi),
2859                        ((signed short)(be16_to_cpup((short *)&data[4])) *
2860                         st->chip_info.multi),
2861                        nvi_ts_ns());
2862 }
2863
2864 static ssize_t nvi_temp_enable_store(struct device *dev,
2865                                      struct device_attribute *attr,
2866                                      const char *buf, size_t count)
2867 {
2868         struct inv_gyro_state_s *inf;
2869         unsigned char enable;
2870         int err;
2871
2872         inf = dev_get_drvdata(dev);
2873         err = kstrtou8(buf, 10, &enable);
2874         if (err)
2875                 return -EINVAL;
2876
2877         if (enable)
2878                 enable = NVI_TEMP_EN;
2879         mutex_lock(&inf->mutex);
2880         if (enable != (inf->chip_config.temp_enable & NVI_TEMP_EN)) {
2881                 dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
2882                 if (enable)
2883                         inf->chip_config.temp_enable |= NVI_TEMP_EN;
2884                 else
2885                         inf->chip_config.temp_enable &= ~NVI_TEMP_EN;
2886                 err = nvi_pm(inf, NVI_PM_AUTO);
2887         }
2888         mutex_unlock(&inf->mutex);
2889         if (err) {
2890                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
2891                         __func__, enable, err);
2892                 return err;
2893         }
2894
2895         return count;
2896 }
2897
2898 static ssize_t nvi_temp_enable_show(struct device *dev,
2899                                     struct device_attribute *attr, char *buf)
2900 {
2901         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2902
2903         return sprintf(buf, "%u\n", inf->chip_config.temp_enable);
2904 }
2905
2906 static ssize_t nvi_temp_fifo_enable_store(struct device *dev,
2907                                           struct device_attribute *attr,
2908                                           const char *buf, size_t count)
2909 {
2910         struct inv_gyro_state_s *inf;
2911         unsigned char fifo_enable;
2912         unsigned char fifo_enable_old;
2913         int err;
2914
2915         inf = dev_get_drvdata(dev);
2916         err = kstrtou8(buf, 10, &fifo_enable);
2917         if (err)
2918                 return -EINVAL;
2919
2920         if (fifo_enable)
2921                 fifo_enable = 1;
2922         dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
2923         mutex_lock(&inf->mutex);
2924         if (fifo_enable != inf->chip_config.temp_fifo_enable) {
2925                 fifo_enable_old = inf->chip_config.temp_fifo_enable;
2926                 inf->chip_config.temp_fifo_enable = fifo_enable;
2927                 err = nvi_pm(inf, NVI_PM_ON);
2928                 err |= nvi_reset(inf, true, false);
2929                 err |= nvi_pm(inf, NVI_PM_AUTO);
2930                 if (err)
2931                         inf->chip_config.temp_fifo_enable = fifo_enable_old;
2932         }
2933         mutex_unlock(&inf->mutex);
2934         if (err) {
2935                 dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
2936                         __func__, fifo_enable, err);
2937                 return err;
2938         }
2939
2940         return count;
2941 }
2942
2943 static ssize_t nvi_temp_fifo_enable_show(struct device *dev,
2944                                          struct device_attribute *attr,
2945                                          char *buf)
2946 {
2947         struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
2948
2949         return sprintf(buf, "%u\n", inf->chip_config.temp_fifo_enable);
2950 }
2951
2952 /**
2953  *  inv_temp_scale_show() - Get the temperature scale factor in LSBs/degree C.
2954  */
2955 static ssize_t inv_temp_scale_show(struct device *dev,
2956                                    struct device_attribute *attr, char *buf)
2957 {
2958         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2959
2960         if (INV_MPU3050 == st->chip_type)
2961                 return sprintf(buf, "280\n");
2962         else
2963                 return sprintf(buf, "340\n");
2964 }
2965
2966 /**
2967  *  inv_temp_offset_show() - Get the temperature offset in LSBs/degree C.
2968  */
2969 static ssize_t inv_temp_offset_show(struct device *dev,
2970                                     struct device_attribute *attr, char *buf)
2971 {
2972         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
2973
2974         if (INV_MPU3050 == st->chip_type)
2975                 return sprintf(buf, "-13200\n");
2976         else
2977                 return sprintf(buf, "-521\n");
2978 }
2979
2980 static ssize_t inv_temperature_show(struct device *dev,
2981                                     struct device_attribute *attr, char *buf)
2982 {
2983         struct inv_gyro_state_s *inf;
2984         ssize_t rtn;
2985
2986         inf = dev_get_drvdata(dev);
2987         mutex_lock(&inf->mutex_temp);
2988         rtn = sprintf(buf, "%d %lld\n", inf->temp_val, inf->temp_ts);
2989         mutex_unlock(&inf->mutex_temp);
2990         return rtn;
2991 }
2992
2993 /**
2994  * inv_key_store() -  calling this function will store authenticate key
2995  */
2996 static ssize_t inv_key_store(struct device *dev,
2997                              struct device_attribute *attr,
2998                              const char *buf, size_t count)
2999 {
3000         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
3001         unsigned int result, data, out;
3002         unsigned char *p, d[4];
3003
3004         if (st->chip_config.enable)
3005                 return -EPERM;
3006
3007         result = kstrtoul(buf, 10, (long unsigned int *)&data);
3008         if (result)
3009                 return result;
3010
3011         out = cpu_to_be32p(&data);
3012         p = (unsigned char *)&out;
3013         result = mem_w(D_AUTH_IN, 4, p);
3014         if (result)
3015                 return result;
3016
3017         result = mpu_memory_read(st->sl_handle, st->i2c_addr,
3018                 D_AUTH_IN, 4, d);
3019         return count;
3020 }
3021
3022 /**
3023  *  inv_reg_dump_show() - Register dump for testing.
3024  *  TODO: Only for testing.
3025  */
3026 static ssize_t inv_reg_dump_show(struct device *dev,
3027                                  struct device_attribute *attr, char *buf)
3028 {
3029         int ii;
3030         char data;
3031         ssize_t bytes_printed = 0;
3032         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
3033
3034         for (ii = 0; ii < st->hw_s->num_reg; ii++) {
3035                 /* don't read fifo r/w register */
3036                 if (ii == st->reg->fifo_r_w)
3037                         data = 0;
3038                 else
3039                         inv_i2c_read(st, ii, 1, &data);
3040                 bytes_printed += sprintf(buf + bytes_printed, "%#2x: %#2x\n",
3041                                          ii, data);
3042         }
3043         return bytes_printed;
3044 }
3045
3046 /**
3047  * inv_gyro_orientation_show() - show orientation matrix
3048  */
3049 static ssize_t inv_gyro_orientation_show(struct device *dev,
3050                                          struct device_attribute *attr,
3051                                          char *buf)
3052 {
3053         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
3054         signed char *m;
3055
3056         m = st->plat_data.orientation;
3057         return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
3058                        m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
3059 }
3060
3061 /**
3062  * inv_accl_matrix_show() - show orientation matrix
3063  */
3064 static ssize_t inv_accl_matrix_show(struct device *dev,
3065                              struct device_attribute *attr, char *buf)
3066 {
3067         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
3068         signed char *m;
3069
3070         if (st->plat_data.sec_slave_type == SECONDARY_SLAVE_TYPE_ACCEL)
3071                 m = st->plat_data.secondary_orientation;
3072         else
3073                 m = st->plat_data.orientation;
3074         return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
3075                        m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
3076 }
3077
3078 /**
3079  * inv_self_test_show() - self test result. 0 for fail; 1 for success.
3080  *                        calling this function will trigger self test
3081  *                        and return test result.
3082  */
3083 static ssize_t inv_self_test_show(struct device *dev,
3084                                   struct device_attribute *attr, char *buf)
3085 {
3086         int result;
3087         int bias[3];
3088         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
3089
3090         if (INV_MPU3050 == st->chip_type) {
3091                 bias[0] = bias[1] = bias[2] = 0;
3092                 result = 0;
3093         } else {
3094                 mutex_lock(&st->mutex);
3095                 result = inv_hw_self_test(st, bias);
3096                 mutex_unlock(&st->mutex);
3097         }
3098         return sprintf(buf, "%d, %d, %d, %d\n",
3099                 bias[0], bias[1], bias[2], result);
3100 }
3101
3102 /**
3103  * inv_get_accl_bias_show() - show accl bias value
3104  */
3105 static ssize_t inv_get_accl_bias_show(struct device *dev,
3106                                       struct device_attribute *attr, char *buf)
3107 {
3108         int result;
3109         int bias[3];
3110         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
3111
3112         result = inv_get_accl_bias(st, bias);
3113         if (result)
3114                 return -EINVAL;
3115
3116         return sprintf(buf, "%d, %d, %d\n", bias[0], bias[1], bias[2]);
3117 }
3118
3119 /**
3120  * inv_key_show() -  calling this function will show the key
3121  *
3122  */
3123 static ssize_t inv_key_show(struct device *dev, struct device_attribute *attr,
3124                             char *buf)
3125 {
3126         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
3127         unsigned char *key;
3128         key = st->plat_data.key;
3129
3130         return sprintf(buf,
3131         "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
3132                 key[0], key[1], key[2], key[3], key[4], key[5], key[6],
3133                 key[7], key[8], key[9], key[10], key[11], key[12],
3134                 key[13], key[14], key[15]);
3135 }
3136
3137 /**
3138  *  OBSOLETE
3139  */
3140 static ssize_t inv_power_state_store(struct device *dev,
3141                                      struct device_attribute *attr,
3142                                      const char *buf, size_t count)
3143 {
3144         struct inv_gyro_state_s *st;
3145         unsigned long power_state;
3146
3147         st = dev_get_drvdata(dev);
3148         if (kstrtoul(buf, 10, &power_state))
3149                 return -EINVAL;
3150
3151         if (power_state)
3152                 st->chip_config.is_asleep = 0;
3153         else
3154                 st->chip_config.is_asleep = 1;
3155         return count;
3156 }
3157
3158 /**
3159  *  OBSOLETE
3160  */
3161 static ssize_t inv_power_state_show(struct device *dev,
3162                                     struct device_attribute *attr, char *buf)
3163 {
3164         struct inv_gyro_state_s *st = dev_get_drvdata(dev);
3165
3166         if (st->chip_config.is_asleep)
3167                 return sprintf(buf, "0\n");
3168
3169         else
3170                 return sprintf(buf, "1\n");
3171 }
3172
3173 static u16 nvi_report_accl(struct inv_gyro_state_s *inf, u8 *data, s64 ts)
3174 {
3175         s16 val[3];
3176         s16 raw[3];
3177         u16 buf_i;
3178         unsigned int i;
3179
3180         if (inf->chip_type == INV_MPU3050) {
3181                 if (inf->mpu_slave != NULL) {
3182                         inf->mpu_slave->combine_data(data, raw);
3183                         for (i = 0; i < 3; i++)
3184                                 val[i] = raw[i];
3185                 } else {
3186                         return 0;
3187                 }
3188         } else {
3189                 raw[AXIS_X] = be16_to_cpup((__be16 *)&data[0]);
3190                 raw[AXIS_Y] = be16_to_cpup((__be16 *)&data[2]);
3191                 raw[AXIS_Z] = be16_to_cpup((__be16 *)&data[4]);
3192                 val[AXIS_X] = raw[AXIS_X] * inf->chip_info.multi;
3193                 val[AXIS_Y] = raw[AXIS_Y] * inf->chip_info.multi;
3194                 val[AXIS_Z] = raw[AXIS_Z] * inf->chip_info.multi;
3195         }
3196
3197         buf_i = 0;
3198         if (!(inf->hw.pwr_mgmt_2 & BIT_STBY_XA)) {
3199                 input_report_rel(inf->idev, REL_RX, val[AXIS_X]);
3200                 buf_i += 2;
3201         } else {
3202                 raw[AXIS_X] = 0;
3203                 val[AXIS_X] = 0;
3204         }
3205         if (!(inf->hw.pwr_mgmt_2 & BIT_STBY_YA)) {
3206                 input_report_rel(inf->idev, REL_RY, val[AXIS_Y]);
3207                 buf_i += 2;
3208         } else {
3209                 raw[AXIS_Y] = 0;
3210                 val[AXIS_Y] = 0;
3211         }
3212         if (!(inf->hw.pwr_mgmt_2 & BIT_STBY_ZA)) {
3213                 input_report_rel(inf->idev, REL_RZ, val[AXIS_Z]);
3214                 buf_i += 2;
3215         } else {
3216                 raw[AXIS_Z] = 0;
3217                 val[AXIS_Z] = 0;
3218         }
3219         for (i = 0; i < 3; i++) {
3220                 inf->accl_raw[i] = raw[i];
3221                 inf->accl[i] = val[i];
3222         }
3223         if (inf->dbg & NVI_DBG_SPEW_ACCL_RAW)
3224                 dev_info(&inf->i2c->dev, "ACCL_RAW %d %d %d %lld\n",
3225                          raw[AXIS_X], raw[AXIS_Y], raw[AXIS_Z], ts);
3226         if (inf->dbg & NVI_DBG_SPEW_ACCL)
3227                 dev_info(&inf->i2c->dev, "ACCL %d %d %d %lld\n",
3228                          val[AXIS_X], val[AXIS_Y], val[AXIS_Z], ts);
3229         return buf_i;
3230 }
3231
3232 void nvi_report_temp(struct inv_gyro_state_s *inf, u8 *data, s64 ts)
3233 {
3234         mutex_lock(&inf->mutex_temp);
3235         inf->temp_val = be16_to_cpup((__be16 *)data);
3236         inf->temp_ts = ts;
3237         if (inf->dbg & NVI_DBG_SPEW_TEMP)
3238                 dev_info(&inf->i2c->dev, "TEMP %d %lld\n", inf->temp_val, ts);
3239         mutex_unlock(&inf->mutex_temp);
3240 }
3241
3242 static u16 nvi_report_gyro(struct inv_gyro_state_s *inf,
3243                            u8 *data, u8 mask, s64 ts)
3244 {
3245         s16 val[3];
3246         u16 buf_i;
3247         bool report;
3248         unsigned int i;
3249
3250         if (ts < inf->gyro_start_ts)
3251                 report = false;
3252         else
3253                 report = true;
3254         for (i = 0; i < 3; i++)
3255                 val[i] = 0;
3256         buf_i = 0;
3257         if (mask & 4) {
3258                 if (report && (!(inf->hw.pwr_mgmt_2 & BIT_STBY_XG))) {
3259                         val[AXIS_X] = be16_to_cpup((__be16 *)&data[buf_i]);
3260                         input_report_rel(inf->idev, REL_X, val[AXIS_X]);
3261                 }
3262                 buf_i += 2;
3263         }
3264         if (mask & 2) {
3265                 if (report && (!(inf->hw.pwr_mgmt_2 & BIT_STBY_YG))) {
3266                         val[AXIS_Y] = be16_to_cpup((__be16 *)&data[buf_i]);
3267                         input_report_rel(inf->idev, REL_Y, val[AXIS_Y]);
3268                 }
3269                 buf_i += 2;
3270         }
3271         if (mask & 1) {
3272                 if (report && (!(inf->hw.pwr_mgmt_2 & BIT_STBY_ZG))) {
3273                         val[AXIS_Z] = be16_to_cpup((__be16 *)&data[buf_i]);
3274                         input_report_rel(inf->idev, REL_Z, val[AXIS_Z]);
3275                 }
3276                 buf_i += 2;
3277         }
3278         for (i = 0; i < 3; i++)
3279                 inf->gyro[i] = val[i];
3280         if (inf->dbg & NVI_DBG_SPEW_GYRO)
3281                 dev_info(&inf->i2c->dev, "GYRO %d %d %d %lld\n",
3282                          val[AXIS_X], val[AXIS_Y], val[AXIS_Z], ts);
3283         return buf_i;
3284 }
3285
3286 static void nvi_sync(struct inv_gyro_state_s *inf, s64 ts)
3287 {
3288         input_report_rel(inf->idev, REL_MISC, (unsigned int)(ts >> 32));
3289         input_report_rel(inf->idev, REL_WHEEL,
3290                          (unsigned int)(ts & 0xffffffff));
3291         input_sync(inf->idev);
3292 }
3293
3294 static int nvi_accl_read(struct inv_gyro_state_s *inf, s64 ts)
3295 {
3296         u8 data[6];
3297         int err;
3298
3299         err = inv_i2c_read(inf, inf->reg->raw_accl, 6, data);
3300         if (!err)
3301                 err = nvi_report_accl(inf, data, ts);
3302         return err;
3303 }
3304
3305 static u16 nvi_fifo_read_accl(struct inv_gyro_state_s *inf,
3306                               u16 buf_index, s64 ts)
3307 {
3308         if (inf->hw.fifo_en & inf->reg->accl_fifo_en) {
3309                 nvi_report_accl(inf, &inf->buf[buf_index], ts);
3310                 buf_index += 6;
3311         }
3312         return buf_index;
3313 }
3314
3315 static u16 nvi_fifo_read_gyro(struct inv_gyro_state_s *inf,
3316                               u16 buf_index, s64 ts)
3317 {
3318         u8 mask;
3319
3320         if (inf->hw.fifo_en & BIT_TEMP_FIFO_EN) {
3321                 nvi_report_temp(inf, &inf->buf[buf_index], ts);
3322                 buf_index += 2;
3323         }
3324         mask = inf->hw.fifo_en;
3325         mask &= (BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
3326         mask >>= 4;
3327         if (mask) {
3328                 buf_index += nvi_report_gyro(inf, &inf->buf[buf_index],
3329                                              mask, ts);
3330         }
3331         return buf_index;
3332 }
3333
3334 static irqreturn_t nvi_irq_thread(int irq, void *dev_id)
3335 {
3336         struct inv_gyro_state_s *inf;
3337         struct aux_port *ap;
3338         u8 mask;
3339         u16 fifo_count = 0;
3340         u16 fifo_sample_size;
3341         u16 fifo_rd_n;
3342         u16 fifo_align;
3343         u16 buf_index;
3344         s64 ts;
3345         s64 ts_irq;
3346         s64 delay;
3347         bool sync;
3348         unsigned int ts_len;
3349         unsigned int samples;
3350         unsigned int copied;
3351         unsigned int len;
3352         int i;
3353         int err;
3354
3355         inf = (struct inv_gyro_state_s *)dev_id;
3356         /* if only accelermeter data */
3357         if ((inf->hw.pwr_mgmt_1 & inf->reg->cycle) || (inf->hw.int_enable &
3358                                                        BIT_MOT_EN)) {
3359                 if (inf->hw.int_enable & BIT_MOT_EN) {
3360                         nvi_pm(inf, NVI_PM_ON);
3361                         nvi_motion_detect_enable(inf, 0);
3362                         nvi_int_enable_wr(inf, true);
3363                         nvi_pm(inf, NVI_PM_AUTO);
3364                         if (inf->chip_config.mot_enable == NVI_MOT_DBG)
3365                                 pr_info("%s motion detect off\n", __func__);
3366                 }
3367                 ts = nvi_ts_ns();
3368                 err = nvi_accl_read(inf, ts);
3369                 if (err < 0)
3370                         goto nvi_irq_thread_exit_clear;
3371
3372                 nvi_sync(inf, ts);
3373                 goto nvi_irq_thread_exit;
3374         }
3375
3376         /* handle FIFO disabled data */
3377         sync = false;
3378         ts = nvi_ts_ns();
3379         if (((~inf->hw.pwr_mgmt_2) & BIT_PWR_ACCL_STBY) &&
3380                                (!(inf->hw.fifo_en & inf->reg->accl_fifo_en))) {
3381                 err = nvi_accl_read(inf, ts);
3382                 if (err > 0)
3383                         sync = true;
3384         }
3385         if ((!(inf->hw.pwr_mgmt_1 & inf->reg->temp_dis)) &&
3386                                      (!(inf->hw.fifo_en & BIT_TEMP_FIFO_EN))) {
3387                 err = inv_i2c_read(inf, inf->reg->temperature, 2, inf->buf);
3388                 if (!err)
3389                         nvi_report_temp(inf, inf->buf, ts);
3390         }
3391         mask = (BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
3392         mask &= ~inf->hw.fifo_en;
3393         mask >>= 4;
3394         if (inf->chip_config.gyro_enable && mask) {
3395                 buf_index = 0;
3396                 err = 0;
3397                 if (mask & 4) {
3398                         err = inv_i2c_read(inf, inf->reg->raw_gyro,
3399                                            2, &inf->buf[buf_index]);
3400                         buf_index = 2;
3401                 }
3402                 if (mask & 2) {
3403                         err |= inv_i2c_read(inf, inf->reg->raw_gyro + 2,
3404                                             2, &inf->buf[buf_index]);
3405                         buf_index += 2;
3406                 }
3407                 if (mask & 1)
3408                         err |= inv_i2c_read(inf, inf->reg->raw_gyro + 4,
3409                                             2, &inf->buf[buf_index]);
3410                 if (!err) {
3411                         buf_index = nvi_report_gyro(inf, inf->buf, mask, ts);
3412                         if (buf_index)
3413                                 sync = true;
3414                 }
3415         }
3416         if (sync)
3417                 nvi_sync(inf, ts);
3418         nvi_aux_read(inf);
3419         if (!(inf->hw.user_ctrl & BIT_FIFO_EN))
3420                 goto nvi_irq_thread_exit;
3421
3422         /* handle FIFO enabled data */
3423         fifo_sample_size = inf->fifo_sample_size;
3424         if (!fifo_sample_size)
3425                 goto nvi_irq_thread_exit;
3426
3427         /* must get IRQ timestamp len first for timestamp best-fit algorithm */
3428         ts_len = kfifo_len(&inf->trigger.timestamps);
3429         err = inv_i2c_read(inf, inf->reg->fifo_count_h, 2, inf->buf);
3430         if (err)
3431                 goto nvi_irq_thread_exit;
3432
3433         fifo_count = be16_to_cpup((__be16 *)(&inf->buf));
3434         /* FIFO threshold */
3435         if (inf->chip_config.fifo_thr > fifo_sample_size) {
3436                 if (fifo_count > inf->chip_config.fifo_thr) {
3437                         dev_dbg(&inf->i2c->dev, "FIFO threshold exceeded\n");
3438                         goto nvi_irq_thread_exit_reset;
3439                 }
3440         }
3441
3442         fifo_align = fifo_count % fifo_sample_size;
3443         if (fifo_count < fifo_sample_size + fifo_align)
3444                 goto nvi_irq_thread_exit;
3445
3446         if (inf->chip_type == INV_MPU3050) {
3447                 /* FIFO HW BUG WAR:
3448                  * The MPU3050 will fire an IRQ on incomplete sampling of data
3449                  * to the FIFO causing misalignment of data.  The WAR is to
3450                  * simply wait for the next IRQ.  This misalignment problem
3451                  * usually works itself out with the next data sample.
3452                  * The safety net is a FIFO reset, should the problem not work
3453                  * itself out by the time the FIFO threshold is reached.
3454                  */
3455                 if (inf->fifo_reset_3050) {
3456                         if (fifo_align)
3457                                 goto nvi_irq_thread_exit;
3458                 } else {
3459                         if (fifo_align != 2)
3460                                 goto nvi_irq_thread_exit;
3461                 }
3462         }
3463
3464         ts = inf->fifo_ts;
3465         delay = inf->sample_delay_us * 1000;
3466         samples = (fifo_count / fifo_sample_size);
3467         if (inf->dbg & NVI_DBG_SPEW_FIFO)
3468                 dev_info(&inf->i2c->dev,
3469                          "fifo_count=%u sample_size=%u offset=%u samples=%u\n",
3470                          fifo_count, fifo_sample_size, fifo_align, samples);
3471         fifo_rd_n = 0;
3472         buf_index = 0;
3473         while (samples) {
3474                 if (buf_index >= fifo_rd_n) {
3475                         fifo_rd_n = sizeof(inf->buf);
3476                         fifo_rd_n -= fifo_align;
3477                         fifo_rd_n /= fifo_sample_size;
3478                         if (samples < fifo_rd_n)
3479                                 fifo_rd_n = samples;
3480                         fifo_rd_n *= fifo_sample_size;
3481                         fifo_rd_n += fifo_align;
3482                         if (inf->chip_type == INV_MPU3050)
3483                                 fifo_rd_n -= 2; /* FIFO_FOOTER */
3484                         err = inv_i2c_read(inf, inf->reg->fifo_r_w,
3485                                            fifo_rd_n, inf->buf);
3486                         if (err)
3487                                 goto nvi_irq_thread_exit;
3488
3489                         buf_index = fifo_align;
3490                         if (inf->chip_type == INV_MPU3050) {
3491                                 if (inf->fifo_reset_3050) {
3492                                         inf->fifo_reset_3050 = false;
3493                                         fifo_align += 2;
3494                                 }
3495                         }
3496                 }
3497
3498                 if (ts_len) {
3499                         len = ts_len;
3500                         for (i = 0; i < len; i++) {
3501                                 err = kfifo_out_peek(&inf->trigger.timestamps,
3502                                                      &ts_irq, 1);
3503                                 if (err != 1)
3504                                         goto nvi_irq_thread_exit_reset;
3505
3506                                 if (ts < (ts_irq - delay))
3507                                         break;
3508
3509                                 err = kfifo_to_user(&inf->trigger.timestamps,
3510                                                     &ts_irq, sizeof(ts_irq),
3511                                                     &copied);
3512                                 if (err)
3513                                         goto nvi_irq_thread_exit_reset;
3514
3515                                 ts_len--;
3516                                 if (ts < (ts_irq + delay)) {
3517                                         ts = ts_irq;
3518                                         break;
3519                                 }
3520                         }
3521                         if ((inf->dbg & NVI_DBG_SPEW_FIFO) && (ts != ts_irq))
3522                                 dev_info(&inf->i2c->dev,
3523                                          "%s TS=%lld != IRQ=%lld s=%u i=%u\n",
3524                                         __func__, ts, ts_irq, samples, ts_len);
3525                 } else {
3526                         if (inf->dbg & NVI_DBG_SPEW_FIFO)
3527                                 dev_info(&inf->i2c->dev,
3528                                          "%s NO IRQ_TS TS=%lld s=%u\n",
3529                                          __func__, ts, samples);
3530                 }
3531                 if (inf->chip_type == INV_MPU3050) {
3532                         buf_index = nvi_fifo_read_gyro(inf, buf_index, ts);
3533                         buf_index = nvi_fifo_read_accl(inf, buf_index, ts);
3534                         buf_index += 2; /* FIFO_FOOTER */
3535                 } else {
3536                         buf_index = nvi_fifo_read_accl(inf, buf_index, ts);
3537                         buf_index = nvi_fifo_read_gyro(inf, buf_index, ts);
3538                 }
3539                 nvi_sync(inf, ts);
3540                 for (i = 0; i < AUX_PORT_SPECIAL; i++) {
3541                         ap = &inf->aux.port[i];
3542                         if (ap->fifo_en &&
3543                                       (inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN)) {
3544                                 len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
3545                                 if (ap->nmp.handler != NULL)
3546                                         ap->nmp.handler(&inf->buf[buf_index],
3547                                                         len, ts,
3548                                                         ap->nmp.ext_driver);
3549                                 buf_index += len;
3550                         }
3551                 }
3552                 ts += delay;
3553                 samples--;
3554         }
3555         if (ts_len) {
3556                 if (inf->dbg & NVI_DBG_SPEW_FIFO)
3557                         dev_info(&inf->i2c->dev, "%s SYNC TO IRQ_TS %lld\n",
3558                                  __func__, ts);
3559                 for (i = 0; i < ts_len; i++) {
3560                         err = kfifo_to_user(&inf->trigger.timestamps,
3561                                             &ts, sizeof(ts), &copied);
3562                         if (err)
3563                                 goto nvi_irq_thread_exit_reset;
3564                 }
3565         }
3566
3567         inf->fifo_ts = ts;
3568 nvi_irq_thread_exit:
3569         return IRQ_HANDLED;
3570
3571 nvi_irq_thread_exit_clear:
3572         /* Ensure HW touched to clear IRQ */
3573         inv_i2c_read(inf, inf->reg->int_status, 1, inf->buf);
3574         return IRQ_HANDLED;
3575
3576 nvi_irq_thread_exit_reset:
3577         if (inf->dbg & NVI_DBG_SPEW_FIFO)
3578                 dev_info(&inf->i2c->dev,
3579                          "%s fifo_count=%u fifo_sample_size=%u\n",
3580                          __func__, fifo_count, fifo_sample_size);
3581         nvi_reset(inf, true, false);
3582         return IRQ_HANDLED;
3583 }
3584
3585 static irqreturn_t nvi_irq_handler(int irq, void *dev_id)
3586 {
3587         struct inv_gyro_state_s *inf;
3588         long long timestamp;
3589
3590         inf = (struct inv_gyro_state_s *)dev_id;
3591         spin_lock(&inf->time_stamp_lock);
3592         if (inf->hw.user_ctrl & BIT_FIFO_EN) {
3593                 timestamp = nvi_ts_ns();
3594                 kfifo_in(&inf->trigger.timestamps, &timestamp, 1);
3595         }
3596         spin_unlock(&inf->time_stamp_lock);
3597         return IRQ_WAKE_THREAD;
3598 }
3599
3600 static ssize_t nvi_data_store(struct device *dev,
3601                               struct device_attribute *attr,
3602                               const char *buf, size_t count)
3603 {
3604         struct inv_gyro_state_s *inf;
3605         unsigned int data_info;
3606         int err;
3607
3608         inf = dev_get_drvdata(dev);
3609         err = kstrtouint(buf, 10, &data_info);
3610         if (err)
3611                 return -EINVAL;
3612
3613         if (data_info >= NVI_DATA_INFO_LIMIT_MAX)
3614                 return -EINVAL;
3615
3616         dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, data_info);
3617         inf->data_info = data_info;
3618         switch (data_info) {
3619         case NVI_DATA_INFO_DATA:
3620                 inf->dbg = 0;
3621                 break;
3622
3623         case NVI_DATA_INFO_DBG:
3624                 inf->dbg ^= NVI_DBG_SPEW_MSG;
3625                 break;
3626
3627         case NVI_DATA_INFO_AUX_SPEW:
3628                 inf->dbg ^= NVI_DBG_SPEW_AUX;
3629                 nvi_aux_dbg(inf, "SNAPSHOT", 0);
3630                 break;
3631
3632         case NVI_DATA_INFO_GYRO_SPEW:
3633                 inf->dbg ^= NVI_DBG_SPEW_GYRO;
3634                 break;
3635
3636         case NVI_DATA_INFO_TEMP_SPEW:
3637                 inf->dbg ^= NVI_DBG_SPEW_TEMP;
3638                 break;
3639
3640         case NVI_DATA_INFO_ACCL_SPEW:
3641                 inf->dbg ^= NVI_DBG_SPEW_ACCL;
3642                 break;
3643
3644         case NVI_DATA_INFO_ACCL_RAW_SPEW:
3645                 inf->dbg ^= NVI_DBG_SPEW_ACCL_RAW;
3646                 break;
3647
3648         case NVI_DATA_INFO_FIFO_SPEW:
3649                 inf->dbg ^= NVI_DBG_SPEW_FIFO;
3650                 break;
3651
3652         default:
3653                 break;
3654         }
3655
3656         return count;
3657 }
3658
3659 static ssize_t nvi_data_show(struct device *dev,
3660                              struct device_attribute *attr,
3661                              char *buf)
3662 {
3663         struct inv_gyro_state_s *inf;
3664         ssize_t t = 0;
3665         enum NVI_DATA_INFO data_info;
3666         char data;
3667         unsigned int i;
3668         int err;
3669
3670         inf = dev_get_drvdata(dev);
3671         data_info = inf->data_info;
3672         inf->data_info = NVI_DATA_INFO_DATA;
3673         switch (data_info) {
3674         case NVI_DATA_INFO_DATA:
3675                 t = sprintf(buf, "ACCL_RAW %hd %hd %hd\n",
3676                             inf->accl_raw[AXIS_X],
3677                             inf->accl_raw[AXIS_Y],
3678                             inf->accl_raw[AXIS_Z]);
3679                 t += sprintf(buf + t, "ACCL %hd %hd %hd\n",
3680                              inf->accl[AXIS_X],
3681                              inf->accl[AXIS_Y],
3682                              inf->accl[AXIS_Z]);
3683                 t += sprintf(buf + t, "GYRO %hd %hd %hd\n",
3684                              inf->gyro[AXIS_X],
3685                              inf->gyro[AXIS_Y],
3686                              inf->gyro[AXIS_Z]);
3687                 mutex_lock(&inf->mutex_temp);
3688                 t += sprintf(buf + t, "TEMP %hd %lld\n",
3689                              inf->temp_val,
3690                              inf->temp_ts);
3691                 mutex_unlock(&inf->mutex_temp);
3692                 return t;
3693
3694         case NVI_DATA_INFO_VER:
3695                 return sprintf(buf, "version=%u\n", NVI_VERSION);
3696
3697         case NVI_DATA_INFO_RESET:
3698                 err = nvi_pm(inf, NVI_PM_OFF_FORCE);
3699                 err |= nvi_pm(inf, NVI_PM_AUTO);
3700                 if (err)
3701                         return sprintf(buf, "reset ERR\n");
3702                 else
3703                         return sprintf(buf, "reset done\n");
3704
3705         case NVI_DATA_INFO_REGS:
3706                 for (i = 0; i < inf->hw_s->num_reg; i++) {
3707                         if (i == inf->reg->fifo_r_w)
3708                                 data = 0;
3709                         else
3710                                 inv_i2c_read(inf, i, 1, &data);
3711                         t += sprintf(buf + t, "%#2x=%#2x\n", i, data);
3712                 }
3713                 return t;
3714
3715         case NVI_DATA_INFO_DBG:
3716                 return sprintf(buf, "DBG spew=%x\n",
3717                                !!(inf->dbg & NVI_DBG_SPEW_MSG));
3718
3719         case NVI_DATA_INFO_AUX_SPEW:
3720                 return sprintf(buf, "AUX spew=%x\n",
3721                                !!(inf->dbg & NVI_DBG_SPEW_AUX));
3722
3723         case NVI_DATA_INFO_GYRO_SPEW:
3724                 return sprintf(buf, "GYRO spew=%x\n",
3725                                !!(inf->dbg & NVI_DBG_SPEW_GYRO));
3726
3727         case NVI_DATA_INFO_TEMP_SPEW:
3728                 return sprintf(buf, "TEMP spew=%x\n",
3729                                !!(inf->dbg & NVI_DBG_SPEW_TEMP));
3730
3731         case NVI_DATA_INFO_ACCL_SPEW:
3732                 return sprintf(buf, "ACCL spew=%x\n",
3733                                !!(inf->dbg & NVI_DBG_SPEW_ACCL));
3734
3735         case NVI_DATA_INFO_ACCL_RAW_SPEW:
3736                 return sprintf(buf, "ACCL_RAW spew=%x\n",
3737                                !!(inf->dbg & NVI_DBG_SPEW_ACCL_RAW));
3738
3739         case NVI_DATA_INFO_FIFO_SPEW:
3740                 return sprintf(buf, "FIFO spew=%x\n",
3741                                !!(inf->dbg & NVI_DBG_SPEW_FIFO));
3742
3743         default:
3744                 break;
3745         }
3746
3747         return -EINVAL;
3748 }
3749
3750 #if DEBUG_SYSFS_INTERFACE
3751 static ssize_t nvi_dbg_i2c_addr_store(struct device *dev,
3752                                       struct device_attribute *attr,
3753                                       const char *buf, size_t count)
3754 {
3755         struct inv_gyro_state_s *inf;
3756         u16 dbg_i2c_addr;
3757         int err;
3758
3759         inf = dev_get_drvdata(dev);
3760         err = kstrtou16(buf, 16, &dbg_i2c_addr);
3761         if (err)
3762                 return -EINVAL;
3763
3764         inf->dbg_i2c_addr = dbg_i2c_addr;
3765         return count;
3766 }
3767
3768 static ssize_t nvi_dbg_i2c_addr_show(struct device *dev,
3769                                      struct device_attribute *attr, char *buf)
3770 {
3771         struct inv_gyro_state_s *inf;
3772         ssize_t ret = 0;
3773         u16 dbg_i2c_addr;
3774
3775         inf = dev_get_drvdata(dev);
3776         if (inf->dbg_i2c_addr)
3777                 dbg_i2c_addr = inf->dbg_i2c_addr;
3778         else
3779                 dbg_i2c_addr = inf->i2c->addr;
3780         ret += sprintf(buf + ret, "%#2x\n", dbg_i2c_addr);
3781         return ret;
3782 }
3783
3784 static ssize_t nvi_dbg_reg_store(struct device *dev,
3785                                  struct device_attribute *attr,
3786                                  const char *buf, size_t count)
3787 {
3788         struct inv_gyro_state_s *inf;
3789         u8 dbg_reg;
3790         int err;
3791
3792         inf = dev_get_drvdata(dev);
3793         err = kstrtou8(buf, 16, &dbg_reg);
3794         if (err)
3795                 return -EINVAL;
3796
3797         inf->dbg_reg = dbg_reg;
3798         return count;
3799 }
3800
3801 static ssize_t nvi_dbg_reg_show(struct device *dev,
3802                                 struct device_attribute *attr, char *buf)
3803 {
3804         struct inv_gyro_state_s *inf;
3805         ssize_t ret = 0;
3806
3807         inf = dev_get_drvdata(dev);
3808         ret += sprintf(buf + ret, "%#2x\n", inf->dbg_reg);
3809         return ret;
3810 }
3811
3812 static ssize_t nvi_dbg_dat_store(struct device *dev,
3813                                  struct device_attribute *attr,
3814                                  const char *buf, size_t count)
3815 {
3816         struct inv_gyro_state_s *inf;
3817         u16 dbg_i2c_addr;
3818         u8 dbg_dat;
3819         int err;
3820
3821         inf = dev_get_drvdata(dev);
3822         err = kstrtou8(buf, 16, &dbg_dat);
3823         if (err)
3824                 return -EINVAL;
3825
3826         if (inf->dbg_i2c_addr)
3827                 dbg_i2c_addr = inf->dbg_i2c_addr;
3828         else
3829                 dbg_i2c_addr = inf->i2c->addr;
3830         err = inv_i2c_single_write_base(inf, dbg_i2c_addr,
3831                                         inf->dbg_reg, dbg_dat);
3832         pr_info("%s dev=%x reg=%x data=%x err=%d\n",
3833                 __func__, dbg_i2c_addr, inf->dbg_reg, dbg_dat, err);
3834         return count;
3835 }
3836
3837 static ssize_t nvi_dbg_dat_show(struct device *dev,
3838                                 struct device_attribute *attr, char *buf)
3839 {
3840         struct inv_gyro_state_s *inf;
3841         ssize_t ret = 0;
3842         u16 dbg_i2c_addr;
3843         u8 dbg_dat = 0;
3844         int err;
3845
3846         inf = dev_get_drvdata(dev);
3847         if (inf->dbg_i2c_addr)
3848                 dbg_i2c_addr = inf->dbg_i2c_addr;
3849         else
3850                 dbg_i2c_addr = inf->i2c->addr;
3851         err = inv_i2c_read_base(inf, dbg_i2c_addr, inf->dbg_reg, 1, &dbg_dat);
3852         ret += sprintf(buf + ret, "%s dev=%x reg=%x data=%x err=%d\n",
3853                        __func__, dbg_i2c_addr, inf->dbg_reg, dbg_dat, err);
3854         return ret;
3855 }
3856 #endif /* DEBUG_SYSFS_INTERFACE */
3857
3858 static DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR | S_IWGRP,
3859                    nvi_gyro_enable_show, nvi_gyro_enable_store);
3860 static DEVICE_ATTR(gyro_fifo_enable, S_IRUGO | S_IWUSR | S_IWGRP,
3861                    nvi_gyro_fifo_enable_show, nvi_gyro_fifo_enable_store);
3862 static DEVICE_ATTR(gyro_delay, S_IRUGO | S_IWUSR | S_IWGRP,
3863                    nvi_gyro_delay_show, inv_gyro_delay_store);
3864 static DEVICE_ATTR(gyro_resolution, S_IRUGO | S_IWUSR | S_IWGRP,
3865                    nvi_gyro_resolution_show, nvi_gyro_resolution_store);
3866 static DEVICE_ATTR(gyro_max_range, S_IRUGO | S_IWUSR | S_IWGRP,
3867                    nvi_gyro_max_range_show, nvi_gyro_max_range_store);
3868 static DEVICE_ATTR(gyro_orientation, S_IRUGO,
3869                    inv_gyro_orientation_show, NULL);
3870 static DEVICE_ATTR(raw_gyro, S_IRUGO,
3871                    inv_raw_gyro_show, NULL);
3872 static DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR | S_IWGRP,
3873                    nvi_accl_enable_show, nvi_accl_enable_store);
3874 static DEVICE_ATTR(accl_fifo_enable, S_IRUGO | S_IWUSR | S_IWGRP,
3875                    nvi_accl_fifo_enable_show, nvi_accl_fifo_enable_store);
3876 static DEVICE_ATTR(accl_delay, S_IRUGO | S_IWUSR | S_IWGRP,
3877                    nvi_accl_delay_show, nvi_accl_delay_store);
3878 static DEVICE_ATTR(accl_resolution, S_IRUGO | S_IWUSR | S_IWGRP,
3879                    nvi_accl_resolution_show, nvi_accl_resolution_store);
3880 static DEVICE_ATTR(accl_max_range, S_IRUGO | S_IWUSR | S_IWGRP,
3881                    nvi_accl_max_range_show, nvi_accl_max_range_store);
3882 static DEVICE_ATTR(accl_orientation, S_IRUGO,
3883                    inv_accl_matrix_show, NULL);
3884 static DEVICE_ATTR(raw_accl, S_IRUGO,
3885                    inv_raw_accl_show, NULL);
3886 static DEVICE_ATTR(accl_bias, S_IRUGO,
3887                    inv_get_accl_bias_show, NULL);
3888 static DEVICE_ATTR(lpa_delay, S_IRUGO | S_IWUSR | S_IWGRP,
3889                    nvi_lpa_delay_enable_show, nvi_lpa_delay_enable_store);
3890 static DEVICE_ATTR(motion_enable, S_IRUGO | S_IWUSR | S_IWGRP,
3891                    nvi_mot_enable_show, nvi_mot_enable_store);
3892 static DEVICE_ATTR(motion_threshold, S_IRUGO | S_IWUSR | S_IWGRP,
3893                    nvi_motion_thr_show, nvi_motion_thr_store);
3894 static DEVICE_ATTR(motion_duration, S_IRUGO | S_IWUSR | S_IWGRP,
3895                    nvi_motion_dur_show, nvi_motion_dur_store);
3896 static DEVICE_ATTR(motion_count, S_IRUGO | S_IWUSR | S_IWGRP,
3897                    nvi_motion_count_show, nvi_motion_count_store);
3898 static DEVICE_ATTR(motion_control, S_IRUGO | S_IWUSR | S_IWGRP,
3899                    nvi_motion_ctrl_show, nvi_motion_ctrl_store);
3900 static DEVICE_ATTR(temp_enable, S_IRUGO | S_IWUSR | S_IWGRP,
3901                    nvi_temp_enable_show, nvi_temp_enable_store);
3902 static DEVICE_ATTR(temp_fifo_enable, S_IRUGO | S_IWUSR | S_IWGRP,
3903                    nvi_temp_fifo_enable_show, nvi_temp_fifo_enable_store);
3904 static DEVICE_ATTR(temp_scale, S_IRUGO,
3905                    inv_temp_scale_show, NULL);
3906 static DEVICE_ATTR(temp_offset, S_IRUGO,
3907                    inv_temp_offset_show, NULL);
3908 static DEVICE_ATTR(temperature, S_IRUGO,
3909                    inv_temperature_show, NULL);
3910 static DEVICE_ATTR(bypass_timeout_ms, S_IRUGO | S_IWUSR | S_IWGRP,
3911                    nvi_bypass_timeout_ms_show, nvi_bypass_timeout_ms_store);
3912 static DEVICE_ATTR(min_delay_us, S_IRUGO | S_IWUSR | S_IWGRP,
3913                    nvi_min_delay_us_show, nvi_min_delay_us_store);
3914 static DEVICE_ATTR(fifo_threshold, S_IRUGO | S_IWUSR | S_IWGRP,
3915                    nvi_fifo_thr_show, nvi_fifo_thr_store);
3916 static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP,
3917                    nvi_enable_show, nvi_enable_store);
3918 static DEVICE_ATTR(self_test, S_IRUGO,
3919                    inv_self_test_show, NULL);
3920 static DEVICE_ATTR(data, S_IRUGO | S_IWUSR | S_IWGRP,
3921                    nvi_data_show, nvi_data_store);
3922 static DEVICE_ATTR(dbg_reg, S_IRUGO | S_IWUSR | S_IWGRP,
3923                    nvi_dbg_reg_show, nvi_dbg_reg_store);
3924 static DEVICE_ATTR(dbg_dat, S_IRUGO | S_IWUSR | S_IWGRP,
3925                    nvi_dbg_dat_show, nvi_dbg_dat_store);
3926 static DEVICE_ATTR(dbg_i2c_addr, S_IRUGO | S_IWUSR | S_IWGRP,
3927                    nvi_dbg_i2c_addr_show, nvi_dbg_i2c_addr_store);
3928 static DEVICE_ATTR(reg_dump, S_IRUGO,
3929                    inv_reg_dump_show, NULL);
3930 static DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR | S_IWGRP,
3931                    inv_power_state_show, inv_power_state_store);
3932 static DEVICE_ATTR(key, S_IRUGO | S_IWUSR | S_IWGRP,
3933                    inv_key_show, inv_key_store);
3934
3935 static struct device_attribute *inv_attributes[] = {
3936         &dev_attr_accl_enable,
3937         &dev_attr_accl_fifo_enable,
3938         &dev_attr_accl_delay,
3939         &dev_attr_accl_max_range,
3940         &dev_attr_accl_orientation,
3941         &dev_attr_raw_accl,
3942         &dev_attr_gyro_enable,
3943         &dev_attr_gyro_fifo_enable,
3944         &dev_attr_gyro_max_range,
3945         &dev_attr_gyro_delay,
3946         &dev_attr_gyro_orientation,
3947         &dev_attr_raw_gyro,
3948         &dev_attr_temp_scale,
3949         &dev_attr_temp_offset,
3950         &dev_attr_temperature,
3951         &dev_attr_reg_dump,
3952         &dev_attr_self_test,
3953         &dev_attr_enable,
3954         &dev_attr_power_state,
3955         &dev_attr_key,
3956 #if DEBUG_SYSFS_INTERFACE
3957         &dev_attr_dbg_reg,
3958         &dev_attr_dbg_dat,
3959         &dev_attr_dbg_i2c_addr,
3960 #endif /* DEBUG_SYSFS_INTERFACE */
3961         NULL
3962 };
3963
3964 static struct device_attribute *inv_mpu6050_attributes[] = {
3965         &dev_attr_gyro_resolution,
3966         &dev_attr_temp_enable,
3967         &dev_attr_temp_fifo_enable,
3968         &dev_attr_accl_resolution,
3969         &dev_attr_accl_bias,
3970         &dev_attr_lpa_delay,
3971         &dev_attr_motion_enable,
3972         &dev_attr_motion_threshold,
3973         &dev_attr_motion_duration,
3974         &dev_attr_motion_count,
3975         &dev_attr_motion_control,
3976         &dev_attr_bypass_timeout_ms,
3977         &dev_attr_min_delay_us,
3978         &dev_attr_fifo_threshold,
3979         &dev_attr_data,
3980         NULL
3981 };
3982
3983 static void inv_input_close(struct input_dev *d)
3984 {
3985         struct inv_gyro_state_s *inf;
3986
3987         inf = input_get_drvdata(d);
3988         nvi_pm_exit(inf);
3989 }
3990
3991 /**
3992  *  inv_setup_input() - internal setup input device.
3993  *  @st:        Device driver instance.
3994  *  @**idev_in  pointer to input device
3995  *  @*client    i2c client
3996  *  @*name      name of the input device.
3997  */
3998 static int inv_setup_input(struct inv_gyro_state_s *st,
3999                            struct input_dev **idev_in,
4000                            struct i2c_client *client, unsigned char *name)
4001 {
4002         int result;
4003         struct input_dev *idev;
4004
4005         idev = input_allocate_device();
4006         if (!idev) {
4007                 result = -ENOMEM;
4008                 return result;
4009         }
4010
4011         /* Setup input device. */
4012         idev->name = name;
4013         idev->id.bustype = BUS_I2C;
4014         idev->id.product = 'S';
4015         idev->id.vendor     = ('I'<<8) | 'S';
4016         idev->id.version    = 1;
4017         idev->dev.parent = &client->dev;
4018         /* Open and close method. */
4019         if (strcmp(name, "INV_DMP"))
4020                 idev->close = inv_input_close;
4021         input_set_capability(idev, EV_REL, REL_X);
4022         input_set_capability(idev, EV_REL, REL_Y);
4023         input_set_capability(idev, EV_REL, REL_Z);
4024         input_set_capability(idev, EV_REL, REL_RX);
4025         input_set_capability(idev, EV_REL, REL_RY);
4026         input_set_capability(idev, EV_REL, REL_RZ);
4027         input_set_capability(idev, EV_REL, REL_MISC);
4028         input_set_capability(idev, EV_REL, REL_WHEEL);
4029         input_set_drvdata(idev, st);
4030         result = input_register_device(idev);
4031         if (result)
4032                 input_free_device(idev);
4033         *idev_in = idev;
4034         return result;
4035 }
4036
4037 static int inv_create_input(struct inv_gyro_state_s *st,
4038                 struct i2c_client *client){
4039         struct input_dev *idev;
4040         int result;
4041
4042         idev = NULL;
4043         result = inv_setup_input(st, &idev, client, st->hw_s->name);
4044         if (result)
4045                 return result;
4046
4047         st->idev = idev;
4048         if (INV_ITG3500 == st->chip_type)
4049                 return 0;
4050
4051         result = inv_setup_input(st, &idev, client, "INV_DMP");
4052         if (result)
4053                 input_unregister_device(st->idev);
4054         else
4055                 st->idev_dmp = idev;
4056         return 0;
4057 }
4058
4059 int create_device_attributes(struct device *dev,
4060         struct device_attribute **attrs)
4061 {
4062         int i;
4063         int err = 0;
4064
4065         for (i = 0 ; NULL != attrs[i] ; ++i) {
4066                 err = sysfs_create_file(&dev->kobj, &attrs[i]->attr);
4067                 if (0 != err)
4068                         break;
4069         }
4070         if (0 != err) {
4071                 for (; i >= 0 ; --i)
4072                         sysfs_remove_file(&dev->kobj, &attrs[i]->attr);
4073         }
4074         return err;
4075 }
4076
4077 void remove_device_attributes(struct device *dev,
4078         struct device_attribute **attrs)
4079 {
4080         int i;
4081
4082         for (i = 0 ; NULL != attrs[i] ; ++i)
4083                 device_remove_file(dev, attrs[i]);
4084 }
4085
4086 static char const *const inv_class_name = "invensense";
4087 static char const *const inv_device_name = "mpu";
4088 static dev_t const inv_device_dev_t = MKDEV(MISC_MAJOR, MISC_DYNAMIC_MINOR);
4089 static struct bin_attribute dmp_firmware = {
4090         .attr = {
4091                 .name = "dmp_firmware",
4092                 .mode = S_IRUGO | S_IWUSR
4093         },
4094         .size = 4096,
4095         .read = inv_dmp_firmware_read,
4096         .write = inv_dmp_firmware_write,
4097 };
4098
4099 static int create_sysfs_interfaces(struct inv_gyro_state_s *st)
4100 {
4101         int result;
4102         result = 0;
4103
4104         st->inv_class = class_create(THIS_MODULE, inv_class_name);
4105         if (IS_ERR(st->inv_class)) {
4106                 result = PTR_ERR(st->inv_class);
4107                 goto exit_nullify_class;
4108         }
4109
4110         st->inv_dev = device_create(st->inv_class, &st->i2c->dev,
4111                         inv_device_dev_t, st, inv_device_name);
4112         if (IS_ERR(st->inv_dev)) {
4113                 result = PTR_ERR(st->inv_dev);
4114                 goto exit_destroy_class;
4115         }
4116
4117         result = create_device_attributes(st->inv_dev, inv_attributes);
4118         if (result < 0)
4119                 goto exit_destroy_device;
4120
4121         if (INV_ITG3500 == st->chip_type)
4122                 return 0;
4123
4124         result = sysfs_create_bin_file(&st->inv_dev->kobj, &dmp_firmware);
4125         if (result < 0)
4126                 goto exit_remove_device_attributes;
4127
4128         if (INV_MPU3050 == st->chip_type)
4129                 return 0;
4130
4131         result = create_device_attributes(st->inv_dev, inv_mpu6050_attributes);
4132         if (result < 0)
4133                 goto exit_remove_bin_file;
4134
4135         return 0;
4136
4137 exit_remove_bin_file:
4138         sysfs_remove_bin_file(&st->inv_dev->kobj, &dmp_firmware);
4139 exit_remove_device_attributes:
4140         remove_device_attributes(st->inv_dev, inv_attributes);
4141 exit_destroy_device:
4142         device_destroy(st->inv_class, inv_device_dev_t);
4143 exit_destroy_class:
4144         st->inv_dev = NULL;
4145         class_destroy(st->inv_class);
4146 exit_nullify_class:
4147         st->inv_class = NULL;
4148         return result;
4149 }
4150
4151 static void remove_sysfs_interfaces(struct inv_gyro_state_s *st)
4152 {
4153         remove_device_attributes(st->inv_dev, inv_attributes);
4154         if (INV_ITG3500 != st->chip_type)
4155                 sysfs_remove_bin_file(&st->inv_dev->kobj, &dmp_firmware);
4156         if ((INV_ITG3500 != st->chip_type) && (INV_MPU3050 != st->chip_type))
4157                 remove_device_attributes(st->inv_dev, inv_mpu6050_attributes);
4158         device_destroy(st->inv_class, inv_device_dev_t);
4159         class_destroy(st->inv_class);
4160         st->inv_dev = NULL;
4161         st->inv_class = NULL;
4162 }
4163
4164 static void nvi_init_config(struct inv_gyro_state_s *inf)
4165 {
4166         inf->chip_config.bypass_timeout_ms = NVI_BYPASS_TIMEOUT_MS;
4167         inf->chip_config.min_delay_us = NVI_DELAY_US_MIN;
4168         inf->chip_config.fifo_thr = 1;
4169         inf->chip_config.temp_fifo_enable = 1;
4170         inf->chip_config.lpf = INV_FILTER_42HZ;
4171         inf->chip_config.gyro_enable = 0;
4172         inf->chip_config.gyro_fifo_enable = 0;
4173         inf->chip_config.gyro_fsr = INV_FSR_2000DPS;
4174         inf->chip_config.gyro_start_delay_ns = 100000000;
4175         inf->chip_config.accl_enable = 0;
4176         inf->chip_config.accl_fifo_enable = 0;
4177         inf->chip_config.accl_fsr = INV_FS_02G;
4178         inf->chip_config.mot_enable = NVI_MOT_DIS;
4179         inf->chip_config.mot_dur = 1;
4180         inf->chip_config.mot_ctrl = 1;
4181         inf->chip_config.mot_cnt = 10;
4182         inf->chip_config.enable = 0;
4183         inf->chip_config.prog_start_addr = DMP_START_ADDR;
4184 }
4185
4186 static int nvi_dev_init(struct inv_gyro_state_s *inf,
4187                         const struct i2c_device_id *id)
4188 {
4189         u8 dev_id;
4190         u8 val;
4191         int err = 0;
4192
4193         dev_id = 0;
4194         if (!strcmp(id->name, "itg3500")) {
4195                 inf->chip_type = INV_ITG3500;
4196         } else if (!strcmp(id->name, "mpu3050")) {
4197                 inf->chip_type = INV_MPU3050;
4198                 inv_setup_reg_mpu3050(inf->reg);
4199         } else if (!strcmp(id->name, "mpu6050")) {
4200                 inf->chip_type = INV_MPU6050;
4201                 dev_id = MPU6050_ID;
4202         } else if (!strcmp(id->name, "mpu9150")) {
4203                 inf->chip_type = INV_MPU6050;
4204                 dev_id = MPU6050_ID;
4205         } else if (!strcmp(id->name, "mpu6500")) {
4206                 inf->chip_type = INV_MPU6500;
4207                 dev_id = MPU6500_ID;
4208         } else if (!strcmp(id->name, "mpu9250")) {
4209                 inf->chip_type = INV_MPU6500;
4210                 dev_id = MPU9250_ID;
4211         } else if (!strcmp(id->name, "mpu6xxx")) {
4212                 inf->chip_type = INV_MPU6050;
4213                 dev_id = 0xFF;
4214         } else {
4215                 return -ENODEV;
4216         }
4217
4218         nvi_pm_init(inf);
4219         if (dev_id) {
4220                 err = inv_i2c_read(inf, inf->reg->who_am_i, 1, &val);
4221                 if (err) {
4222                         dev_err(&inf->i2c->dev, "%s I2C ID READ ERR\n",
4223                                 __func__);
4224                         if (dev_id == 0xFF) {
4225                                 dev_err(&inf->i2c->dev, "%s AUTO ID FAILED\n",
4226                                         __func__);
4227                                 return -EPERM;
4228                         }
4229                 } else {
4230                         if ((dev_id != 0xFF) && (dev_id != val))
4231                                 dev_err(&inf->i2c->dev, "%s %s_ID %x != %x\n",
4232                                         __func__, id->name, dev_id, val);
4233                         switch (val) {
4234                         case MPU6050_ID:
4235                                 inf->chip_type = INV_MPU6050;
4236                                 break;
4237
4238                         case MPU6500_ID:
4239                                 inf->chip_type = INV_MPU6500;
4240                                 break;
4241
4242                         case MPU9250_ID:
4243                                 inf->chip_type = INV_MPU6500;
4244                                 break;
4245
4246                         default:
4247                                 dev_err(&inf->i2c->dev, "%s ERR: NO ID %x\n",
4248                                         __func__, val);
4249                         }
4250                 }
4251         }
4252
4253         inf->hw_s = (struct inv_hw_s *)(hw_info + inf->chip_type);
4254         dev_dbg(&inf->i2c->dev, "%s: BRD_CFG=%s ID=%x USING: %s\n",
4255                 __func__, id->name, val, inf->hw_s->name);
4256         nvi_init_config(inf);
4257         switch (inf->chip_type) {
4258         case INV_ITG3500:
4259                 inf->hal.fifo_size = NVI_FIFO_SIZE_3050;
4260                 break;
4261
4262         case INV_MPU3050:
4263                 inf->hal.fifo_size = NVI_FIFO_SIZE_3050;
4264                 err = inv_init_config_mpu3050(inf);
4265                 break;
4266
4267         case INV_MPU6050:
4268                 inf->hal.fifo_size = NVI_FIFO_SIZE_6050;
4269                 inf->hal.lpa_tbl = &nvi_lpa_delay_us_tbl_6050[0];
4270                 inf->hal.lpa_tbl_n = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6050);
4271                 err = inv_get_silicon_rev_mpu6050(inf);
4272                 break;
4273
4274         case INV_MPU6500:
4275                 inf->hal.fifo_size = NVI_FIFO_SIZE_6500;
4276                 inf->hal.lpa_tbl = &nvi_lpa_delay_us_tbl_6500[0];
4277                 inf->hal.lpa_tbl_n = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6500);
4278                 err = inv_get_silicon_rev_mpu6500(inf);
4279                 break;
4280
4281         default:
4282                 err = -ENODEV;
4283                 break;
4284         }
4285
4286         nvi_pm(inf, NVI_PM_OFF);
4287         return err;
4288 }
4289
4290 static int nvi_suspend(struct device *dev)
4291 {
4292         struct inv_gyro_state_s *inf;
4293         int err;
4294
4295         inf = dev_get_drvdata(dev);
4296         inf->suspend = true;
4297         disable_irq(inf->i2c->irq);
4298         err = nvi_pm(inf, NVI_PM_OFF);
4299         if (err)
4300                 dev_err(dev, "%s ERR\n", __func__);
4301         if (inf->dbg & NVI_DBG_SPEW_MSG)
4302                 dev_info(dev, "%s done\n", __func__);
4303         return 0;
4304 }
4305
4306 static int nvi_resume(struct device *dev)
4307 {
4308         struct inv_gyro_state_s *inf;
4309
4310         inf = dev_get_drvdata(dev);
4311         inf->suspend = false;
4312         enable_irq(inf->i2c->irq);
4313         if (inf->dbg & NVI_DBG_SPEW_MSG)
4314                 dev_info(dev, "%s done\n", __func__);
4315         return 0;
4316 }
4317
4318 static const struct dev_pm_ops nvi_pm_ops = {
4319         .suspend = nvi_suspend,
4320         .resume = nvi_resume,
4321 };
4322
4323 static void nvi_shutdown(struct i2c_client *client)
4324 {
4325         struct inv_gyro_state_s *inf;
4326         int i;
4327
4328         disable_irq(client->irq);
4329         inf = i2c_get_clientdata(client);
4330         if (inf != NULL) {
4331                 for (i = 0; i < AUX_PORT_SPECIAL; i++) {
4332                         if (inf->aux.port[i].nmp.shutdown_bypass) {
4333                                 nvi_aux_bypass_enable(inf, true);
4334                                 break;
4335                         }
4336                 }
4337                 inf->shutdown = true;
4338                 if (inf->inv_dev)
4339                         remove_sysfs_interfaces(inf);
4340                 kfifo_free(&inf->trigger.timestamps);
4341                 free_irq(client->irq, inf);
4342                 if (inf->idev)
4343                         input_unregister_device(inf->idev);
4344                 if ((INV_ITG3500 != inf->chip_type) && (inf->idev_dmp))
4345                         input_unregister_device(inf->idev_dmp);
4346         }
4347 }
4348
4349 static int nvi_remove(struct i2c_client *client)
4350 {
4351         struct inv_gyro_state_s *inf;
4352
4353         nvi_shutdown(client);
4354         inf = i2c_get_clientdata(client);
4355         if (inf != NULL) {
4356                 nvi_pm_exit(inf);
4357                 kfree(inf);
4358         }
4359         dev_info(&client->dev, "%s\n", __func__);
4360         return 0;
4361 }
4362
4363 static int nvi_probe(struct i2c_client *client,
4364                      const struct i2c_device_id *id)
4365 {
4366         struct inv_gyro_state_s *st;
4367         int result;
4368
4369         pr_info("%s: Probe name %s\n", __func__, id->name);
4370         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
4371                 result = -ENODEV;
4372                 goto out_no_free;
4373         }
4374
4375         st = kzalloc(sizeof(*st), GFP_KERNEL);
4376         if (!st) {
4377                 result = -ENOMEM;
4378                 goto out_no_free;
4379         }
4380
4381         /* Make state variables available to all _show and _store functions. */
4382         i2c_set_clientdata(client, st);
4383         st->i2c = client;
4384         st->sl_handle = client->adapter;
4385         st->reg = (struct inv_reg_map_s *)&chip_reg ;
4386         st->hw_s = (struct inv_hw_s *)hw_info;
4387         st->i2c_addr = client->addr;
4388         st->plat_data =
4389                    *(struct mpu_platform_data *)dev_get_platdata(&client->dev);
4390
4391         result = nvi_dev_init(st, id);
4392         if (result)
4393                 goto out_free;
4394
4395         mutex_init(&st->mutex);
4396         mutex_init(&st->mutex_temp);
4397         INIT_KFIFO(st->trigger.timestamps);
4398         result = create_sysfs_interfaces(st);
4399         if (result)
4400                 goto out_free_kfifo;
4401
4402         if (!client->irq) {
4403                 dev_err(&client->adapter->dev, "IRQ not assigned.\n");
4404                 result = -EPERM;
4405                 goto out_close_sysfs;
4406         }
4407
4408         st->trigger.irq = client->irq;
4409         result = request_threaded_irq(client->irq,
4410                                       nvi_irq_handler, nvi_irq_thread,
4411                                       IRQF_TRIGGER_RISING | IRQF_SHARED,
4412                                       "inv_irq", st);
4413         if (result)
4414                 goto out_close_sysfs;
4415
4416         spin_lock_init(&st->time_stamp_lock);
4417         result = inv_create_input(st, client);
4418         if (result) {
4419                 free_irq(client->irq, st);
4420                 goto out_close_sysfs;
4421         }
4422
4423         inf_local = st;
4424         dev_info(&client->adapter->dev, "%s is ready to go\n", st->hw_s->name);
4425         return 0;
4426
4427 out_close_sysfs:
4428         remove_sysfs_interfaces(st);
4429 out_free_kfifo:
4430         kfifo_free(&st->trigger.timestamps);
4431 out_free:
4432         nvi_pm_exit(st);
4433         kfree(st);
4434 out_no_free:
4435         dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
4436         return -EIO;
4437 }
4438
4439 static unsigned short normal_i2c[] = { I2C_CLIENT_END };
4440
4441 /* device id table is used to identify what device can be
4442  * supported by this driver
4443  */
4444 static struct i2c_device_id nvi_mpu_id[] = {
4445         {"itg3500", INV_ITG3500},
4446         {"mpu3050", INV_MPU3050},
4447         {"mpu6050", INV_MPU6050},
4448         {"mpu9150", INV_MPU9150},
4449         {"mpu6500", INV_MPU6500},
4450         {"mpu9250", INV_MPU9250},
4451         {"mpu6xxx", INV_MPU6XXX},
4452         {}
4453 };
4454
4455 MODULE_DEVICE_TABLE(i2c, nvi_mpu_id);
4456
4457 static struct i2c_driver inv_mod_driver = {
4458         .class = I2C_CLASS_HWMON,
4459         .probe          =       nvi_probe,
4460         .remove         =       nvi_remove,
4461         .id_table       =       nvi_mpu_id,
4462         .driver = {
4463                 .owner  =       THIS_MODULE,
4464                 .name   =       "inv_dev",
4465 #ifdef CONFIG_PM
4466                 .pm     =       &nvi_pm_ops,
4467 #endif
4468         },
4469         .address_list = normal_i2c,
4470         .shutdown       =       nvi_shutdown,
4471 };
4472
4473 static int __init inv_mod_init(void)
4474 {
4475         int result = i2c_add_driver(&inv_mod_driver);
4476
4477         if (result) {
4478                 pr_err("%s failed\n", __func__);
4479                 return result;
4480         }
4481
4482         return 0;
4483 }
4484
4485 static void __exit inv_mod_exit(void)
4486 {
4487         i2c_del_driver(&inv_mod_driver);
4488 }
4489
4490 module_init(inv_mod_init);
4491 module_exit(inv_mod_exit);
4492
4493 MODULE_AUTHOR("NVIDIA Corporation");
4494 MODULE_DESCRIPTION("Invensense device driver");
4495 MODULE_LICENSE("GPL");
4496 MODULE_ALIAS("inv_dev");
4497