iio: common: NVS v209 common timestamps
[linux-3.10.git] / drivers / iio / imu / nvi_mpu / nvi.c
1 /* Copyright (c) 2014-2015, NVIDIA CORPORATION.  All rights reserved.
2  *
3  * This software is licensed under the terms of the GNU General Public
4  * License version 2, as published by the Free Software Foundation, and
5  * may be copied, distributed, and modified under those terms.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10  * GNU General Public License for more details.
11  */
12
13 /* The NVS = NVidia Sensor framework */
14 /* See nvs_iio.c and nvs.h for documentation */
15
16
17 #include <linux/i2c.h>
18 #include <linux/module.h>
19 #include <linux/slab.h>
20 #include <linux/err.h>
21 #include <linux/delay.h>
22 #include <linux/interrupt.h>
23 #include <linux/regulator/consumer.h>
24 #include <linux/of.h>
25 #include <linux/nvs.h>
26 #include <linux/mpu_iio.h>
27
28 #include "nvi.h"
29
30 #define NVI_DRIVER_VERSION              (210)
31 #define NVI_NAME                        "mpu6xxx"
32 #define NVI_NAME_MPU6050                "MPU6050"
33 #define NVI_NAME_MPU6500                "MPU6500"
34 #define NVI_NAME_MPU9250                "MPU9250"
35 #define NVI_NAME_MPU6515                "MPU6515"
36 #define NVI_NAME_MPU9350                "MPU9350"
37 #define NVI_NAME_ICM20628               "ICM20628"
38 #define NVI_NAME_ICM20630               "ICM20630"
39 #define NVI_NAME_ICM20632               "ICM20632"
40 #define NVI_VENDOR                      "Invensense"
41
42 enum NVI_INFO {
43         NVI_INFO_VER = 0,
44         NVI_INFO_DBG,
45         NVI_INFO_AUX_SPEW,
46         NVI_INFO_FIFO_SPEW,
47         NVI_INFO_FIFO_BUF,
48         NVI_INFO_LIMIT_MAX,
49 };
50
51 /* regulator names in order of powering on */
52 static char *nvi_vregs[] = {
53         "vdd",
54         "vlogic",
55 };
56
57 static unsigned short nvi_i2c_addrs[] = {
58         0x68,
59         0x69,
60 };
61
62 static struct nvi_state *nvi_state_local;
63
64
65 static int nvi_nb_vreg(struct nvi_state *st,
66                        unsigned long event, unsigned int i);
67
68 static int nvi_nb_vreg_vdd(struct notifier_block *nb,
69                            unsigned long event, void *ignored)
70 {
71         struct nvi_state *st = container_of(nb, struct nvi_state, nb_vreg[0]);
72
73         return nvi_nb_vreg(st, event, 0);
74 }
75
76 static int nvi_nb_vreg_vlogic(struct notifier_block *nb,
77                               unsigned long event, void *ignored)
78 {
79         struct nvi_state *st = container_of(nb, struct nvi_state, nb_vreg[1]);
80
81         return nvi_nb_vreg(st, event, 1);
82 }
83
84 static int (* const nvi_nb_vreg_pf[])(struct notifier_block *nb,
85                                       unsigned long event, void *ignored) = {
86         nvi_nb_vreg_vdd,
87         nvi_nb_vreg_vlogic,
88 };
89
90 static void nvi_err(struct nvi_state *st)
91 {
92         st->errs++;
93         if (!st->errs)
94                 st->errs--;
95 }
96
97 static void nvi_mutex_lock(struct nvi_state *st)
98 {
99         unsigned int i;
100
101         if (st->nvs) {
102                 for (i = 0; i < DEV_N; i++) {
103                         if (st->nvs_st[i])
104                                 st->nvs->nvs_mutex_lock(st->nvs_st[i]);
105                 }
106         }
107 }
108
109 static void nvi_mutex_unlock(struct nvi_state *st)
110 {
111         unsigned int i;
112
113         if (st->nvs) {
114                 for (i = 0; i < DEV_N; i++) {
115                         if (st->nvs_st[i])
116                                 st->nvs->nvs_mutex_unlock(st->nvs_st[i]);
117                 }
118         }
119 }
120
121 static void nvi_disable_irq(struct nvi_state *st)
122 {
123         if (st->i2c->irq && !st->irq_dis) {
124                 disable_irq_nosync(st->i2c->irq);
125                 st->irq_dis = true;
126                 if (st->sts & NVS_STS_SPEW_MSG)
127                         dev_info(&st->i2c->dev, "%s IRQ disabled\n", __func__);
128         }
129 }
130
131 static void nvi_enable_irq(struct nvi_state *st)
132 {
133         if (st->i2c->irq && st->irq_dis) {
134                 enable_irq(st->i2c->irq);
135                 st->irq_dis = false;
136                 if (st->sts & NVS_STS_SPEW_MSG)
137                         dev_info(&st->i2c->dev, "%s IRQ enabled\n", __func__);
138         }
139 }
140
141 int nvi_i2c_write(struct nvi_state *st, u16 addr, u16 len, u8 *buf)
142 {
143         struct i2c_msg msg;
144
145         if (addr) {
146                 msg.addr = addr;
147                 msg.flags = 0;
148                 msg.len = len;
149                 msg.buf = buf;
150                 if (i2c_transfer(st->i2c->adapter, &msg, 1) != 1) {
151                         nvi_err(st);
152                         return -EIO;
153                 }
154         }
155
156         return 0;
157 }
158
159 int nvi_i2c_wr(struct nvi_state *st, u8 reg, u8 val)
160 {
161         u8 buf[2];
162
163         buf[0] = reg;
164         buf[1] = val;
165         return nvi_i2c_write(st, st->i2c_addr, sizeof(buf), buf);
166 }
167
168 /* Register REG_BANK_SEL */
169 static int nvi_wr_reg_bank_sel(struct nvi_state *st, u8 reg_bank_sel)
170 {
171         int ret = 0;
172
173         if (st->hal->part >= ICM20628) {
174                 reg_bank_sel <<= 4;
175                 if (reg_bank_sel != st->rc.reg_bank_sel) {
176                         ret = nvi_i2c_wr(st, st->hal->reg->reg_bank_sel.reg,
177                                          reg_bank_sel);
178                         if (ret) {
179                                 dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
180                                         __func__, st->rc.reg_bank_sel,
181                                         reg_bank_sel, ret);
182                         } else {
183                                 if (st->dbg & NVI_DBG_SPEW_MSG)
184                                         dev_info(&st->i2c->dev, "%s: %x->%x\n",
185                                                  __func__, st->rc.reg_bank_sel,
186                                                  reg_bank_sel);
187                                 st->rc.reg_bank_sel = reg_bank_sel;
188                         }
189                 }
190         }
191         return ret;
192 }
193
194 int nvi_i2c_read(struct nvi_state *st, u16 addr, u8 reg, u16 len, u8 *buf)
195 {
196         struct i2c_msg msg[2];
197
198         if (addr) {
199                 msg[0].addr = addr;
200                 msg[0].flags = 0;
201                 msg[0].len = 1;
202                 msg[0].buf = &reg;
203                 msg[1].addr = addr;
204                 msg[1].flags = I2C_M_RD;
205                 msg[1].len = len;
206                 msg[1].buf = buf;
207                 if (i2c_transfer(st->i2c->adapter, msg, 2) != 2) {
208                         nvi_err(st);
209                         return -EIO;
210                 }
211         }
212
213         return 0;
214 }
215
216 int nvi_i2c_rd(struct nvi_state *st, u8 bank, u8 reg, u16 len, u8 *buf)
217 {
218         int ret;
219
220         ret = nvi_wr_reg_bank_sel(st, bank);
221         if (!ret)
222                 ret = nvi_i2c_read(st, st->i2c_addr, reg, len, buf);
223         return ret;
224 }
225
226 /* Register ACCEL OFFSET */
227 static int nvi_rd_accel_offset(struct nvi_state *st)
228 {
229         u8 buf[2];
230         unsigned int i;
231         int ret;
232
233         for (i = 0; i < AXIS_N; i++) {
234                 ret = nvi_i2c_rd(st, st->hal->reg->a_offset_h[i].bank,
235                                  st->hal->reg->a_offset_h[i].reg, 2, buf);
236                 if (!ret)
237                         st->rc.accel_offset[i] = be16_to_cpup((__be16 *)buf);
238         }
239         return ret;
240 }
241
242 /* Register ACCEL OFFSET */
243 int nvi_wr_accel_offset(struct nvi_state *st, unsigned int axis, u16 offset)
244 {
245         u8 buf[3];
246         u16 offset_le;
247         int ret = 0;
248
249         if (axis >= AXIS_N)
250                 return -EINVAL;
251
252         if ((offset != st->rc.accel_offset[axis]) || st->rc_dis) {
253                 ret = nvi_wr_reg_bank_sel(st,
254                                           st->hal->reg->a_offset_h[axis].bank);
255                 if (ret)
256                         return ret;
257
258                 buf[0] = st->hal->reg->a_offset_h[axis].reg;
259                 offset_le = cpu_to_le16(offset);
260                 buf[1] = offset_le >> 8;
261                 buf[2] = offset_le & 0xFF;
262                 ret = nvi_i2c_write(st, st->i2c_addr, sizeof(buf), buf);
263                 if (ret) {
264                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
265                                 __func__, st->hal->reg->a_offset_h[axis].reg,
266                                 st->rc.accel_offset[axis], offset, ret);
267                 } else {
268                         if (st->dbg & NVI_DBG_SPEW_MSG)
269                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
270                                          __func__, st->hal->reg->
271                                          a_offset_h[axis].reg,
272                                          st->rc.accel_offset[axis], offset);
273                         st->rc.accel_offset[axis] = offset;
274                 }
275         }
276         return ret;
277 }
278
279 /* Register GYRO OFFSET */
280 static int nvi_rd_gyro_offset(struct nvi_state *st)
281 {
282         u8 buf[AXIS_N * 2];
283         unsigned int i;
284         int ret;
285
286         ret = nvi_i2c_rd(st, st->hal->reg->xg_offset_h.bank,
287                          st->hal->reg->xg_offset_h.reg, 6, buf);
288         if (!ret) {
289                 for (i = 0; i < AXIS_N; i++)
290                         st->rc.gyro_offset[i] = be16_to_cpup((__be16 *)
291                                                              (&buf[i << 1]));
292         }
293         return ret;
294 }
295
296 /* Register GYRO OFFSET */
297 int nvi_wr_gyro_offset(struct nvi_state *st, unsigned int axis, u16 offset)
298 {
299         u8 buf[3];
300         u16 offset_le;
301         int ret = 0;
302
303         if (axis >= AXIS_N)
304                 return -EINVAL;
305
306         if ((offset != st->rc.gyro_offset[axis]) || st->rc_dis) {
307                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->xg_offset_h.bank);
308                 if (ret)
309                         return ret;
310
311                 buf[0] = st->hal->reg->xg_offset_h.reg + (axis << 1);
312                 offset_le = cpu_to_le16(offset);
313                 buf[1] = offset_le >> 8;
314                 buf[2] = offset_le & 0xFF;
315                 ret = nvi_i2c_write(st, st->i2c_addr,
316                                     sizeof(buf), buf);
317                 if (ret) {
318                         dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
319                                 __func__, st->rc.gyro_offset[axis],
320                                 offset, ret);
321                 } else {
322                         if (st->dbg & NVI_DBG_SPEW_MSG)
323                                 dev_info(&st->i2c->dev, "%s: %x->%x\n",
324                                          __func__,
325                                          st->rc.gyro_offset[axis],
326                                          offset);
327                         st->rc.gyro_offset[axis] = offset;
328                 }
329         }
330         return ret;
331 }
332
333 /* Register ACCEL_SMPLRT_DIV */
334 /* Register GYRO_SMPLRT_DIV */
335 int nvi_wr_smplrt_div(struct nvi_state *st, unsigned int dev, u16 val)
336 {
337         u8 buf[3];
338         u16 len;
339         u16 val_be;
340         int ret = 0;
341
342         dev = st->hal->smplrt[dev]->dev;
343         if ((val != st->rc.smplrt_div[dev]) || st->rc_dis) {
344                 ret = nvi_wr_reg_bank_sel(st,
345                                           st->hal->reg->smplrt_div[dev].bank);
346                 if (ret)
347                         return ret;
348
349                 len = st->hal->bit->smplrt_div_n[dev] / 8;
350                 if (st->hal->bit->smplrt_div_n[dev] % 8)
351                         len++;
352                 val_be = cpu_to_be16(val);
353                 val_be >>= ((sizeof(val_be) - len) * 8);
354                 len++;
355                 buf[0] = st->hal->reg->smplrt_div[dev].reg;
356                 buf[1] = val_be & 0xFF;
357                 buf[2] = val_be >> 8;
358                 ret = nvi_i2c_write(st, st->i2c_addr, len, buf);
359                 if (ret) {
360                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
361                                 __func__, st->hal->reg->smplrt_div[dev].reg,
362                                 st->rc.smplrt_div[dev], val, ret);
363                 } else {
364                         if (st->dbg & NVI_DBG_SPEW_MSG)
365                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
366                                         __func__,
367                                          st->hal->reg->smplrt_div[dev].reg,
368                                          st->rc.smplrt_div[dev], val);
369                         st->rc.smplrt_div[dev] = val;
370                 }
371         }
372         return ret;
373 }
374
375 /* Register GYRO_CONFIG1 */
376 static int nvi_wr_gyro_config1(struct nvi_state *st, u8 val)
377 {
378         int ret = 0;
379
380         val |= st->hal->reg->gyro_config1.dflt;
381         if ((val != st->rc.gyro_config1) || st->rc_dis) {
382                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->gyro_config1.bank);
383                 if (ret)
384                         return ret;
385
386                 ret = nvi_i2c_wr(st, st->hal->reg->gyro_config1.reg, val);
387                 if (ret) {
388                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
389                                 __func__, st->hal->reg->gyro_config1.reg,
390                                 st->rc.gyro_config1, val, ret);
391                 } else {
392                         if (st->dbg & NVI_DBG_SPEW_MSG)
393                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
394                                          __func__,
395                                          st->hal->reg->gyro_config1.reg,
396                                          st->rc.gyro_config1, val);
397                         st->rc.gyro_config1 = val;
398                 }
399         }
400         return ret;
401 }
402
403 /* Register GYRO_CONFIG2 */
404 int nvi_wr_gyro_config(struct nvi_state *st, u8 test, u8 avg, u8 fsr, u8 lpf)
405 {
406         u8 val = 0;
407         int ret = 0;
408
409         if (st->hal->part < MPU6500) {
410                 if (lpf < 8)
411                         ret = nvi_wr_gyro_config1(st, lpf);
412                 if (fsr > 3)
413                         val |= st->rc.gyro_config2 & 0x18;
414                 else
415                         val |= fsr << 3;
416         } else if (st->hal->part < ICM20628) {
417                 if (lpf < 8)
418                         ret = nvi_wr_gyro_config1(st, lpf);
419                 if (test > 7)
420                         val |= st->rc.gyro_config2 & 0xE0;
421                 else
422                         val |= test << 5;
423                 if (fsr > 3)
424                         val |= st->rc.gyro_config2 & 0x18;
425                 else
426                         val |= fsr << 3;
427         } else {
428                 if (lpf > 7)
429                         val |= st->rc.gyro_config1 & 0x39;
430                 else
431                         val |= lpf << 3;
432                 if (fsr > 3)
433                         val |= st->rc.gyro_config1 & 0x06;
434                 else
435                         val |= fsr << 1;
436                 ret = nvi_wr_gyro_config1(st, val);
437                 val = 0;
438                 if (test > 7)
439                         val |= st->rc.gyro_config2 & 0x38;
440                 else
441                         val |= test << 3;
442                 if (avg > 7)
443                         val |= st->rc.gyro_config2 & 0x07;
444                 else
445                         val |= avg;
446         }
447         if (((val != st->rc.gyro_config2) || st->rc_dis) && !ret) {
448                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->gyro_config2.bank);
449                 if (ret)
450                         return ret;
451
452                 ret = nvi_i2c_wr(st, st->hal->reg->gyro_config2.reg, val);
453                 if (ret) {
454                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
455                                 __func__, st->hal->reg->gyro_config2.reg,
456                                 st->rc.gyro_config2, val, ret);
457
458                 } else {
459                         if (st->dbg & NVI_DBG_SPEW_MSG)
460                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
461                                          __func__,
462                                          st->hal->reg->gyro_config2.reg,
463                                          st->rc.gyro_config2, val);
464                         st->rc.gyro_config2 = val;
465                 }
466         }
467         return ret;
468 }
469
470 /* Register ACCEL_CONFIG2 */
471 static int nvi_wr_accel_config2(struct nvi_state *st, u8 val)
472 {
473         int ret = 0;
474
475         if ((val != st->rc.accel_config2) || st->rc_dis) {
476                 ret = nvi_wr_reg_bank_sel(st,
477                                           st->hal->reg->accel_config2.bank);
478                 if (ret)
479                         return ret;
480
481                 ret = nvi_i2c_wr(st, st->hal->reg->accel_config2.reg, val);
482                 if (ret) {
483                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
484                                 __func__, st->hal->reg->accel_config2.reg,
485                                 st->rc.accel_config2, val, ret);
486                 } else {
487                         if (st->dbg & NVI_DBG_SPEW_MSG)
488                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
489                                          __func__,
490                                          st->hal->reg->accel_config2.reg,
491                                          st->rc.accel_config2, val);
492                         st->rc.accel_config2 = val;
493                 }
494         }
495         return ret;
496 }
497
498 /* Register ACCEL_CONFIG */
499 int nvi_wr_accel_config(struct nvi_state *st, u8 test, u8 avg, u8 fsr, u8 lpf)
500 {
501         u8 val = 0;
502         int ret = 0;
503
504         if (st->hal->part < MPU6500) {
505                 if (test > 7)
506                         val |= st->rc.accel_config & 0xE0;
507                 else
508                         val |= test << 5;
509                 if (fsr > 3)
510                         val |= st->rc.accel_config & 0x18;
511                 else
512                         val |= fsr << 3;
513                 if (lpf > 7)
514                         val |= st->rc.accel_config & 0x07;
515                 else
516                         val |= lpf;
517         } else if (st->hal->part < ICM20628) {
518                 if (lpf < 8)
519                         ret = nvi_wr_accel_config2(st, BIT_FIFO_SIZE_1K | lpf);
520                 if (test > 7)
521                         val |= st->rc.accel_config & 0xE0;
522                 else
523                         val |= test << 5;
524                 if (fsr > 3)
525                         val |= st->rc.accel_config & 0x18;
526                 else
527                         val |= fsr << 3;
528         } else {
529                 if (test > 7)
530                         val |= st->rc.accel_config2 & 0x1C;
531                 else
532                         val |= test << 2;
533                 if (avg > 3)
534                         val |= st->rc.accel_config2 & 0x03;
535                 else
536                         val |= avg;
537                 ret = nvi_wr_accel_config2(st, val);
538                 val = 0;
539                 /* lpf > 8: preserve
540                  * lpf = 0: disable
541                  * lpf 1 - 8: enable lpf - 1
542                  */
543                 if (lpf) {
544                         if (lpf > 8) {
545                                 val |= st->rc.accel_config & 0x39;
546                         } else {
547                                 lpf--;
548                                 val |= lpf << 3;
549                                 val |= 0x01;
550                         }
551                 }
552                 if (fsr > 3)
553                         val |= st->rc.accel_config & 0x06;
554                 else
555                         val |= fsr << 1;
556         }
557         if (((val != st->rc.accel_config) || st->rc_dis) && !ret) {
558                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->accel_config.bank);
559                 if (ret)
560                         return ret;
561
562                 ret = nvi_i2c_wr(st, st->hal->reg->accel_config.reg, val);
563                 if (ret) {
564                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
565                                 __func__, st->hal->reg->accel_config.reg,
566                                 st->rc.accel_config, val, ret);
567                 } else {
568                         if (st->dbg & NVI_DBG_SPEW_MSG)
569                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
570                                          __func__,
571                                          st->hal->reg->accel_config.reg,
572                                          st->rc.accel_config, val);
573                         st->rc.accel_config = val;
574                 }
575         }
576         return ret;
577 }
578
579 /* Register LP_CONFIG */
580 int nvi_wr_lp_config(struct nvi_state *st, u8 val)
581 {
582         int ret = 0;
583
584         val |= st->hal->reg->lp_config.dflt;
585         if ((val != st->rc.lp_config) || st->rc_dis) {
586                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->lp_config.bank);
587                 if (ret)
588                         return ret;
589
590                 ret = nvi_i2c_wr(st, st->hal->reg->lp_config.reg, val);
591                 if (ret) {
592                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
593                                 __func__, st->hal->reg->lp_config.reg,
594                                 st->rc.lp_config, val, ret);
595                 } else {
596                         if (st->dbg & NVI_DBG_SPEW_MSG)
597                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
598                                          __func__, st->hal->reg->lp_config.reg,
599                                          st->rc.lp_config, val);
600                         st->rc.lp_config = val;
601                 }
602         }
603         return ret;
604 }
605
606 /* Register FIFO_EN */
607 int nvi_wr_fifo_en(struct nvi_state *st, u16 fifo_en)
608 {
609         u8 buf[3];
610         u16 len = 1 + (st->hal->bit->bit_fifo_en_max / 8);
611         u16 fifo_en_le = cpu_to_le16(fifo_en);
612         int ret = 0;
613
614         if ((fifo_en != st->rc.fifo_en) || st->rc_dis) {
615                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->fifo_en.bank);
616                 if (ret)
617                         return ret;
618
619                 buf[0] = st->hal->reg->fifo_en.reg;
620                 buf[1] = st->hal->reg->fifo_en.dflt & 0xFF;
621                 buf[1] |= fifo_en_le & 0xFF;
622                 buf[2] = st->hal->reg->fifo_en.dflt >> 8;
623                 buf[2] |= fifo_en_le >> 8;
624                 ret = nvi_i2c_write(st, st->i2c_addr, len, buf);
625                 if (ret) {
626                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
627                                 __func__, st->hal->reg->fifo_en.reg,
628                                 st->rc.fifo_en, fifo_en, ret);
629                 } else {
630                         if (st->dbg & NVI_DBG_SPEW_MSG)
631                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
632                                          __func__, st->hal->reg->fifo_en.reg,
633                                          st->rc.fifo_en, fifo_en);
634                         st->rc.fifo_en = fifo_en;
635                 }
636         }
637         return ret;
638 }
639
640 /* Register I2C_MST_ODR_CONFIG */
641 static int nvi_wr_i2c_mst_odr_config(struct nvi_state *st, u8 val)
642 {
643         int ret = 0;
644
645         if (st->hal->reg->i2c_mst_odr_config.bank == 0xFF)
646                 return 0;
647
648         if ((val != st->rc.i2c_mst_odr_config) || st->rc_dis) {
649                 ret = nvi_wr_reg_bank_sel(st,
650                                         st->hal->reg->i2c_mst_odr_config.bank);
651                 if (ret)
652                         return ret;
653
654                 ret = nvi_i2c_wr(st, st->hal->reg->i2c_mst_odr_config.reg,
655                                  val);
656                 if (ret) {
657                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
658                                 __func__, st->hal->reg->i2c_mst_odr_config.reg,
659                                 st->rc.i2c_mst_odr_config, val, ret);
660                 } else {
661                         if (st->dbg & NVI_DBG_SPEW_MSG)
662                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
663                                          __func__, st->hal->reg->
664                                          i2c_mst_odr_config.reg,
665                                          st->rc.i2c_mst_odr_config, val);
666                         st->rc.i2c_mst_odr_config = val;
667                 }
668         }
669         return ret;
670 }
671
672 /* Register I2C_MST_DELAY_CTRL */
673 static int nvi_wr_i2c_mst_delay_ctrl(struct nvi_state *st, u8 val)
674 {
675         int ret = 0;
676
677         if ((val != st->rc.i2c_mst_delay_ctrl) || st->rc_dis) {
678                 ret = nvi_wr_reg_bank_sel(st,
679                                         st->hal->reg->i2c_mst_delay_ctrl.bank);
680                 if (ret)
681                         return ret;
682
683                 ret = nvi_i2c_wr(st, st->hal->reg->i2c_mst_delay_ctrl.reg,
684                                  val);
685                 if (ret) {
686                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
687                                 __func__, st->hal->reg->i2c_mst_delay_ctrl.reg,
688                                 st->rc.i2c_mst_delay_ctrl, val, ret);
689                 } else {
690                         if (st->dbg & NVI_DBG_SPEW_MSG)
691                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
692                                          __func__,
693                                          st->hal->reg->i2c_mst_delay_ctrl.reg,
694                                          st->rc.i2c_mst_delay_ctrl, val);
695                         st->rc.i2c_mst_delay_ctrl = val;
696                 }
697         }
698         return ret;
699 }
700
701 /* Register I2C_SLV0_ADDR */
702 /* Register I2C_SLV1_ADDR */
703 /* Register I2C_SLV2_ADDR */
704 /* Register I2C_SLV3_ADDR */
705 /* Register I2C_SLV4_ADDR */
706 static int nvi_wr_i2c_slv_addr(struct nvi_state *st, int port, u8 val)
707 {
708         u8 reg;
709         int ret = 0;
710
711         if ((val != st->rc.i2c_slv_addr[port]) || st->rc_dis) {
712                 ret = nvi_wr_reg_bank_sel(st,
713                                           st->hal->reg->i2c_slv0_addr.bank);
714                 if (ret)
715                         return ret;
716
717                 if (st->hal->part >= ICM20628)
718                         reg = (st->hal->reg->i2c_slv0_addr.reg + (port << 2));
719                 else
720                         reg = (st->hal->reg->i2c_slv0_addr.reg + (port * 3));
721                 ret = nvi_i2c_wr(st, reg, val);
722                 if (ret) {
723                         dev_err(&st->i2c->dev, "%s[%d]:%x=%x->%x ERR=%d\n",
724                                 __func__, port, reg, st->rc.i2c_slv_addr[port],
725                                 val, ret);
726                 } else {
727                         if (st->dbg & NVI_DBG_SPEW_MSG)
728                                 dev_info(&st->i2c->dev, "%s[%d]:%x=%x->%x\n",
729                                          __func__, port, reg,
730                                          st->rc.i2c_slv_addr[port], val);
731                         st->rc.i2c_slv_addr[port] = val;
732                 }
733         }
734         return ret;
735 }
736
737 /* Register I2C_SLV0_REG */
738 /* Register I2C_SLV1_REG */
739 /* Register I2C_SLV2_REG */
740 /* Register I2C_SLV3_REG */
741 /* Register I2C_SLV4_REG */
742 static int nvi_wr_i2c_slv_reg(struct nvi_state *st, int port, u8 val)
743 {
744         u8 reg;
745         int ret = 0;
746
747         if ((val != st->rc.i2c_slv_reg[port]) || st->rc_dis) {
748                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->i2c_slv0_reg.bank);
749                 if (ret)
750                         return ret;
751
752                 if (st->hal->part >= ICM20628)
753                         reg = (st->hal->reg->i2c_slv0_reg.reg + (port << 2));
754                 else
755                         reg = (st->hal->reg->i2c_slv0_reg.reg + (port * 3));
756                 ret = nvi_i2c_wr(st, reg, val);
757                 if (ret) {
758                         dev_err(&st->i2c->dev, "%s[%d]:%x=%x->%x ERR=%d\n",
759                                 __func__, port, reg,
760                                 st->rc.i2c_slv_reg[port], val, ret);
761                 } else {
762                         if (st->dbg & NVI_DBG_SPEW_MSG)
763                                 dev_info(&st->i2c->dev, "%s[%d]:%x=%x->%x\n",
764                                          __func__, port, reg,
765                                          st->rc.i2c_slv_reg[port], val);
766                         st->rc.i2c_slv_reg[port] = val;
767                 }
768         }
769         return ret;
770 }
771
772 /* Register I2C_SLV0_CTRL */
773 /* Register I2C_SLV1_CTRL */
774 /* Register I2C_SLV2_CTRL */
775 /* Register I2C_SLV3_CTRL */
776 static int nvi_wr_i2c_slv_ctrl(struct nvi_state *st, int port, u8 val)
777 {
778         u8 reg;
779         int ret = 0;
780
781         if ((val != st->rc.i2c_slv_ctrl[port]) || st->rc_dis) {
782                 ret = nvi_wr_reg_bank_sel(st,
783                                           st->hal->reg->i2c_slv0_ctrl.bank);
784                 if (ret)
785                         return ret;
786
787                 if (st->hal->part >= ICM20628)
788                         reg = (st->hal->reg->i2c_slv0_ctrl.reg + (port << 2));
789                 else
790                         reg = (st->hal->reg->i2c_slv0_ctrl.reg + (port * 3));
791                 ret = nvi_i2c_wr(st, reg, val);
792                 if (ret) {
793                         dev_err(&st->i2c->dev, "%s[%d]:%x=%x->%x ERR=%d\n",
794                                 __func__, port, reg,
795                                 st->rc.i2c_slv_ctrl[port], val, ret);
796                 } else {
797                         if (st->dbg & NVI_DBG_SPEW_MSG)
798                                 dev_info(&st->i2c->dev, "%s[%d]:%x=%x->%x\n",
799                                          __func__, port, reg,
800                                          st->rc.i2c_slv_ctrl[port], val);
801                         st->rc.i2c_slv_ctrl[port] = val;
802                         ret = 1; /* flag change made */
803                 }
804         }
805         return ret;
806 }
807
808 /* Register I2C_SLV0_DO */
809 /* Register I2C_SLV1_DO */
810 /* Register I2C_SLV2_DO */
811 /* Register I2C_SLV3_DO */
812 /* Register I2C_SLV4_DO */
813 static int nvi_wr_i2c_slv_do(struct nvi_state *st, int port, u8 val)
814 {
815         u8 reg;
816         int ret = 0;
817
818         if ((val != st->rc.i2c_slv_do[port]) || st->rc_dis) {
819                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->i2c_slv0_do.bank);
820                 if (ret)
821                         return ret;
822
823                 if (st->hal->part >= ICM20628) {
824                         reg = (st->hal->reg->i2c_slv0_do.reg + (port << 2));
825                 } else {
826                         if (port == AUX_PORT_IO)
827                                 reg = st->hal->reg->i2c_slv4_do.reg;
828                         else
829                                 reg = st->hal->reg->i2c_slv0_do.reg + port;
830                 }
831                 ret = nvi_i2c_wr(st, reg, val);
832                 if (ret)
833                         dev_err(&st->i2c->dev, "%s[%d]:%x=%x->%x ERR=%d\n",
834                                 __func__, port, reg,
835                                 st->rc.i2c_slv_do[port], val, ret);
836                 else
837                         st->rc.i2c_slv_do[port] = val;
838         }
839         return ret;
840 }
841
842 /* Register I2C_SLV4_CTRL */
843 static int nvi_wr_i2c_slv4_ctrl(struct nvi_state *st, bool slv4_en)
844 {
845         u8 val;
846         int ret = 0;
847
848         val = st->aux.delay_hw;
849         val |= (st->aux.port[AUX_PORT_IO].nmp.ctrl & BIT_I2C_SLV_REG_DIS);
850         if (slv4_en)
851                 val |= BIT_SLV_EN;
852         if ((val != st->rc.i2c_slv_ctrl[AUX_PORT_IO]) || st->rc_dis) {
853                 ret = nvi_wr_reg_bank_sel(st,
854                                           st->hal->reg->i2c_slv4_ctrl.bank);
855                 if (ret)
856                         return ret;
857
858                 ret = nvi_i2c_wr(st, st->hal->reg->i2c_slv4_ctrl.reg, val);
859                 if (ret) {
860                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
861                                 __func__, st->hal->reg->i2c_slv4_ctrl.reg,
862                                 st->rc.i2c_slv_ctrl[AUX_PORT_IO], val, ret);
863                 } else {
864                         if (st->dbg & NVI_DBG_SPEW_MSG)
865                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
866                                          __func__,
867                                          st->hal->reg->i2c_slv4_ctrl.reg,
868                                         st->rc.i2c_slv_ctrl[AUX_PORT_IO], val);
869                         st->rc.i2c_slv_ctrl[AUX_PORT_IO] = val;
870                         ret = 1; /* flag change made */
871                 }
872         }
873         return ret;
874 }
875
876 /* Register INT_PIN_CFG */
877 static int nvi_wr_int_pin_cfg(struct nvi_state *st, u8 val)
878 {
879         int ret = 0;
880
881         if ((val != st->rc.int_pin_cfg) || st->rc_dis) {
882                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->int_pin_cfg.bank);
883                 if (ret)
884                         return ret;
885
886                 ret = nvi_i2c_wr(st, st->hal->reg->int_pin_cfg.reg, val);
887                 if (ret) {
888                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
889                                 __func__, st->hal->reg->int_pin_cfg.reg,
890                                 st->rc.int_pin_cfg, val, ret);
891                 } else {
892                         if (st->dbg & NVI_DBG_SPEW_MSG)
893                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
894                                          __func__,
895                                          st->hal->reg->int_pin_cfg.reg,
896                                          st->rc.int_pin_cfg, val);
897                         st->rc.int_pin_cfg = val;
898                 }
899         }
900         return ret;
901 }
902
903 /* Register INT_ENABLE */
904 static int nvi_wr_int_enable(struct nvi_state *st, u32 int_enable)
905 {
906         u8 buf[5];
907         u16 len = 1 + (st->hal->bit->bit_int_enable_max / 8);
908         u32 int_enable_le = cpu_to_le32(int_enable);
909         int ret = 0;
910
911         if ((int_enable != st->rc.int_enable) || st->rc_dis) {
912                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->int_enable.bank);
913                 if (ret)
914                         return ret;
915
916                 buf[0] = st->hal->reg->int_enable.reg;
917                 buf[1] = int_enable_le & 0xFF;
918                 buf[2] = int_enable_le >> 8;
919                 buf[3] = int_enable_le >> 16;
920                 buf[4] = int_enable_le >> 24;
921                 ret = nvi_i2c_write(st, st->i2c_addr, len, buf);
922                 if (ret) {
923                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
924                                 __func__, st->hal->reg->int_enable.reg,
925                                 st->rc.int_enable, int_enable, ret);
926                 } else {
927                         if (st->dbg & NVI_DBG_SPEW_MSG)
928                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
929                                         __func__, st->hal->reg->int_enable.reg,
930                                          st->rc.int_enable, int_enable);
931                         st->rc.int_enable = int_enable;
932                         if (int_enable)
933                                 nvi_enable_irq(st);
934                 }
935         }
936         return ret;
937 }
938
939 int nvi_int_able(struct nvi_state *st, bool enable)
940 {
941         u32 int_enable = 0;
942         int ret;
943
944         if (enable) {
945                 if (st->master_enable & (1 << DEV_DMP))
946                         int_enable |= 1 << st->hal->bit->dmp_int_en;
947                 else if (st->master_enable &
948                            (DEV_MPU_MASK | (1 << DEV_AUX)))
949                         int_enable |= 1 << st->hal->bit->raw_data_0_rdy_en;
950         }
951         ret = nvi_wr_int_enable(st, int_enable);
952         return ret;
953 }
954
955 /* Register USER_CTRL */
956 static int nvi_wr_user_ctrl_rc(struct nvi_state *st, u8 user_ctrl)
957 {
958         int ret = 0;
959
960         if ((user_ctrl != st->rc.user_ctrl) || st->rc_dis) {
961                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->user_ctrl.bank);
962                 if (ret)
963                         return ret;
964
965                 ret = nvi_i2c_wr(st, st->hal->reg->user_ctrl.reg, user_ctrl);
966                 if (ret) {
967                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
968                                 __func__, st->hal->reg->user_ctrl.reg,
969                                 st->rc.user_ctrl, user_ctrl, ret);
970                 } else {
971                         if (st->dbg & NVI_DBG_SPEW_MSG)
972                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
973                                          __func__, st->hal->reg->user_ctrl.reg,
974                                          st->rc.user_ctrl, user_ctrl);
975                         st->rc.user_ctrl = user_ctrl;
976                 }
977         }
978         return ret;
979 }
980
981 int nvi_user_ctrl_en(struct nvi_state *st, bool fifo_enable, bool i2c_enable)
982 {
983         u16 val = 0;
984         u16 fifo_sample_size = 0;
985         int i;
986         int ret;
987
988         st->fifo_sample_size = 0;
989         if (fifo_enable && !(st->master_enable & (1 << DEV_DMP))) {
990                 if (st->enabled[DEV_ACCEL]) {
991                         val |= 1 << st->hal->bit->accel_fifo_en;
992                         fifo_sample_size += 6;
993                 }
994                 if (st->enabled[DEV_TEMP] && st->chip_config.temp_fifo_en) {
995                         val |= 1 << st->hal->bit->temp_fifo_en;
996                         fifo_sample_size += 2;
997                 }
998                 if (st->enabled[DEV_ANGLVEL] & (1 << AXIS_X)) {
999                         val |= 1 << st->hal->bit->gyro_x_fifo_en;
1000                         fifo_sample_size += 2;
1001                 }
1002                 if (st->enabled[DEV_ANGLVEL] & (1 << AXIS_Y)) {
1003                         val |= 1 << st->hal->bit->gyro_y_fifo_en;
1004                         fifo_sample_size += 2;
1005                 }
1006                 if (st->enabled[DEV_ANGLVEL] & (1 << AXIS_Z)) {
1007                         val |= 1 << st->hal->bit->gyro_z_fifo_en;
1008                         fifo_sample_size += 2;
1009                 }
1010                 for (i = 0; i < AUX_PORT_IO; i++) {
1011                         if (st->aux.port[i].fifo_en &&
1012                                   (st->aux.port[i].nmp.addr & BIT_I2C_READ) &&
1013                                       (st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN)) {
1014                                 val |= 1 << st->hal->bit->slv_fifo_en[i];
1015                                 fifo_sample_size += st->aux.port[i].nmp.ctrl &
1016                                                     BITS_I2C_SLV_CTRL_LEN;
1017                         }
1018                 }
1019         }
1020         st->fifo_sample_size = fifo_sample_size;
1021         ret = nvi_wr_fifo_en(st, val);
1022         if (val && !ret)
1023                 val = BIT_FIFO_EN;
1024         else
1025                 val = 0;
1026         if (i2c_enable && (st->master_enable & (1 << DEV_AUX)))
1027                 val |= BIT_I2C_MST_EN;
1028         if (st->master_enable & (1 << DEV_DMP))
1029                 val |= BIT_DMP_EN;
1030         ret |= nvi_wr_user_ctrl_rc(st, val);
1031         return ret;
1032 }
1033
1034 static void nvi_flush_aux(struct nvi_state *st, int port)
1035 {
1036         struct aux_port *ap = &st->aux.port[port];
1037
1038         if (ap->nmp.handler)
1039                 ap->nmp.handler(NULL, 0, 0LL, ap->nmp.ext_driver);
1040 }
1041
1042 static void nvi_flush_push(struct nvi_state *st)
1043 {
1044         struct aux_port *ap;
1045         unsigned int i;
1046         int ret;
1047
1048         for (i = 0; i < DEV_N; i++) {
1049                 if (st->flush[i] && st->nvs_st[i]) {
1050                         ret = st->nvs->handler(st->nvs_st[i], NULL, 0LL);
1051                         if (ret >= 0)
1052                                 st->flush[i] = false;
1053                 }
1054         }
1055         for (i = 0; i < AUX_PORT_IO; i++) {
1056                 ap = &st->aux.port[i];
1057                 if (ap->flush)
1058                         nvi_flush_aux(st, i);
1059                 ap->flush = false;
1060         }
1061 }
1062
1063 /* Register USER_CTRL */
1064 int nvi_wr_user_ctrl(struct nvi_state *st, u8 user_ctrl)
1065 {
1066         bool fifo_enable = true;
1067         bool i2c_enable = true;
1068         int i;
1069         int ret;
1070         int ret_t = 0;
1071
1072         if (!(user_ctrl & BITS_USER_CTRL_RST))
1073                 return nvi_wr_user_ctrl_rc(st, user_ctrl);
1074
1075         if (user_ctrl & BIT_I2C_MST_RST)
1076                 i2c_enable = false;
1077         if (user_ctrl & BIT_SIG_COND_RST)
1078                 user_ctrl = BITS_USER_CTRL_RST;
1079         if (user_ctrl & BIT_DMP_RST)
1080                 user_ctrl |= BIT_FIFO_RST;
1081         if (user_ctrl & BIT_FIFO_RST) {
1082                 /* must make sure FIFO is off or IRQ storm will occur */
1083                 nvi_user_ctrl_en(st, false, i2c_enable);
1084                 if (st->hal->part >= ICM20628) {
1085                         ret_t = nvi_wr_reg_bank_sel(st, REG_FIFO_RST_BANK);
1086                         if (!ret_t) {
1087                                 ret_t =  nvi_i2c_wr(st, REG_FIFO_RST, 0x1F);
1088                                 ret_t |=  nvi_i2c_wr(st, REG_FIFO_RST, 0x1E);
1089                                 if (!ret_t)
1090                                         nvi_flush_push(st);
1091                         }
1092                         if (user_ctrl == BIT_FIFO_RST)
1093                                 /* then done */
1094                                 return ret_t;
1095
1096                         user_ctrl &= ~BIT_FIFO_RST;
1097                 }
1098                 fifo_enable = false;
1099         }
1100
1101         nvi_user_ctrl_en(st, fifo_enable, i2c_enable);
1102         ret_t |= nvi_wr_reg_bank_sel(st, st->hal->reg->user_ctrl.bank);
1103         if (!ret_t) {
1104                 ret_t =  nvi_i2c_wr(st, st->hal->reg->user_ctrl.reg,
1105                                     user_ctrl);
1106                 if (ret_t) {
1107                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
1108                                 __func__, st->hal->reg->user_ctrl.reg,
1109                                 st->rc.user_ctrl, user_ctrl, ret_t);
1110                 } else {
1111                         if (user_ctrl & BIT_FIFO_RST)
1112                                 nvi_flush_push(st);
1113                         if (st->dbg & NVI_DBG_SPEW_MSG)
1114                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
1115                                          __func__, st->hal->reg->user_ctrl.reg,
1116                                          st->rc.user_ctrl, user_ctrl);
1117                         for (i = 0; i < POWER_UP_TIME; i++) {
1118                                 user_ctrl = -1;
1119                                 ret = nvi_i2c_rd(st,
1120                                                  st->hal->reg->user_ctrl.bank,
1121                                                  st->hal->reg->user_ctrl.reg,
1122                                                  1, &user_ctrl);
1123                                 if (!(user_ctrl & BITS_USER_CTRL_RST))
1124                                         break;
1125
1126                                 mdelay(1);
1127                         }
1128                         ret_t |= ret;
1129                         st->rc.user_ctrl = user_ctrl;
1130                 }
1131         }
1132         return ret_t;
1133 }
1134
1135 /* Register PWR_MGMT_1 */
1136 static int nvi_wr_pwr_mgmt_1_war(struct nvi_state *st)
1137 {
1138         u8 val;
1139         int i;
1140         int ret;
1141
1142         ret = nvi_wr_reg_bank_sel(st, st->hal->reg->pwr_mgmt_1.bank);
1143         if (!ret) {
1144                 for (i = 0; i < (POWER_UP_TIME / REG_UP_TIME); i++) {
1145                         ret = nvi_i2c_wr(st, st->hal->reg->pwr_mgmt_1.reg,
1146                                          st->hal->reg->pwr_mgmt_1.dflt);
1147                         mdelay(REG_UP_TIME);
1148                         val = -1;
1149                         ret = nvi_i2c_rd(st, st->hal->reg->pwr_mgmt_1.bank,
1150                                         st->hal->reg->pwr_mgmt_1.reg, 1, &val);
1151                         if ((!ret) && (val == st->hal->reg->pwr_mgmt_1.dflt))
1152                                 break;
1153                 }
1154                 st->rc.pwr_mgmt_1 = val;
1155         }
1156         return ret;
1157 }
1158
1159 /* Register PWR_MGMT_1 */
1160 int nvi_wr_pwr_mgmt_1(struct nvi_state *st, u8 pwr_mgmt_1)
1161 {
1162         unsigned int i;
1163         int ret = 0;
1164
1165         pwr_mgmt_1 |= st->hal->reg->pwr_mgmt_1.dflt;
1166         if ((pwr_mgmt_1 != st->rc.pwr_mgmt_1) || st->rc_dis) {
1167                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->pwr_mgmt_1.bank);
1168                 if (ret)
1169                         return ret;
1170
1171                 if (pwr_mgmt_1 & BIT_H_RESET) {
1172                         nvi_wr_user_ctrl(st, BITS_USER_CTRL_RST);
1173                         ret = nvi_i2c_wr(st, st->hal->reg->pwr_mgmt_1.reg,
1174                                          BIT_H_RESET);
1175                 } else {
1176                         ret = nvi_i2c_wr(st, st->hal->reg->pwr_mgmt_1.reg,
1177                                          pwr_mgmt_1);
1178                 }
1179                 if (ret) {
1180                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
1181                                 __func__, st->hal->reg->pwr_mgmt_1.reg,
1182                                 st->rc.pwr_mgmt_1, pwr_mgmt_1, ret);
1183                 } else {
1184                         if (pwr_mgmt_1 & BIT_H_RESET) {
1185                                 memset(&st->rc, 0, sizeof(struct nvi_rc));
1186                                 if (st->hal->por2rc)
1187                                         st->hal->por2rc(st);
1188                                 for (i = 0; i < DEV_N_AUX; i++)
1189                                         st->smplrt_delay_us[i] = 0;
1190                                 for (i = 0; i < (POWER_UP_TIME / REG_UP_TIME);
1191                                                                          i++) {
1192                                         mdelay(REG_UP_TIME);
1193                                         pwr_mgmt_1 = -1;
1194                                         ret = nvi_i2c_rd(st,
1195                                                  st->hal->reg->pwr_mgmt_1.bank,
1196                                                   st->hal->reg->pwr_mgmt_1.reg,
1197                                                          1, &pwr_mgmt_1);
1198                                         if ((!ret) &&
1199                                                  (!(pwr_mgmt_1 & BIT_H_RESET)))
1200                                                 break;
1201                                 }
1202                                 nvi_rd_accel_offset(st);
1203                                 nvi_rd_gyro_offset(st);
1204                         }
1205                         if (st->dbg & NVI_DBG_SPEW_MSG)
1206                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
1207                                         __func__, st->hal->reg->pwr_mgmt_1.reg,
1208                                          st->rc.pwr_mgmt_1, pwr_mgmt_1);
1209                         st->rc.pwr_mgmt_1 = pwr_mgmt_1;
1210                 }
1211         }
1212         return ret;
1213 }
1214
1215 /* Register PWR_MGMT_2 */
1216 static int nvi_wr_pwr_mgmt_2(struct nvi_state *st, u8 pwr_mgmt_2)
1217 {
1218         int ret = 0;
1219
1220         pwr_mgmt_2 |= st->hal->reg->pwr_mgmt_2.dflt;
1221         if ((pwr_mgmt_2 != st->rc.pwr_mgmt_2) || st->rc_dis) {
1222                 ret = nvi_wr_reg_bank_sel(st, st->hal->reg->pwr_mgmt_2.bank);
1223                 if (ret)
1224                         return ret;
1225
1226                 ret = nvi_i2c_wr(st, st->hal->reg->pwr_mgmt_2.reg, pwr_mgmt_2);
1227                 if (ret) {
1228                         dev_err(&st->i2c->dev, "%s:%x=%x->%x ERR=%d\n",
1229                                 __func__, st->hal->reg->pwr_mgmt_2.reg,
1230                                 st->rc.pwr_mgmt_2, pwr_mgmt_2, ret);
1231                 } else {
1232                         if (st->dbg & NVI_DBG_SPEW_MSG)
1233                                 dev_info(&st->i2c->dev, "%s:%x=%x->%x\n",
1234                                         __func__, st->hal->reg->pwr_mgmt_2.reg,
1235                                          st->rc.pwr_mgmt_2, pwr_mgmt_2);
1236                         st->rc.pwr_mgmt_2 = pwr_mgmt_2;
1237                 }
1238         }
1239         return ret;
1240 }
1241
1242 static int nvi_nb_vreg(struct nvi_state *st,
1243                        unsigned long event, unsigned int i)
1244 {
1245         if (event & REGULATOR_EVENT_POST_ENABLE)
1246                 st->vreg_en_ts[i] = nvs_timestamp();
1247         else if (event & (REGULATOR_EVENT_DISABLE |
1248                           REGULATOR_EVENT_FORCE_DISABLE))
1249                 st->vreg_en_ts[i] = 0;
1250         if (st->dbg & NVI_DBG_SPEW_MSG)
1251                 dev_info(&st->i2c->dev, "%s %s event=0x%x ts=%lld\n",
1252                          __func__, st->vreg[i].supply, (unsigned int)event,
1253                          st->vreg_en_ts[i]);
1254         return NOTIFY_OK;
1255 }
1256
1257 int nvi_pm_wr(struct nvi_state *st, u8 pwr_mgmt_1, u8 pwr_mgmt_2, u8 lp)
1258 {
1259         s64 por_ns;
1260         bool rc_dis;
1261         unsigned int delay_ms;
1262         unsigned int i;
1263         int ret;
1264         int ret_t = 0;
1265
1266         ret = nvs_vregs_enable(&st->i2c->dev, st->vreg, ARRAY_SIZE(nvi_vregs));
1267         if (ret) {
1268                 rc_dis = st->rc_dis;
1269                 st->rc_dis = true;
1270                 delay_ms = 0;
1271                 for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++) {
1272                         por_ns = nvs_timestamp() - st->vreg_en_ts[i];
1273                         if ((por_ns < 0) || (!st->vreg_en_ts[i])) {
1274                                 delay_ms = (POR_MS * 1000000);
1275                                 break;
1276                         }
1277
1278                         if (por_ns < (POR_MS * 1000000)) {
1279                                 por_ns = (POR_MS * 1000000) - por_ns;
1280                                 if (por_ns > delay_ms)
1281                                         delay_ms = (unsigned int)por_ns;
1282                         }
1283                 }
1284                 delay_ms /= 1000000;
1285                 if (st->dbg & NVI_DBG_SPEW_MSG)
1286                         dev_info(&st->i2c->dev, "%s %ums delay\n",
1287                                  __func__, delay_ms);
1288                 if (delay_ms)
1289                         msleep(delay_ms);
1290                 ret_t |= nvi_wr_pwr_mgmt_1_war(st);
1291                 ret_t |= nvi_wr_pwr_mgmt_1(st, BIT_H_RESET);
1292                 ret_t |= nvi_wr_pwr_mgmt_1_war(st);
1293                 st->rc_dis = rc_dis;
1294         } else {
1295                 ret_t |= nvi_wr_pwr_mgmt_1_war(st);
1296         }
1297         if (st->hal->part < MPU6500) {
1298                 pwr_mgmt_2 |= lp << 6;
1299                 ret = nvi_wr_pwr_mgmt_2(st, pwr_mgmt_2);
1300                 if (ret)
1301                         ret_t |= ret;
1302                 else
1303                         st->rc.lp_config = lp;
1304                 ret_t |= nvi_wr_pwr_mgmt_1(st, pwr_mgmt_1);
1305         } else if (st->hal->part < ICM20628) {
1306                 if (pwr_mgmt_1 & BIT_CYCLE) {
1307                         ret_t |= nvi_wr_lp_config(st, lp);
1308                         ret_t |= nvi_wr_accel_config2(st, BIT_FIFO_SIZE_1K |
1309                                                       BIT_ACCEL_FCHOCIE_B);
1310                 }
1311                 ret_t |= nvi_wr_pwr_mgmt_2(st, pwr_mgmt_2);
1312                 ret_t |= nvi_wr_pwr_mgmt_1(st, pwr_mgmt_1);
1313                 if (!(pwr_mgmt_1 & BIT_CYCLE))
1314                         ret_t |= nvi_wr_accel_config2(st, BIT_FIFO_SIZE_1K);
1315         } else { /* ICM20628 */
1316                 ret_t |= nvi_wr_lp_config(st, lp);
1317                 ret_t |= nvi_wr_pwr_mgmt_2(st, pwr_mgmt_2);
1318                 ret_t |= nvi_wr_pwr_mgmt_1(st, pwr_mgmt_1);
1319         }
1320         return ret_t;
1321 }
1322
1323 static int nvi_reset(struct nvi_state *st,
1324                      bool reset_fifo, bool reset_i2c);
1325 static int nvi_aux_bypass_enable(struct nvi_state *st, bool enable);
1326
1327 /**
1328  * @param st
1329  * @param pm_req: call with one of the following:
1330  *      NVI_PM_OFF_FORCE = force off state
1331  *      NVI_PM_ON = minimum power for device access
1332  *      NVI_PM_ON_FULL = power for anglvel
1333  *      NVI_PM_AUTO = automatically sets power for configuration
1334  *      Typical use is to set needed power for configuration and
1335  *      then call with NVI_PM_AUTO when done.
1336  *      All other NVI_PM_ levels are handled automatically and
1337  *      are for internal use.
1338  * @return int: returns 0 for success or error code
1339  */
1340 int nvi_pm(struct nvi_state *st, int pm_req)
1341 {
1342         bool irq;
1343         u8 pwr_mgmt_1;
1344         u8 pwr_mgmt_2;
1345         u8 lp;
1346         int i;
1347         int pm;
1348         int ret = 0;
1349
1350         lp = st->rc.lp_config;
1351         if (pm_req == NVI_PM_AUTO) {
1352                 pwr_mgmt_2 = 0;
1353                 if (!(st->enabled[DEV_ACCEL] & (1 << AXIS_X)))
1354                         pwr_mgmt_2 |= BIT_STBY_XA;
1355                 if (!(st->enabled[DEV_ACCEL] & (1 << AXIS_Y)))
1356                         pwr_mgmt_2 |= BIT_STBY_YA;
1357                 if (!(st->enabled[DEV_ACCEL] & (1 << AXIS_Z)))
1358                         pwr_mgmt_2 |= BIT_STBY_ZA;
1359                 if (!(st->enabled[DEV_ANGLVEL] & (1 << AXIS_X)))
1360                         pwr_mgmt_2 |= BIT_STBY_XG;
1361                 if (!(st->enabled[DEV_ANGLVEL] & (1 << AXIS_Y)))
1362                         pwr_mgmt_2 |= BIT_STBY_YG;
1363                 if (!(st->enabled[DEV_ANGLVEL] & (1 << AXIS_Z)))
1364                         pwr_mgmt_2 |= BIT_STBY_ZG;
1365                 if (st->master_enable & DEV_PM_ON_FULL) {
1366                         pm = NVI_PM_ON_FULL;
1367                 } else if (st->master_enable & DEV_PM_ON) {
1368                         pm = NVI_PM_ON;
1369                 } else if ((st->master_enable & DEV_PM_LPA) == DEV_PM_LPA) {
1370                         if (st->delay_us[DEV_ACCEL] >=
1371                                                 st->chip_config.lpa_delay_us) {
1372                                 for (lp = 0; lp < st->hal->lpa_tbl_n; lp++) {
1373                                         if (st->delay_us[DEV_ACCEL] >=
1374                                                           st->hal->lpa_tbl[lp])
1375                                                 break;
1376                                 }
1377                                 pm = NVI_PM_ON_CYCLE;
1378                         } else {
1379                                 pm = NVI_PM_ON;
1380                         }
1381                 } else if (st->master_enable & (1 << DEV_ACCEL)) {
1382                         pm = NVI_PM_ON;
1383                 } else if ((st->master_enable & DEV_PM_STDBY) ||
1384                                                          st->aux.bypass_lock) {
1385                         pm = NVI_PM_STDBY;
1386                 } else {
1387                         pm = NVI_PM_OFF;
1388                 }
1389         } else {
1390                 pwr_mgmt_2 = st->rc.pwr_mgmt_2;
1391                 if ((pm_req > NVI_PM_STDBY) && (pm_req < st->pm))
1392                         pm = st->pm;
1393                 else
1394                         pm = pm_req;
1395         }
1396         if (pm == NVI_PM_OFF) {
1397                 for (i = 0; i < AUX_PORT_IO; i++) {
1398                         if (st->aux.port[i].nmp.shutdown_bypass) {
1399                                 nvi_aux_bypass_enable(st, true);
1400                                 pm = NVI_PM_STDBY;
1401                                 break;
1402                         }
1403                 }
1404                 if (st->master_enable & EN_FW)
1405                         pm = NVI_PM_STDBY;
1406         }
1407
1408         switch (pm) {
1409         case NVI_PM_OFF_FORCE:
1410         case NVI_PM_OFF:
1411                 pm = NVI_PM_OFF;
1412         case NVI_PM_STDBY:
1413                 pwr_mgmt_2 = (BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY);
1414                 pwr_mgmt_1 = BIT_SLEEP;
1415                 break;
1416
1417         case NVI_PM_ON_CYCLE:
1418                 pwr_mgmt_1 = BIT_CYCLE;
1419                 break;
1420
1421         case NVI_PM_ON:
1422                 pwr_mgmt_1 = INV_CLK_INTERNAL;
1423                 break;
1424
1425         case NVI_PM_ON_FULL:
1426                 pwr_mgmt_1 = INV_CLK_PLL;
1427                 /* anglvel must be turned on before going to PLL clock */
1428                 pwr_mgmt_2 &= ~BIT_PWR_GYRO_STBY;
1429                 break;
1430
1431         default:
1432                 dev_err(&st->i2c->dev, "%s %d=>%d ERR=EINVAL\n",
1433                         __func__, st->pm, pm);
1434                 return -EINVAL;
1435         }
1436
1437         if ((pm != st->pm) || (lp != st->rc.lp_config) ||
1438                                            (pwr_mgmt_1 != st->rc.pwr_mgmt_1) ||
1439                                             (pwr_mgmt_2 != (st->rc.pwr_mgmt_2 &
1440                                   (BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY)))) {
1441                 nvi_int_able(st, false);
1442                 st->ts_gyro = 0;
1443                 if (pm == NVI_PM_OFF) {
1444                         switch (st->pm) {
1445                         case NVI_PM_STDBY:
1446                         case NVI_PM_OFF_FORCE:
1447                         case NVI_PM_OFF:
1448                         case NVI_PM_ERR:
1449                                 break;
1450
1451                         default:
1452                                 /* disables aux before turning off */
1453                                 nvi_reset(st, true, false);
1454                                 break;
1455                         }
1456                 }
1457                 if ((!(st->rc.pwr_mgmt_1 & (BIT_SLEEP | BIT_CYCLE))) &&
1458                              (pm < NVI_PM_ON) && (st->pm > NVI_PM_ON_CYCLE)) {
1459                         /* tasks that need access before low power state */
1460                         if (pm_req == NVI_PM_AUTO)
1461                                 /* turn off FIFO and I2C */
1462                                 nvi_user_ctrl_en(st, false, false);
1463                 }
1464                 if (pm == NVI_PM_OFF) {
1465                         if (st->pm > NVI_PM_OFF) {
1466                                 ret |= nvi_wr_pwr_mgmt_1_war(st);
1467                                 ret |= nvi_wr_pwr_mgmt_1(st, BIT_H_RESET);
1468                         }
1469                         ret |= nvi_pm_wr(st, pwr_mgmt_1, pwr_mgmt_2, lp);
1470                         ret |= nvs_vregs_disable(&st->i2c->dev, st->vreg,
1471                                                  ARRAY_SIZE(nvi_vregs));
1472                 } else {
1473                         ret |= nvi_pm_wr(st, pwr_mgmt_1, pwr_mgmt_2, lp);
1474                         if (pm > NVI_PM_STDBY)
1475                                 mdelay(REG_UP_TIME);
1476                 }
1477                 if (ret < 0) {
1478                         dev_err(&st->i2c->dev, "%s %d=>%d ERR=%d\n",
1479                                 __func__, st->pm, pm, ret);
1480                         pm = NVI_PM_ERR;
1481                 }
1482                 if (st->dbg & NVI_DBG_SPEW_MSG)
1483                         dev_info(&st->i2c->dev, "%s %d=>%d PM2=%x LPA=%x\n",
1484                                  __func__, st->pm, pm, pwr_mgmt_2, lp);
1485                 st->pm = pm;
1486                 if (ret > 0)
1487                         ret = 0;
1488         }
1489         if (pm_req == NVI_PM_AUTO) {
1490                 if (pm > NVI_PM_STDBY)
1491                         irq = true;
1492                 else
1493                         irq = false;
1494                 if (pm > NVI_PM_ON_CYCLE)
1495                         nvi_user_ctrl_en(st, true, true);
1496                 if ((pm == NVI_PM_ON_FULL) && (!st->ts_gyro))
1497                         st->ts_gyro = nvs_timestamp() +
1498                                            st->chip_config.gyro_start_delay_ns;
1499         } else {
1500                 /* interrupts are disabled until NVI_PM_AUTO */
1501                 irq = false;
1502         }
1503         nvi_int_able(st, irq);
1504         return ret;
1505 }
1506
1507 static void nvi_pm_exit(struct nvi_state *st)
1508 {
1509         if (st->hal)
1510                 nvi_pm(st, NVI_PM_OFF_FORCE);
1511         nvs_vregs_exit(&st->i2c->dev, st->vreg, ARRAY_SIZE(nvi_vregs));
1512 }
1513
1514 static int nvi_pm_init(struct nvi_state *st)
1515 {
1516         int ret;
1517
1518         ret = nvs_vregs_init(&st->i2c->dev,
1519                              st->vreg, ARRAY_SIZE(nvi_vregs), nvi_vregs);
1520         st->pm = NVI_PM_ERR;
1521         return ret;
1522 }
1523
1524 static int nvi_aux_delay(struct nvi_state *st)
1525 {
1526         u8 val;
1527         unsigned int delay_us;
1528         unsigned int delay_new;
1529         int i;
1530         int j;
1531         int ret = 0;
1532
1533         /* determine valid delays by ports enabled */
1534         delay_new = 0;
1535         for (i = 0; i < AUX_PORT_MAX; i++) {
1536                 if (st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN) {
1537                         if (delay_new < st->aux.port[i].nmp.delay_ms)
1538                                 delay_new = st->aux.port[i].nmp.delay_ms;
1539                 }
1540         }
1541         delay_new *= 1000;
1542         /* fastest delays for AUX smplrt source */
1543         delay_us = -1;
1544         for (i = 0; i < st->hal->smplrt[DEV_AUX]->dev_delays_n; i++) {
1545                 j = st->hal->smplrt[DEV_AUX]->dev_delays[i];
1546                 if (st->enabled[j] && st->delay_us[j]) {
1547                         if (st->delay_us[j] < delay_us)
1548                                 delay_us = st->delay_us[j];
1549                 }
1550         }
1551         if (delay_us == -1)
1552                 delay_us = st->hal->smplrt[DEV_AUX]->delay_us_dflt;
1553         if (delay_us < st->hal->smplrt[DEV_AUX]->delay_us_min)
1554                 delay_us = st->hal->smplrt[DEV_AUX]->delay_us_min;
1555         if (delay_us > st->hal->smplrt[DEV_AUX]->delay_us_max)
1556                 delay_us = st->hal->smplrt[DEV_AUX]->delay_us_max;
1557         st->smplrt_delay_us[DEV_AUX] = delay_us;
1558
1559         if (delay_new % delay_us) {
1560                 delay_new /= delay_us;
1561         } else {
1562                 delay_new /= delay_us;
1563                 if (delay_new)
1564                         delay_new--;
1565         }
1566         st->aux.delay_hw = delay_new;
1567         ret = nvi_wr_i2c_slv4_ctrl(st, (bool)
1568                               (st->rc.i2c_slv_ctrl[AUX_PORT_IO] & BIT_SLV_EN));
1569         if (ret > 0)
1570                 ret = 0;
1571         /* HW port delay enable */
1572         val = BIT_DELAY_ES_SHADOW;
1573         for (i = 0; i < AUX_PORT_MAX; i++) {
1574                 if (st->aux.port[i].nmp.delay_ms)
1575                         val |= (1 << i);
1576         }
1577         ret |= nvi_wr_i2c_mst_delay_ctrl(st, val);
1578
1579         if (st->hal->part >= ICM20628)
1580                 /* FIXME */
1581                 ret |= nvi_wr_i2c_mst_odr_config(st, 4);
1582         return ret;
1583 }
1584
1585 static int nvi_dev_delay(struct nvi_state *st, unsigned int dev)
1586 {
1587         unsigned int delay_us;
1588         unsigned int delay_us_old;
1589         unsigned int fs_hz;
1590         u8 lpf = 0;
1591         u16 smplrt_div;
1592         int i;
1593         int j;
1594         int ret;
1595         int ret_t = 0;
1596
1597         /* find the fastest polling of all the enabled devices */
1598         delay_us = -1;
1599         for (i = 0; i < st->hal->smplrt[dev]->dev_delays_n; i++) {
1600                 j = st->hal->smplrt[dev]->dev_delays[i];
1601                 if (st->enabled[j] && st->delay_us[j]) {
1602                         if (st->delay_us[j] < delay_us)
1603                                 delay_us = st->delay_us[j];
1604                 }
1605         }
1606         if (delay_us == -1)
1607                 delay_us = st->hal->smplrt[dev]->delay_us_dflt;
1608         if (delay_us < st->hal->smplrt[dev]->delay_us_min)
1609                 delay_us = st->hal->smplrt[dev]->delay_us_min;
1610         if (delay_us > st->hal->smplrt[dev]->delay_us_max)
1611                 delay_us = st->hal->smplrt[dev]->delay_us_max;
1612         delay_us_old = st->smplrt_delay_us[dev];
1613         st->smplrt_delay_us[dev] = delay_us;
1614         /* calculate smplrt_div */
1615         fs_hz = st->hal->smplrt[dev]->base_hz;
1616         smplrt_div = st->smplrt_delay_us[dev] / fs_hz - 1;
1617         /* calculate LPF */
1618         if (st->hal->smplrt[dev]->lpf_us_tbl_n) {
1619                 delay_us <<= 1;
1620                 for (lpf = 0; lpf < st->hal->smplrt[dev]->lpf_us_tbl_n;
1621                                                                        lpf++) {
1622                         if (delay_us < st->hal->smplrt[dev]->lpf_us_tbl[lpf])
1623                                 break;
1624                 }
1625         }
1626
1627         if (st->dbg)
1628                 dev_info(&st->i2c->dev, "%s dev=%u delay=%u\n",
1629                          __func__, dev, st->smplrt_delay_us[dev]);
1630         if (st->smplrt_delay_us[dev] < delay_us_old) {
1631                 /* go faster */
1632                 nvi_aux_delay(st);
1633                 ret = st->hal->smplrt[dev]->lpf_wr(st, -1, -1, -1, lpf);
1634                 if (ret < 0)
1635                         ret_t |= ret;
1636                 ret_t |= nvi_wr_smplrt_div(st, dev, smplrt_div);
1637         } else {
1638                 /* go slower */
1639                 ret_t |= nvi_wr_smplrt_div(st, dev, smplrt_div);
1640                 ret = st->hal->smplrt[dev]->lpf_wr(st, -1, -1, -1, lpf);
1641                 nvi_aux_delay(st);
1642                 if (ret < 0)
1643                         ret_t |= ret;
1644         }
1645         return ret_t;
1646 }
1647
1648 int nvi_en(struct nvi_state *st)
1649 {
1650         unsigned int master_enable;
1651         int i;
1652         int ret;
1653         int ret_t;
1654
1655         master_enable = st->master_enable;
1656         st->master_enable &= ~DEV_MPU_MASK;
1657         for (i = 0; i < DEV_N; i++) {
1658                 if (st->enabled[i])
1659                         st->master_enable |= (1 << i);
1660         }
1661         if (st->master_enable & (1 << DEV_ANGLVEL))
1662                 ret_t = nvi_pm(st, NVI_PM_ON_FULL);
1663         else if (st->master_enable & (DEV_MPU_MASK | (1 << DEV_AUX)))
1664                 ret_t = nvi_pm(st, NVI_PM_ON);
1665         else
1666                 return nvi_pm(st, NVI_PM_AUTO);
1667
1668         if (st->master_enable & (1 << DEV_ACCEL)) {
1669                 for (i = 0; i < AXIS_N; i++)
1670                         ret_t |= nvi_wr_accel_offset(st, i,
1671                                                 (u16)(st->rom_accel_offset[i] +
1672                                             (st->input_accel_offset[i] << 1)));
1673                 ret = nvi_wr_accel_config(st, 0, 0,
1674                                           st->chip_config.accel_fs, 0);
1675                 if (ret < 0)
1676                         ret_t |= ret;
1677                 ret_t |= nvi_dev_delay(st, DEV_ACCEL);
1678         }
1679         if (st->master_enable & (1 << DEV_ANGLVEL)) {
1680                 for (i = 0; i < AXIS_N; i++)
1681                         ret_t |= nvi_wr_gyro_offset(st, i,
1682                                                  (u16)(st->rom_gyro_offset[i] +
1683                                                     st->input_gyro_offset[i]));
1684                 ret = nvi_wr_gyro_config(st, 0, 0, st->chip_config.fsr, 0);
1685                 if (ret < 0)
1686                         ret_t |= ret;
1687                 ret_t |= nvi_dev_delay(st, DEV_ANGLVEL);
1688         }
1689         if ((st->master_enable & DEV_MPU_MASK) || (st->aux.reset_fifo &&
1690                                                 (st->master_enable & DEV_AUX)))
1691                 ret_t |= nvi_reset(st, true, false);
1692         ret_t |= nvi_pm(st, NVI_PM_AUTO);
1693         if (st->dbg & NVI_DBG_SPEW_MSG)
1694                 dev_info(&st->i2c->dev, "%s master_enable=%x=>%x ret=%d\n",
1695                          __func__, master_enable, st->master_enable, ret_t);
1696         return ret_t;
1697 }
1698
1699 static void nvi_aux_dbg(struct nvi_state *st, char *tag, int val)
1700 {
1701         struct nvi_mpu_port *n;
1702         struct aux_port *p;
1703         struct aux_ports *a;
1704         u8 data[4];
1705         int i;
1706
1707         if (!(st->dbg & NVI_DBG_SPEW_AUX))
1708                 return;
1709
1710         dev_info(&st->i2c->dev, "%s %s %d\n", __func__, tag, val);
1711         for (i = 0; i < AUX_PORT_MAX; i++) {
1712                 nvi_i2c_rd(st, st->hal->reg->i2c_slv0_addr.bank,
1713                            st->hal->reg->i2c_slv0_addr.reg + (i * 3), 3, data);
1714                 nvi_i2c_rd(st, st->hal->reg->i2c_slv0_do.bank,
1715                            st->hal->reg->i2c_slv0_do.reg + i, 1, &data[3]);
1716                 /* HW = hardware */
1717                 pr_info("HW: P%d AD=%x RG=%x CL=%x DO=%x\n",
1718                         i, data[0], data[1], data[2], data[3]);
1719                 n = &st->aux.port[i].nmp;
1720                 /* NS = nmp structure */
1721                 pr_info("NS: P%d AD=%x RG=%x CL=%x DO=%x MS=%u US=%lu SB=%x\n",
1722                         i, n->addr, n->reg, n->ctrl, n->data_out, n->delay_ms,
1723                         n->delay_us, n->shutdown_bypass);
1724                 p = &st->aux.port[i];
1725                 /* PS = port structure */
1726                 pr_info("PS: P%d OFFSET=%u EN=%x FIFOEN=%x HWDOUT=%x\n",
1727                         i, p->ext_data_offset,
1728                         !!(st->enabled[DEV_AUX] & (1 << i)),
1729                         p->fifo_en, p->hw_do);
1730         }
1731         a = &st->aux;
1732         pr_info("AUX: EN=%x MEN=%x MDLY=%x GDLY=%u DATN=%u BPEN=%x BPLK=%d\n",
1733                 !!(st->master_enable & (1 << DEV_AUX)),
1734                 !!(st->rc.user_ctrl & BIT_I2C_MST_EN),
1735                 (st->rc.i2c_slv_ctrl[AUX_PORT_IO] & BITS_I2C_MST_DLY),
1736                 st->smplrt_delay_us[DEV_AUX], a->ext_data_n,
1737                 (st->rc.int_pin_cfg & BIT_BYPASS_EN), a->bypass_lock);
1738 }
1739
1740 static void nvi_aux_read(struct nvi_state *st)
1741 {
1742         struct aux_port *ap;
1743         s64 ts;
1744         u8 *p;
1745         unsigned int i;
1746         unsigned int len;
1747         int ret;
1748
1749         if ((!st->aux.ext_data_n) || (!(st->rc.user_ctrl & BIT_I2C_MST_EN)))
1750                 return;
1751
1752         ret = nvi_i2c_rd(st, st->hal->reg->ext_sens_data_00.bank,
1753                          st->hal->reg->ext_sens_data_00.reg,
1754                          st->aux.ext_data_n, (u8 *)&st->aux.ext_data);
1755         if (ret)
1756                 return;
1757
1758         ts = nvs_timestamp();
1759         for (i = 0; i < AUX_PORT_IO; i++) {
1760                 ap = &st->aux.port[i];
1761                 if ((st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN) && (!ap->fifo_en) &&
1762                                                (ap->nmp.addr & BIT_I2C_READ) &&
1763                                                    (ap->nmp.handler != NULL)) {
1764                         p = &st->aux.ext_data[ap->ext_data_offset];
1765                         len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
1766                         ap->nmp.handler(p, len, ts, ap->nmp.ext_driver);
1767                 }
1768         }
1769 }
1770
1771 static void nvi_aux_ext_data_offset(struct nvi_state *st)
1772 {
1773         int i;
1774         unsigned short offset;
1775
1776         offset = 0;
1777         for (i = 0; i < AUX_PORT_IO; i++) {
1778                 if ((st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN) &&
1779                                   (st->aux.port[i].nmp.addr & BIT_I2C_READ)) {
1780                         st->aux.port[i].ext_data_offset = offset;
1781                         offset += (st->aux.port[i].nmp.ctrl &
1782                                    BITS_I2C_SLV_CTRL_LEN);
1783                 }
1784         }
1785         if (offset > AUX_EXT_DATA_REG_MAX) {
1786                 offset = AUX_EXT_DATA_REG_MAX;
1787                 dev_err(&st->i2c->dev,
1788                         "%s ERR MPU slaves exceed data storage\n", __func__);
1789         }
1790         st->aux.ext_data_n = offset;
1791         return;
1792 }
1793
1794 static int nvi_aux_port_data_out(struct nvi_state *st,
1795                                  int port, u8 data_out)
1796 {
1797         int ret;
1798
1799         ret = nvi_wr_i2c_slv_do(st, port, data_out);
1800         if (!ret) {
1801                 st->aux.port[port].nmp.data_out = data_out;
1802                 st->aux.port[port].hw_do = true;
1803         } else {
1804                 st->aux.port[port].hw_do = false;
1805         }
1806         return ret;
1807 }
1808
1809 static int nvi_aux_port_wr(struct nvi_state *st, int port)
1810 {
1811         struct aux_port *ap;
1812         int ret;
1813
1814         ap = &st->aux.port[port];
1815         ret = nvi_wr_i2c_slv_addr(st, port, ap->nmp.addr);
1816         ret |= nvi_wr_i2c_slv_reg(st, port, ap->nmp.reg);
1817         ret |= nvi_wr_i2c_slv_do(st, port, ap->nmp.data_out);
1818         return ret;
1819 }
1820
1821 static int nvi_aux_port_en(struct nvi_state *st,
1822                            int port, bool en)
1823 {
1824         struct aux_port *ap;
1825         u8 val;
1826         int ret = 0;
1827
1828         st->aux.ext_data_n = 0;
1829         ap = &st->aux.port[port];
1830         if ((!(st->rc.i2c_slv_addr[port])) && en) {
1831                 ret = nvi_aux_port_wr(st, port);
1832                 if (!ret)
1833                         ap->hw_do = true;
1834         }
1835         if ((!ap->hw_do) && en)
1836                 nvi_aux_port_data_out(st, port, ap->nmp.data_out);
1837         if (port == AUX_PORT_IO) {
1838                 ret = nvi_wr_i2c_slv4_ctrl(st, en);
1839         } else {
1840                 if (en)
1841                         val = (ap->nmp.ctrl | BIT_SLV_EN);
1842                 else
1843                         val = 0;
1844                 ret = nvi_wr_i2c_slv_ctrl(st, port, val);
1845         }
1846         if (ret > 0) {
1847                 nvi_aux_ext_data_offset(st);
1848                 ret = 0;
1849         }
1850         return ret;
1851 }
1852
1853 static int nvi_aux_enable(struct nvi_state *st, bool enable)
1854 {
1855         bool en;
1856         unsigned int i;
1857         int ret = 0;
1858
1859         if (st->rc.int_pin_cfg & BIT_BYPASS_EN)
1860                 enable = false;
1861         en = false;
1862         if (enable) {
1863                 /* global enable is honored only if a port is enabled */
1864                 for (i = 0; i < AUX_PORT_MAX; i++) {
1865                         if (st->enabled[DEV_AUX] & (1 << i)) {
1866                                 en = true;
1867                                 break;
1868                         }
1869                 }
1870                 if (en == (bool)(st->rc.user_ctrl & BIT_I2C_MST_EN))
1871                         /* if already on then just update delays */
1872                         nvi_dev_delay(st, DEV_AUX);
1873         }
1874         if (en)
1875                 st->master_enable |= (1 << DEV_AUX);
1876         else
1877                 st->master_enable &= (~(1 << DEV_AUX));
1878         if ((bool)(st->rc.user_ctrl & BIT_I2C_MST_EN) == en) {
1879                 if (st->aux.reset_fifo)
1880                         nvi_reset(st, true, false);
1881                 return 0;
1882         }
1883
1884         if (en) {
1885                 for (i = 0; i < AUX_PORT_MAX; i++) {
1886                         if (st->enabled[DEV_AUX] & (1 << i))
1887                                 ret |= nvi_aux_port_en(st, i, true);
1888                 }
1889         } else {
1890                 for (i = 0; i < AUX_PORT_MAX; i++) {
1891                         if (st->rc.i2c_slv_addr[i])
1892                                 nvi_aux_port_en(st, i, false);
1893                 }
1894         }
1895         ret |= nvi_dev_delay(st, DEV_AUX);
1896         if (st->aux.reset_fifo)
1897                 ret |= nvi_reset(st, true, false);
1898         else
1899                 ret |= nvi_user_ctrl_en(st, true, en);
1900         return ret;
1901 }
1902
1903 static int nvi_aux_port_enable(struct nvi_state *st, int port, bool enable)
1904 {
1905         bool fifo_enable = true;
1906         unsigned int i;
1907         int ret;
1908
1909         if (enable)
1910                 st->enabled[DEV_AUX] |= (1 << port);
1911         else
1912                 st->enabled[DEV_AUX] &= ~(1 << port);
1913         /* find the fastest polling of all the enabled aux devices */
1914         st->delay_us[DEV_AUX] = st->hal->smplrt[DEV_AUX]->delay_us_max;
1915         for (i = 0; i < AUX_PORT_MAX; i++) {
1916                 if (st->aux.port[i].nmp.delay_us && (st->enabled[DEV_AUX] &
1917                                                      (1 << i))) {
1918                         if (st->aux.port[i].nmp.delay_us <
1919                                                          st->delay_us[DEV_AUX])
1920                                 st->delay_us[DEV_AUX] =
1921                                                   st->aux.port[i].nmp.delay_us;
1922                 }
1923         }
1924         if ((!enable) || (!(st->aux.port[port].nmp.addr & BIT_I2C_READ)))
1925                 fifo_enable = false;
1926 #ifdef NVI_AUX_FIFO_ENABLE
1927         if (st->aux.port[port].fifo_en != fifo_enable)
1928                 st->aux.reset_fifo = true;
1929         st->aux.port[port].fifo_en = fifo_enable;
1930 #else
1931         st->aux.port[port].fifo_en = false;
1932 #endif /* NVI_AUX_FIFO_ENABLE */
1933         if (enable && (st->rc.int_pin_cfg & BIT_BYPASS_EN))
1934                 return 0;
1935
1936         ret = nvi_aux_port_en(st, port, enable);
1937         ret |= nvi_aux_enable(st, true);
1938         return ret;
1939 }
1940
1941 static int nvi_reset(struct nvi_state *st,
1942                      bool reset_fifo, bool reset_i2c)
1943 {
1944         u8 val;
1945         bool irq = true;
1946         int ret;
1947
1948         if (st->dbg & NVI_DBG_SPEW_MSG)
1949                 dev_info(&st->i2c->dev, "%s FIFO=%x I2C=%x\n",
1950                          __func__, reset_fifo, reset_i2c);
1951         if (st->irq_dis)
1952                 irq = false;
1953         ret = nvi_int_able(st, false);
1954         val = 0;
1955         if (reset_i2c) {
1956                 st->aux.reset_i2c = false;
1957                 ret |= nvi_aux_enable(st, false);
1958                 val |= BIT_I2C_MST_RST;
1959         }
1960         if (reset_fifo) {
1961                 st->aux.reset_fifo = false;
1962                 val |= BIT_FIFO_RST;
1963                 if (st->master_enable & (1 << DEV_DMP))
1964                         val |= BIT_DMP_RST;
1965         }
1966         ret |= nvi_user_ctrl_en(st, !reset_fifo, !reset_i2c);
1967         val |= st->rc.user_ctrl;
1968         ret |= nvi_wr_user_ctrl(st, val);
1969         if (reset_i2c)
1970                 ret |= nvi_aux_enable(st, true);
1971         else
1972                 ret |= nvi_user_ctrl_en(st, true, true);
1973         if (reset_fifo && (st->rc.user_ctrl & BIT_FIFO_EN))
1974                 st->ts_last = nvs_timestamp();
1975         if (irq)
1976                 ret |= nvi_int_able(st, true);
1977         return ret;
1978 }
1979
1980 static int nvi_aux_port_free(struct nvi_state *st, int port)
1981 {
1982         memset(&st->aux.port[port], 0, sizeof(struct aux_port));
1983         st->enabled[DEV_AUX] &= ~(1 << port);
1984         if (st->rc.i2c_slv_addr[port]) {
1985                 nvi_aux_port_wr(st, port);
1986                 nvi_aux_port_en(st, port, false);
1987                 nvi_aux_enable(st, false);
1988                 nvi_aux_enable(st, true);
1989                 if (port != AUX_PORT_IO)
1990                         st->aux.reset_i2c = true;
1991         }
1992         return 0;
1993 }
1994
1995 static int nvi_aux_port_alloc(struct nvi_state *st,
1996                               struct nvi_mpu_port *nmp, int port)
1997 {
1998         int i;
1999
2000         if (st->aux.reset_i2c)
2001                 nvi_reset(st, false, true);
2002         if (port < 0) {
2003                 for (i = 0; i < AUX_PORT_IO; i++) {
2004                         if (st->aux.port[i].nmp.addr == 0)
2005                                 break;
2006                 }
2007                 if (i == AUX_PORT_IO)
2008                         return -ENODEV;
2009         } else {
2010                 if (st->aux.port[port].nmp.addr == 0)
2011                         i = port;
2012                 else
2013                         return -ENODEV;
2014         }
2015
2016         memset(&st->aux.port[i], 0, sizeof(struct aux_port));
2017         memcpy(&st->aux.port[i].nmp, nmp, sizeof(struct nvi_mpu_port));
2018         return i;
2019 }
2020
2021 static int nvi_aux_bypass_enable(struct nvi_state *st, bool enable)
2022 {
2023         u8 val;
2024         int ret;
2025
2026         if ((bool)(st->rc.int_pin_cfg & BIT_BYPASS_EN) == enable)
2027                 return 0;
2028
2029         val = st->rc.int_pin_cfg;
2030         if (enable) {
2031                 ret = nvi_aux_enable(st, false);
2032                 if (!ret) {
2033                         val |= BIT_BYPASS_EN;
2034                         ret = nvi_wr_int_pin_cfg(st, val);
2035                 }
2036         } else {
2037                 val &= ~BIT_BYPASS_EN;
2038                 ret = nvi_wr_int_pin_cfg(st, val);
2039                 if (!ret)
2040                         nvi_aux_enable(st, true);
2041         }
2042         return ret;
2043 }
2044
2045 static int nvi_aux_bypass_request(struct nvi_state *st, bool enable)
2046 {
2047         s64 ns;
2048         s64 to;
2049         int ret = 0;
2050
2051         if ((bool)(st->rc.int_pin_cfg & BIT_BYPASS_EN) == enable) {
2052                 st->aux.bypass_timeout_ns = nvs_timestamp();
2053                 st->aux.bypass_lock++;
2054                 if (!st->aux.bypass_lock)
2055                         dev_err(&st->i2c->dev, "%s rollover ERR\n", __func__);
2056         } else {
2057                 if (st->aux.bypass_lock) {
2058                         ns = nvs_timestamp() - st->aux.bypass_timeout_ns;
2059                         to = st->chip_config.bypass_timeout_ms * 1000000;
2060                         if (ns > to)
2061                                 st->aux.bypass_lock = 0;
2062                         else
2063                                 ret = -EBUSY;
2064                 }
2065                 if (!st->aux.bypass_lock) {
2066                         ret = nvi_aux_bypass_enable(st, enable);
2067                         if (ret)
2068                                 dev_err(&st->i2c->dev, "%s ERR=%d\n",
2069                                         __func__, ret);
2070                         else
2071                                 st->aux.bypass_lock++;
2072                 }
2073         }
2074         return ret;
2075 }
2076
2077 static int nvi_aux_bypass_release(struct nvi_state *st)
2078 {
2079         int ret = 0;
2080
2081         if (st->aux.bypass_lock)
2082                 st->aux.bypass_lock--;
2083         if (!st->aux.bypass_lock) {
2084                 ret = nvi_aux_bypass_enable(st, false);
2085                 if (ret)
2086                         dev_err(&st->i2c->dev, "%s ERR=%d\n", __func__, ret);
2087         }
2088         return ret;
2089 }
2090
2091 static int nvi_aux_dev_valid(struct nvi_state *st,
2092                              struct nvi_mpu_port *nmp, u8 *data)
2093 {
2094         u8 val;
2095         int i;
2096         int ret;
2097
2098         /* turn off bypass */
2099         ret = nvi_aux_bypass_request(st, false);
2100         if (ret)
2101                 return -EBUSY;
2102
2103         /* grab the special port */
2104         ret = nvi_aux_port_alloc(st, nmp, AUX_PORT_IO);
2105         if (ret != AUX_PORT_IO) {
2106                 nvi_aux_bypass_release(st);
2107                 return -EBUSY;
2108         }
2109
2110         /* enable it */
2111         st->aux.port[AUX_PORT_IO].nmp.delay_ms = 0;
2112         st->aux.port[AUX_PORT_IO].nmp.delay_us =
2113                                         st->hal->smplrt[DEV_AUX]->delay_us_min;
2114         ret = nvi_aux_port_enable(st, AUX_PORT_IO, true);
2115         if (ret) {
2116                 nvi_aux_port_free(st, AUX_PORT_IO);
2117                 nvi_aux_bypass_release(st);
2118                 return -EBUSY;
2119         }
2120
2121         /* now turn off all the other ports for fastest response */
2122         for (i = 0; i < AUX_PORT_IO; i++) {
2123                 if (st->rc.i2c_slv_addr[i])
2124                         nvi_aux_port_en(st, i, false);
2125         }
2126         /* start reading the results */
2127         for (i = 0; i < AUX_DEV_VALID_READ_LOOP_MAX; i++) {
2128                 mdelay(AUX_DEV_VALID_READ_DELAY_MS);
2129                 val = 0;
2130                 ret = nvi_i2c_rd(st, st->hal->reg->i2c_mst_status.bank,
2131                                  st->hal->reg->i2c_mst_status.reg, 1, &val);
2132                 if (ret)
2133                         continue;
2134
2135                 if (val & 0x50)
2136                         break;
2137         }
2138         /* these will restore all previously disabled ports */
2139         nvi_aux_bypass_release(st);
2140         nvi_aux_port_free(st, AUX_PORT_IO);
2141         if (i == AUX_DEV_VALID_READ_LOOP_MAX)
2142                 return -ENODEV;
2143
2144         if (val & 0x10) /* NACK */
2145                 return -EIO;
2146
2147         if (nmp->addr & BIT_I2C_READ) {
2148                 ret = nvi_i2c_rd(st, st->hal->reg->i2c_slv4_di.bank,
2149                                  st->hal->reg->i2c_slv4_di.reg, 1, &val);
2150                 if (ret)
2151                         return -EBUSY;
2152
2153                 *data = (u8)val;
2154                 dev_info(&st->i2c->dev, "%s MPU read 0x%x from device 0x%x\n",
2155                         __func__, val, (nmp->addr & ~BIT_I2C_READ));
2156         } else {
2157                 dev_info(&st->i2c->dev, "%s MPU found device 0x%x\n",
2158                         __func__, (nmp->addr & ~BIT_I2C_READ));
2159         }
2160         return 0;
2161 }
2162
2163 static int nvi_aux_mpu_call_pre(struct nvi_state *st, int port)
2164 {
2165         if ((port < 0) || (port >= AUX_PORT_IO))
2166                 return -EINVAL;
2167
2168         if (st->sts & (NVS_STS_SHUTDOWN | NVS_STS_SUSPEND))
2169                 return -EPERM;
2170
2171         if (!st->aux.port[port].nmp.addr)
2172                 return -EINVAL;
2173
2174         return 0;
2175 }
2176
2177 static int nvi_aux_mpu_call_post(struct nvi_state *st,
2178                                  char *tag, int ret)
2179 {
2180         if (ret < 0)
2181                 ret = -EBUSY;
2182         nvi_aux_dbg(st, tag, ret);
2183         return ret;
2184 }
2185
2186 /* See the mpu.h file for details on the nvi_mpu_ calls.
2187  */
2188 int nvi_mpu_dev_valid(struct nvi_mpu_port *nmp, u8 *data)
2189 {
2190         struct nvi_state *st = nvi_state_local;
2191         int ret = -EPERM;
2192
2193         if (st != NULL) {
2194                 if (st->dbg & NVI_DBG_SPEW_AUX)
2195                         pr_info("%s\n", __func__);
2196         } else {
2197                 pr_debug("%s ERR -EAGAIN\n", __func__);
2198                 return -EAGAIN;
2199         }
2200
2201         if (nmp == NULL)
2202                 return -EINVAL;
2203
2204         if ((nmp->addr & BIT_I2C_READ) && (data == NULL))
2205                 return -EINVAL;
2206
2207         nvi_mutex_lock(st);
2208         if (!(st->sts & (NVS_STS_SHUTDOWN | NVS_STS_SUSPEND))) {
2209                 nvi_pm(st, NVI_PM_ON);
2210                 ret = nvi_aux_dev_valid(st, nmp, data);
2211                 nvi_pm(st, NVI_PM_AUTO);
2212                 nvi_aux_dbg(st, "nvi_mpu_dev_valid ret=", ret);
2213         }
2214         nvi_mutex_unlock(st);
2215         return ret;
2216 }
2217 EXPORT_SYMBOL(nvi_mpu_dev_valid);
2218
2219 int nvi_mpu_port_alloc(struct nvi_mpu_port *nmp)
2220 {
2221         struct nvi_state *st = nvi_state_local;
2222         int ret = -EPERM;
2223
2224         if (st != NULL) {
2225                 if (st->dbg & NVI_DBG_SPEW_AUX)
2226                         pr_info("%s\n", __func__);
2227         } else {
2228                 pr_debug("%s ERR -EAGAIN\n", __func__);
2229                 return -EAGAIN;
2230         }
2231
2232         if (nmp == NULL)
2233                 return -EINVAL;
2234
2235         if (!(nmp->ctrl & BITS_I2C_SLV_CTRL_LEN))
2236                 return -EINVAL;
2237
2238         nvi_mutex_lock(st);
2239         if (!(st->sts & (NVS_STS_SHUTDOWN | NVS_STS_SUSPEND))) {
2240                 nvi_pm(st, NVI_PM_ON);
2241                 ret = nvi_aux_port_alloc(st, nmp, -1);
2242                 nvi_pm(st, NVI_PM_AUTO);
2243                 ret = nvi_aux_mpu_call_post(st,
2244                                          "nvi_mpu_port_alloc ret/port=", ret);
2245         }
2246         nvi_mutex_unlock(st);
2247         return ret;
2248 }
2249 EXPORT_SYMBOL(nvi_mpu_port_alloc);
2250
2251 int nvi_mpu_port_free(int port)
2252 {
2253         struct nvi_state *st = nvi_state_local;
2254         int ret;
2255
2256         if (st != NULL) {
2257                 if (st->dbg & NVI_DBG_SPEW_AUX)
2258                         pr_info("%s port %d\n", __func__, port);
2259         } else {
2260                 pr_debug("%s port %d ERR -EAGAIN\n", __func__, port);
2261                 return -EAGAIN;
2262         }
2263
2264         nvi_mutex_lock(st);
2265         ret = nvi_aux_mpu_call_pre(st, port);
2266         if (!ret) {
2267                 nvi_pm(st, NVI_PM_ON);
2268                 ret = nvi_aux_port_free(st, port);
2269                 nvi_pm(st, NVI_PM_AUTO);
2270                 ret = nvi_aux_mpu_call_post(st, "nvi_mpu_port_free ret=", ret);
2271         }
2272         nvi_mutex_unlock(st);
2273         return ret;
2274 }
2275 EXPORT_SYMBOL(nvi_mpu_port_free);
2276
2277 int nvi_mpu_enable(int port, bool enable)
2278 {
2279         struct nvi_state *st = nvi_state_local;
2280         int ret;
2281
2282         if (st != NULL) {
2283                 if (st->dbg & NVI_DBG_SPEW_AUX)
2284                         pr_info("%s port %d: %x\n", __func__, port, enable);
2285         } else {
2286                 pr_debug("%s port %d: %x ERR -EAGAIN\n",
2287                          __func__, port, enable);
2288                 return -EAGAIN;
2289         }
2290
2291         nvi_mutex_lock(st);
2292         ret = nvi_aux_mpu_call_pre(st, port);
2293         if (!ret) {
2294                 nvi_pm(st, NVI_PM_ON);
2295                 ret = nvi_aux_port_enable(st, port, enable);
2296                 nvi_pm(st, NVI_PM_AUTO);
2297                 ret = nvi_aux_mpu_call_post(st, "nvi_mpu_enable ret=", ret);
2298         }
2299         nvi_mutex_unlock(st);
2300         return ret;
2301 }
2302 EXPORT_SYMBOL(nvi_mpu_enable);
2303
2304 int nvi_mpu_delay_ms(int port, u8 delay_ms)
2305 {
2306         struct nvi_state *st = nvi_state_local;
2307         int ret;
2308
2309         if (st != NULL) {
2310                 if (st->dbg & NVI_DBG_SPEW_AUX)
2311                         pr_info("%s port %d: %u\n", __func__, port, delay_ms);
2312         } else {
2313                 pr_debug("%s port %d: %u ERR -EAGAIN\n",
2314                          __func__, port, delay_ms);
2315                 return -EAGAIN;
2316         }
2317
2318         nvi_mutex_lock(st);
2319         ret = nvi_aux_mpu_call_pre(st, port);
2320         if (!ret) {
2321                 st->aux.port[port].nmp.delay_ms = delay_ms;
2322                 if (st->rc.i2c_slv_ctrl[port] & BIT_SLV_EN)
2323                         ret = nvi_dev_delay(st, DEV_AUX);
2324                 ret |= nvi_aux_mpu_call_post(st, "nvi_mpu_delay_ms ret=", ret);
2325         }
2326         nvi_mutex_unlock(st);
2327         return ret;
2328 }
2329 EXPORT_SYMBOL(nvi_mpu_delay_ms);
2330
2331 int nvi_mpu_delay_us(int port, unsigned long delay_us)
2332 {
2333         struct nvi_state *st = nvi_state_local;
2334         int ret;
2335
2336         if (st != NULL) {
2337                 if (st->dbg & NVI_DBG_SPEW_AUX)
2338                         pr_info("%s port %d: %lu\n", __func__, port, delay_us);
2339         } else {
2340                 pr_debug("%s port %d: %lu ERR -EAGAIN\n",
2341                         __func__, port, delay_us);
2342                 return -EAGAIN;
2343         }
2344
2345         nvi_mutex_lock(st);
2346         ret = nvi_aux_mpu_call_pre(st, port);
2347         if (!ret) {
2348                 if (st->aux.port[port].nmp.delay_us != delay_us) {
2349                         st->aux.port[port].nmp.delay_us = delay_us;
2350                         if (st->rc.i2c_slv_ctrl[port] & BIT_SLV_EN)
2351                                 ret = nvi_dev_delay(st, DEV_AUX);
2352                 }
2353                 ret |= nvi_aux_mpu_call_post(st, "nvi_mpu_delay_us ret=", ret);
2354         }
2355         nvi_mutex_unlock(st);
2356         return ret;
2357 }
2358 EXPORT_SYMBOL(nvi_mpu_delay_us);
2359
2360 int nvi_mpu_data_out(int port, u8 data_out)
2361 {
2362         struct nvi_state *st = nvi_state_local;
2363         int ret;
2364
2365         if (st == NULL)
2366                 return -EAGAIN;
2367
2368         ret = nvi_aux_mpu_call_pre(st, port);
2369         if (!ret) {
2370                 if (st->rc.i2c_slv_ctrl[port] & BIT_SLV_EN) {
2371                         ret = nvi_aux_port_data_out(st, port, data_out);
2372                 } else {
2373                         st->aux.port[port].nmp.data_out = data_out;
2374                         st->aux.port[port].hw_do = false;
2375                 }
2376                 if (ret < 0)
2377                         ret = -EBUSY;
2378         }
2379         return ret;
2380 }
2381 EXPORT_SYMBOL(nvi_mpu_data_out);
2382
2383 int nvi_mpu_batch(int port, unsigned int flags,
2384                   unsigned int period_us, unsigned int timeout_us)
2385 {
2386         struct nvi_state *st = nvi_state_local;
2387         int ret;
2388
2389         if (st != NULL) {
2390                 if (st->dbg & NVI_DBG_SPEW_AUX)
2391                         pr_info("%s port %d: f=%x p=%u t=%u\n",
2392                                 __func__, port, flags, period_us, timeout_us);
2393         } else {
2394                 pr_debug("%s port %d: f=%x p=%u t=%u ERR -EAGAIN\n",
2395                         __func__, port, flags, period_us, timeout_us);
2396                 return -EAGAIN;
2397         }
2398
2399         nvi_mutex_lock(st);
2400         ret = nvi_aux_mpu_call_pre(st, port);
2401         if (!ret) {
2402                 if ((st->aux.port[port].nmp.id != ID_INVALID) &&
2403                                 (st->aux.port[port].nmp.id < ID_INVALID_END)) {
2404                         st->aux.port[port].batch_flags = flags;
2405                         st->aux.port[port].batch_period_us = period_us;
2406                         st->aux.port[port].batch_timeout_us = timeout_us;
2407                         if (st->aux.port[port].nmp.delay_us != period_us) {
2408                                 st->aux.port[port].nmp.delay_us = period_us;
2409                                 if (st->rc.i2c_slv_ctrl[port] & BIT_SLV_EN)
2410                                         ret = nvi_dev_delay(st, DEV_AUX);
2411                         }
2412                         ret = nvi_aux_mpu_call_post(st,
2413                                               "nvi_mpu_batch ret/flags=", ret);
2414                 } else {
2415                         ret = -EINVAL;
2416                 }
2417         }
2418         nvi_mutex_unlock(st);
2419         return ret;
2420 }
2421 EXPORT_SYMBOL(nvi_mpu_batch);
2422
2423 int nvi_mpu_flush(int port)
2424 {
2425         struct nvi_state *st = nvi_state_local;
2426         int ret;
2427
2428         if (st != NULL) {
2429                 if (st->dbg & NVI_DBG_SPEW_AUX)
2430                         pr_info("%s port %d\n", __func__, port);
2431         } else {
2432                 pr_debug("%s port %d ERR -EAGAIN\n", __func__, port);
2433                 return -EAGAIN;
2434         }
2435
2436         nvi_mutex_lock(st);
2437         ret = nvi_aux_mpu_call_pre(st, port);
2438         if (!ret) {
2439                 if ((st->aux.port[port].nmp.id != ID_INVALID) &&
2440                                 (st->aux.port[port].nmp.id < ID_INVALID_END)) {
2441                         if (st->aux.port[port].fifo_en) {
2442                                 st->aux.port[port].flush = true;
2443                                 ret = nvi_en(st);
2444                         } else {
2445                                 nvi_flush_aux(st, port);
2446                         }
2447                         ret = nvi_aux_mpu_call_post(st, "nvi_mpu_flush ret=",
2448                                                     ret);
2449                 } else {
2450                         ret = -EINVAL;
2451                 }
2452         }
2453         nvi_mutex_unlock(st);
2454         return ret;
2455 }
2456 EXPORT_SYMBOL(nvi_mpu_flush);
2457
2458 int nvi_mpu_fifo(int port, unsigned int *reserve, unsigned int *max)
2459 {
2460         struct nvi_state *st = nvi_state_local;
2461         int ret;
2462
2463         if (st != NULL) {
2464                 if (st->dbg & NVI_DBG_SPEW_AUX)
2465                         pr_info("%s port %d\n", __func__, port);
2466         } else {
2467                 pr_debug("%s port %d ERR -EAGAIN\n", __func__, port);
2468                 return -EAGAIN;
2469         }
2470
2471         nvi_mutex_lock(st);
2472         ret = nvi_aux_mpu_call_pre(st, port);
2473         if (!ret) {
2474                 if ((st->aux.port[port].nmp.id != ID_INVALID) &&
2475                         (st->aux.port[port].nmp.id < ID_INVALID_END)) {
2476                         if (reserve)
2477                                 /* batch not supported at this time */
2478                                 *reserve = 0;
2479                         if (max)
2480                                 /* batch not supported at this time */
2481                                 *max = 0;
2482                         ret = nvi_aux_mpu_call_post(st, "nvi_mpu_fifo ret=",
2483                                                     ret);
2484                 } else {
2485                         ret = -EINVAL;
2486                 }
2487         }
2488         nvi_mutex_unlock(st);
2489         return ret;
2490 }
2491 EXPORT_SYMBOL(nvi_mpu_fifo);
2492
2493 int nvi_mpu_bypass_request(bool enable)
2494 {
2495         struct nvi_state *st = nvi_state_local;
2496         int ret = -EPERM;
2497
2498         if (st != NULL) {
2499                 if (st->dbg & NVI_DBG_SPEW_AUX)
2500                         pr_info("%s enable=%x\n", __func__, enable);
2501         } else {
2502                 pr_debug("%s ERR -EAGAIN\n", __func__);
2503                 return -EAGAIN;
2504         }
2505
2506         nvi_mutex_lock(st);
2507         if (!(st->sts & (NVS_STS_SHUTDOWN | NVS_STS_SUSPEND))) {
2508                 nvi_pm(st, NVI_PM_ON);
2509                 ret = nvi_aux_bypass_request(st, enable);
2510                 nvi_pm(st, NVI_PM_AUTO);
2511                 ret = nvi_aux_mpu_call_post(st, "nvi_mpu_bypass_request ret=",
2512                                             ret);
2513         }
2514         nvi_mutex_unlock(st);
2515         return ret;
2516 }
2517 EXPORT_SYMBOL(nvi_mpu_bypass_request);
2518
2519 int nvi_mpu_bypass_release(void)
2520 {
2521         struct nvi_state *st = nvi_state_local;
2522
2523         if (st != NULL) {
2524                 if (st->dbg & NVI_DBG_SPEW_AUX)
2525                         pr_info("%s\n", __func__);
2526         } else {
2527                 pr_debug("%s\n", __func__);
2528                 return 0;
2529         }
2530
2531         nvi_mutex_lock(st);
2532         if (!(st->sts & (NVS_STS_SHUTDOWN | NVS_STS_SUSPEND))) {
2533                 nvi_pm(st, NVI_PM_ON);
2534                 nvi_aux_bypass_release(st);
2535                 nvi_pm(st, NVI_PM_AUTO);
2536                 nvi_aux_mpu_call_post(st, "nvi_mpu_bypass_release", 0);
2537         }
2538         nvi_mutex_unlock(st);
2539         return 0;
2540 }
2541 EXPORT_SYMBOL(nvi_mpu_bypass_release);
2542
2543
2544 static unsigned int nvi_report_accel(struct nvi_state *st, u8 *data, s64 ts)
2545 {
2546         s16 accel[AXIS_N + 1]; /* +1 for FSYNC status */
2547         s16 accel_uc;
2548         unsigned int i;
2549         unsigned int buf_i = 0;
2550
2551         for (i = 0; i < AXIS_N; i++) {
2552                 if (st->enabled[DEV_ACCEL] & (1 << i)) {
2553                         accel_uc = be16_to_cpup((__be16 *)&data[2 * i]);
2554                         accel[buf_i] = accel_uc * st->chip_info.multi;
2555                         buf_i++;
2556                 }
2557         }
2558         accel[AXIS_N] = 0;
2559         if (st->fsync[DEV_ACCEL]) {
2560                 /* FSYNC enabled for this sensor */
2561                 if (accel[st->fsync[DEV_ACCEL] - 1] & 1)
2562                         /* FSYNC asserted if LSb set for this axis */
2563                         accel[AXIS_N] = 0x10 << st->fsync[DEV_ACCEL];
2564         }
2565         st->nvs->handler(st->nvs_st[DEV_ACCEL], accel, ts);
2566         return buf_i * 2;
2567 }
2568
2569 static void nvi_report_temp(struct nvi_state *st, u8 *data, s64 ts)
2570 {
2571         s16 temp;
2572
2573         temp = be16_to_cpup((__be16 *)data);
2574         st->nvs->handler(st->nvs_st[DEV_TEMP], &temp, ts);
2575 }
2576
2577 static unsigned int nvi_report_gyro(struct nvi_state *st, u8 *data, s64 ts)
2578 {
2579         s16 anglvel[AXIS_N + 1]; /* +1 for FSYNC status */
2580         unsigned int i;
2581         unsigned int buf_i = 0;
2582
2583         for (i = 0; i < AXIS_N; i++) {
2584                 if (st->enabled[DEV_ANGLVEL] & (1 << i)) {
2585                         anglvel[buf_i] =
2586                                       be16_to_cpup((__be16 *)&data[2 * buf_i]);
2587                         buf_i++;
2588                 }
2589         }
2590         anglvel[AXIS_N] = 0;
2591         if (st->fsync[DEV_ANGLVEL]) {
2592                 /* FSYNC enabled for this sensor */
2593                 if (anglvel[st->fsync[DEV_ANGLVEL] - 1] & 1)
2594                         /* FSYNC asserted if LSb set for this axis */
2595                         anglvel[AXIS_N] = 0x10 << st->fsync[DEV_ANGLVEL];
2596         }
2597         if (ts >= st->ts_gyro)
2598                 st->nvs->handler(st->nvs_st[DEV_ANGLVEL], &anglvel, ts);
2599         return buf_i * 2;
2600 }
2601
2602 static int nvi_accel_read(struct nvi_state *st, s64 ts)
2603 {
2604         u8 data[6];
2605         int ret;
2606
2607         ret = nvi_i2c_rd(st, st->hal->reg->accel_xout_h.bank,
2608                          st->hal->reg->accel_xout_h.reg, 6, data);
2609         if (!ret)
2610                 ret = nvi_report_accel(st, data, ts);
2611         return ret;
2612 }
2613
2614 static unsigned int nvi_fifo_read_accel(struct nvi_state *st,
2615                                         unsigned int buf_i, s64 ts)
2616 {
2617         if (st->rc.fifo_en & (1 << st->hal->bit->accel_fifo_en)) {
2618                 nvi_report_accel(st, &st->buf[buf_i], ts);
2619                 buf_i += 6;
2620         }
2621         return buf_i;
2622 }
2623
2624 static unsigned int nvi_fifo_read_anglvel(struct nvi_state *st,
2625                                           unsigned int buf_i, s64 ts)
2626 {
2627         /* if gyro is enabled then FIFO is enabled for gyro */
2628         if (st->enabled[DEV_ANGLVEL])
2629                 buf_i += nvi_report_gyro(st, &st->buf[buf_i], ts);
2630         return buf_i;
2631 }
2632
2633 static unsigned int nvi_fifo_read_temp(struct nvi_state *st,
2634                                        unsigned int buf_i, s64 ts)
2635 {
2636         if (st->rc.fifo_en & (1 << st->hal->bit->temp_fifo_en)) {
2637                 nvi_report_temp(st, &st->buf[buf_i], ts);
2638                 buf_i += 2;
2639         }
2640         return buf_i;
2641 }
2642
2643 static irqreturn_t nvi_irq_thread(int irq, void *dev_id)
2644 {
2645         struct nvi_state *st = (struct nvi_state *)dev_id;
2646         struct aux_port *ap;
2647         u16 fifo_count = 0;
2648         u16 fifo_sample_size = 0;
2649         u16 fifo_rd_n;
2650         u16 fifo_align;
2651         s64 ts;
2652         s64 ts_now;
2653         s64 ts_end;
2654         s64 ts_rng;
2655         unsigned int ts_dly;
2656         unsigned int ts_n;
2657         unsigned int buf_index;
2658         unsigned int samples;
2659         unsigned int len;
2660         unsigned int i;
2661         int ret;
2662
2663         nvi_mutex_lock(st);
2664         if (st->irq_storm_n > NVI_IRQ_STORM_MAX_N)
2665                 goto nvi_irq_thread_exit_reset;
2666
2667         if (st->sts & (NVS_STS_SUSPEND | NVS_STS_SHUTDOWN))
2668                 goto nvi_irq_thread_exit;
2669
2670         /* if only accelermeter data */
2671         if (st->rc.pwr_mgmt_1 & BIT_CYCLE) {
2672                 ts = nvs_timestamp();
2673                 ret = nvi_accel_read(st, ts);
2674                 goto nvi_irq_thread_exit_ts;
2675         }
2676
2677         nvi_aux_read(st);
2678         /* handle FIFO enabled data */
2679         fifo_sample_size = st->fifo_sample_size;
2680         if (!fifo_sample_size)
2681                 goto nvi_irq_thread_exit;
2682
2683         ts_now = nvs_timestamp();
2684         ret = nvi_i2c_rd(st, st->hal->reg->fifo_count_h.bank,
2685                          st->hal->reg->fifo_count_h.reg, 2, st->buf);
2686         if (ret)
2687                 goto nvi_irq_thread_exit;
2688
2689         ts_end = atomic64_read(&st->ts_irq);
2690         fifo_count = be16_to_cpup((__be16 *)(&st->buf));
2691         /* FIFO threshold */
2692         if (st->chip_config.fifo_thr > fifo_sample_size) {
2693                 if (fifo_count > st->chip_config.fifo_thr) {
2694                         if (st->dbg & NVI_DBG_SPEW_FIFO)
2695                                 dev_info(&st->i2c->dev,
2696                                          "FIFO threshold exceeded\n");
2697                         goto nvi_irq_thread_exit_reset;
2698                 }
2699         }
2700
2701         fifo_align = fifo_count % fifo_sample_size;
2702         if (fifo_count < fifo_sample_size + fifo_align)
2703                 /* consider resetting FIFO if doesn't divide cleanly */
2704                 goto nvi_irq_thread_exit;
2705
2706         samples = (fifo_count / fifo_sample_size);
2707         if (st->dbg & NVI_DBG_SPEW_FIFO)
2708                 dev_info(&st->i2c->dev,
2709                          "fifo_count=%u sample_size=%u offset=%u samples=%u\n",
2710                          fifo_count, fifo_sample_size, fifo_align, samples);
2711         if (!samples)
2712                 goto nvi_irq_thread_exit;
2713
2714         /* ts_dly = the programmed sample rate */
2715         ts_dly = st->smplrt_delay_us[0] * 1000;
2716         if (ts_end < (ts_now - ts_dly))
2717                 /* if TS IRQ hasn't occured within the rate then use now TS */
2718                 ts_end = ts_now;
2719         /* ts_rng = the time range we have for TSs to fit */
2720         ts_rng = ts_end - st->ts_last;
2721         ts = ts_rng;
2722         do_div(ts, ts_dly);
2723         /* ts_n = the number of TS we'll have at unmodified rate */
2724         ts_n = ts;
2725         if (samples > ts_n) {
2726                 /* modify rate to fit */
2727                 do_div(ts_rng, samples);
2728                 ts_dly = ts_rng;
2729         }
2730         ts = ts_end - (ts_dly * samples);
2731         fifo_rd_n = 0;
2732         buf_index = 0;
2733         while (samples) {
2734                 if (buf_index >= fifo_rd_n) {
2735                         fifo_rd_n = sizeof(st->buf);
2736                         fifo_rd_n -= fifo_align;
2737                         fifo_rd_n /= fifo_sample_size;
2738                         if (samples < fifo_rd_n)
2739                                 fifo_rd_n = samples;
2740                         fifo_rd_n *= fifo_sample_size;
2741                         fifo_rd_n += fifo_align;
2742                         ret = nvi_i2c_rd(st, st->hal->reg->fifo_r_w.bank,
2743                                          st->hal->reg->fifo_r_w.reg,
2744                                          fifo_rd_n, st->buf);
2745                         if (ret)
2746                                 goto nvi_irq_thread_exit_ts;
2747
2748                         buf_index = fifo_align;
2749                 }
2750
2751                 ts += ts_dly;
2752                 for (i = 0; i < st->hal->fifo_read_n; i++)
2753                         buf_index = st->hal->fifo_read[i](st, buf_index, ts);
2754                 for (i = 0; i < AUX_PORT_IO; i++) {
2755                         ap = &st->aux.port[i];
2756                         if (ap->fifo_en &&
2757                                       (st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN)) {
2758                                 len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
2759                                 if (ap->nmp.handler != NULL)
2760                                         ap->nmp.handler(&st->buf[buf_index],
2761                                                         len, ts,
2762                                                         ap->nmp.ext_driver);
2763                                 buf_index += len;
2764                         }
2765                 }
2766                 samples--;
2767         }
2768 nvi_irq_thread_exit_ts:
2769         st->ts_last = ts;
2770 nvi_irq_thread_exit:
2771         nvi_enable_irq(st);
2772         nvi_mutex_unlock(st);
2773         return IRQ_HANDLED;
2774
2775 nvi_irq_thread_exit_reset:
2776         if (st->dbg & NVI_DBG_SPEW_FIFO)
2777                 dev_info(&st->i2c->dev,
2778                          "%s_exit_reset fifo_count=%u fifo_sample_size=%u\n",
2779                          __func__, fifo_count, fifo_sample_size);
2780         nvi_reset(st, true, false);
2781         nvi_mutex_unlock(st);
2782         return IRQ_HANDLED;
2783 }
2784
2785 static irqreturn_t nvi_irq_handler(int irq, void *dev_id)
2786 {
2787         struct nvi_state *st = (struct nvi_state *)dev_id;
2788         u64 ts = nvs_timestamp();
2789         u64 ts_old = atomic64_xchg(&st->ts_irq, ts);
2790         u64 ts_diff = ts - ts_old;
2791
2792         /* test for MPU IRQ storm problem */
2793         if (ts_diff < NVI_IRQ_STORM_MIN_NS) {
2794                 st->irq_storm_n++;
2795                 if (st->irq_storm_n > NVI_IRQ_STORM_MAX_N)
2796                         nvi_disable_irq(st);
2797         } else {
2798                 st->irq_storm_n = 0;
2799         }
2800
2801         if (st->sts & NVS_STS_SPEW_IRQ)
2802                 dev_info(&st->i2c->dev, "%s ts=%llu ts_diff=%llu irq_dis=%x\n",
2803                          __func__, ts, ts_diff, st->irq_dis);
2804         return IRQ_WAKE_THREAD;
2805 }
2806
2807 static int nvi_enable(void *client, int snsr_id, int enable)
2808 {
2809         struct nvi_state *st = (struct nvi_state *)client;
2810
2811         if (enable < 0)
2812                 return st->enabled[snsr_id];
2813
2814         st->enabled[snsr_id] = enable;
2815         return nvi_en(st);
2816 }
2817
2818 static int nvi_batch(void *client, int snsr_id, int flags,
2819                      unsigned int period, unsigned int timeout)
2820 {
2821         struct nvi_state *st = (struct nvi_state *)client;
2822         unsigned int old;
2823         int ret = 0;
2824
2825         if (timeout)
2826                 /* timeout not supported at this time */
2827                 return -EINVAL;
2828
2829         old = st->delay_us[snsr_id];
2830         st->delay_us[snsr_id] = period;
2831         if (st->enabled[snsr_id]) {
2832                 ret = nvi_en(st);
2833                 if (ret)
2834                         st->delay_us[snsr_id] = old;
2835         }
2836         return ret;
2837 }
2838
2839 static int nvi_flush(void *client, int snsr_id)
2840 {
2841         struct nvi_state *st = (struct nvi_state *)client;
2842         int ret = -EINVAL;
2843
2844         if (st->enabled[snsr_id]) {
2845                 st->flush[snsr_id] = true;
2846                 ret = nvi_en(st);
2847         }
2848         return ret;
2849 }
2850
2851 static int nvi_max_range(void *client, int snsr_id, int max_range)
2852 {
2853         struct nvi_state *st = (struct nvi_state *)client;
2854         unsigned int i = max_range;
2855         unsigned int axis;
2856         unsigned int old;
2857         int ret;
2858
2859         switch (snsr_id) {
2860         case DEV_ACCEL:
2861                 if (i >= NUM_ACCEL_FSR)
2862                         return -EINVAL;
2863
2864                 old = st->chip_config.accel_fs;
2865                 st->chip_config.accel_fs = i;
2866                 if (st->enabled[DEV_ACCEL]) {
2867                         ret = nvi_en(st);
2868                         if (ret) {
2869                                 st->chip_config.accel_fs = old;
2870                                 return -EINVAL;
2871                         }
2872                 }
2873                 break;
2874
2875         case DEV_ANGLVEL:
2876                 if (i >= NUM_FSR)
2877                         return -EINVAL;
2878
2879                 old = st->chip_config.fsr;
2880                 st->chip_config.fsr = i;
2881                 if (st->enabled[DEV_ANGLVEL]) {
2882                         ret = nvi_en(st);
2883                         if (ret) {
2884                                 st->chip_config.fsr = old;
2885                                 return -EINVAL;
2886                         }
2887                 }
2888                 break;
2889
2890         case DEV_TEMP:
2891                 if (i)
2892                         return -EINVAL;
2893
2894                 st->cfg[DEV_TEMP].offset.ival =
2895                                            st->hal->dev[DEV_TEMP]->offset.ival;
2896                 st->cfg[DEV_TEMP].offset.fval =
2897                                            st->hal->dev[DEV_TEMP]->offset.fval;
2898                 st->cfg[DEV_TEMP].scale.ival =
2899                                             st->hal->dev[DEV_TEMP]->scale.ival;
2900                 st->cfg[DEV_TEMP].scale.fval =
2901                                             st->hal->dev[DEV_TEMP]->scale.fval;
2902                 break;
2903
2904         default:
2905                 return -EINVAL;
2906         }
2907
2908         if (snsr_id != DEV_TEMP) {
2909                 for (axis = 0; axis < AXIS_N; axis++) {
2910                         st->cfg[snsr_id].scales[axis].ival =
2911                                   st->hal->dev[snsr_id]->rr[i].resolution.ival;
2912                         st->cfg[snsr_id].scales[axis].fval =
2913                                   st->hal->dev[snsr_id]->rr[i].resolution.fval;
2914                 }
2915         }
2916         st->cfg[snsr_id].resolution.ival =
2917                                   st->hal->dev[snsr_id]->rr[i].resolution.ival;
2918         st->cfg[snsr_id].resolution.fval =
2919                                   st->hal->dev[snsr_id]->rr[i].resolution.fval;
2920         st->cfg[snsr_id].max_range.ival =
2921                                    st->hal->dev[snsr_id]->rr[i].max_range.ival;
2922         st->cfg[snsr_id].max_range.fval =
2923                                    st->hal->dev[snsr_id]->rr[i].max_range.fval;
2924         return 0;
2925 }
2926
2927 static int nvi_offset(void *client, int snsr_id, int channel, int offset)
2928 {
2929         struct nvi_state *st = (struct nvi_state *)client;
2930         int old;
2931         int ret;
2932
2933         switch (snsr_id) {
2934         case DEV_ACCEL:
2935                 if (channel < 0)
2936                         return -EINVAL;
2937
2938                 old = st->input_accel_offset[channel];
2939                 st->input_accel_offset[channel] = offset;
2940                 if (st->master_enable & (1 << DEV_ACCEL)) {
2941                         ret = nvi_en(st);
2942                         if (ret) {
2943                                 st->input_accel_offset[channel] = old;
2944                                 return -EINVAL;
2945                         }
2946                 }
2947                 break;
2948
2949         case DEV_ANGLVEL:
2950                 if (channel < 0)
2951                         return -EINVAL;
2952
2953                 old = st->input_gyro_offset[channel];
2954                 st->input_gyro_offset[channel] = offset;
2955                 if (st->master_enable & (1 << DEV_ANGLVEL)) {
2956                         ret = nvi_en(st);
2957                         if (ret) {
2958                                 st->input_gyro_offset[channel] = old;
2959                                 return -EINVAL;
2960                         }
2961                 }
2962                 break;
2963
2964         case DEV_TEMP:
2965                 st->cfg[DEV_TEMP].offset.ival = offset;
2966                 break;
2967
2968         default:
2969                 return -EINVAL;
2970         }
2971
2972         return 0;
2973 }
2974
2975 static int nvi_reset_dev(void *client, int snsr_id)
2976 {
2977         struct nvi_state *st = (struct nvi_state *)client;
2978         int ret;
2979
2980         ret = nvi_pm(st, NVI_PM_ON);
2981         ret |= nvi_wr_pwr_mgmt_1(st, BIT_H_RESET);
2982         ret |= nvi_en(st);
2983         return ret;
2984 }
2985
2986 static int nvi_self_test(void *client, int snsr_id, char *buf)
2987 {
2988         struct nvi_state *st = (struct nvi_state *)client;
2989         int ret = 0;
2990
2991         if (snsr_id != DEV_TEMP) {
2992                 nvi_aux_enable(st, false);
2993                 ret = inv_hw_self_test(st, snsr_id);
2994                 nvi_aux_enable(st, true);
2995                 if (ret)
2996                         return sprintf(buf, "%d   FAIL\n", ret);
2997         }
2998
2999         return sprintf(buf, "%d   PASS\n", ret);
3000 }
3001
3002 static int nvi_regs(void *client, int snsr_id, char *buf)
3003 {
3004         struct nvi_state *st = (struct nvi_state *)client;
3005         ssize_t t;
3006         u8 data;
3007         unsigned int i;
3008         unsigned int j;
3009         int ret;
3010
3011         t = sprintf(buf, "registers: (only data != 0 shown)\n");
3012         for (j = 0; j < st->hal->reg_bank_n; j++) {
3013                 t += sprintf(buf + t, "bank %u:\n", j);
3014                 for (i = 0; i < st->hal->regs_n; i++) {
3015                         if ((j == st->hal->reg->fifo_r_w.bank) &&
3016                                              (i == st->hal->reg->fifo_r_w.reg))
3017                                 continue;
3018
3019                         ret = nvi_i2c_rd(st, j, i, 1, &data);
3020                         if (ret)
3021                                 t += sprintf(buf + t, "%#2x=ERR\n", i);
3022                         else if (data)
3023                                 t += sprintf(buf + t,
3024                                              "%#2x=%#2x\n", i, data);
3025                 }
3026         }
3027         return t;
3028 }
3029
3030 static int nvi_nvs_write(void *client, int snsr_id, unsigned int nvs)
3031 {
3032         struct nvi_state *st = (struct nvi_state *)client;
3033
3034         st->info = nvs;
3035         switch (nvs) {
3036         case NVI_INFO_VER:
3037                 st->dbg = 0;
3038                 break;
3039
3040         case NVI_INFO_DBG:
3041                 st->dbg ^= NVI_DBG_SPEW_MSG;
3042                 break;
3043
3044         case NVI_INFO_AUX_SPEW:
3045                 st->dbg ^= NVI_DBG_SPEW_AUX;
3046                 nvi_aux_dbg(st, "SNAPSHOT", 0);
3047                 break;
3048
3049         case NVI_INFO_FIFO_SPEW:
3050                 st->dbg ^= NVI_DBG_SPEW_FIFO;
3051                 break;
3052
3053         case NVI_INFO_FIFO_BUF:
3054                 st->dbg ^= NVI_DBG_SPEW_BUF;
3055                 break;
3056
3057         default:
3058                 return -EINVAL;
3059         }
3060
3061         return 0;
3062
3063 }
3064
3065 static int nvi_nvs_read(void *client, int snsr_id, char *buf)
3066 {
3067         struct nvi_state *st = (struct nvi_state *)client;
3068         enum NVI_INFO info;
3069         ssize_t t;
3070
3071         info = st->info;
3072         st->info = NVI_INFO_VER;
3073         switch (info) {
3074         case NVI_INFO_VER:
3075                 t = sprintf(buf, "NVI driver v. %u\n", NVI_DRIVER_VERSION);
3076                 t += sprintf(buf + t, "standby_en=%x\n",
3077                              !!(st->master_enable & (1 << EN_STDBY)));
3078                 t += sprintf(buf + t, "lpa_delay_us=%u\n",
3079                              st->chip_config.lpa_delay_us);
3080                 t += sprintf(buf + t, "gyro_start_delay_ns=%lld\n",
3081                              st->chip_config.gyro_start_delay_ns);
3082                 t += sprintf(buf + t, "bypass_timeout_ms=%u\n",
3083                              st->chip_config.bypass_timeout_ms);
3084                 t += sprintf(buf + t, "temp_fifo_en=%u\n",
3085                              st->chip_config.temp_fifo_en);
3086                 t += sprintf(buf + t, "fifo_threshold=%u\n",
3087                              st->chip_config.fifo_thr);
3088                 return t;
3089
3090         case NVI_INFO_DBG:
3091                 return sprintf(buf, "DBG spew=%x\n",
3092                                !!(st->dbg & NVI_DBG_SPEW_MSG));
3093
3094         case NVI_INFO_AUX_SPEW:
3095                 return sprintf(buf, "AUX spew=%x\n",
3096                                !!(st->dbg & NVI_DBG_SPEW_AUX));
3097
3098         case NVI_INFO_FIFO_SPEW:
3099                 return sprintf(buf, "FIFO spew=%x\n",
3100                                !!(st->dbg & NVI_DBG_SPEW_FIFO));
3101
3102         case NVI_INFO_FIFO_BUF:
3103                 return sprintf(buf, "BUF spew=%x\n",
3104                                !!(st->dbg & NVI_DBG_SPEW_BUF));
3105
3106         default:
3107                 break;
3108         }
3109
3110         return -EINVAL;
3111 }
3112
3113 static struct nvs_fn_dev nvi_fn_dev = {
3114         .enable                         = nvi_enable,
3115         .batch                          = nvi_batch,
3116         .flush                          = nvi_flush,
3117         .max_range                      = nvi_max_range,
3118         .offset                         = nvi_offset,
3119         .reset                          = nvi_reset_dev,
3120         .self_test                      = nvi_self_test,
3121         .regs                           = nvi_regs,
3122         .nvs_write                      = nvi_nvs_write,
3123         .nvs_read                       = nvi_nvs_read,
3124 };
3125
3126 static int nvi_suspend(struct device *dev)
3127 {
3128         struct i2c_client *client = to_i2c_client(dev);
3129         struct nvi_state *st = i2c_get_clientdata(client);
3130         unsigned int i;
3131         int ret = 0;
3132
3133         st->sts |= NVS_STS_SUSPEND;
3134         if (st->nvs) {
3135                 for (i = 0; i < DEV_N; i++) {
3136                         if (st->nvs_st[i])
3137                                 ret |= st->nvs->suspend(st->nvs_st[i]);
3138                 }
3139         }
3140         if (st->sts & NVS_STS_SPEW_MSG)
3141                 dev_info(&client->dev, "%s err=%d\n", __func__, ret);
3142         return ret;
3143 }
3144
3145 static int nvi_resume(struct device *dev)
3146 {
3147         struct i2c_client *client = to_i2c_client(dev);
3148         struct nvi_state *st = i2c_get_clientdata(client);
3149         unsigned int i;
3150         int ret = 0;
3151
3152         nvi_mutex_lock(st);
3153         for (i = 0; i < AUX_PORT_MAX; i++) {
3154                 if (st->aux.port[i].nmp.shutdown_bypass)
3155                         break;
3156         }
3157         if (i < AUX_PORT_MAX) {
3158                 nvi_pm(st, NVI_PM_ON);
3159                 nvi_aux_bypass_enable(st, false);
3160         }
3161         nvi_mutex_unlock(st);
3162         if (st->nvs) {
3163                 for (i = 0; i < DEV_N; i++) {
3164                         if (st->nvs_st[i])
3165                                 ret |= st->nvs->resume(st->nvs_st[i]);
3166                 }
3167         }
3168         st->sts &= ~NVS_STS_SUSPEND;
3169         if (st->sts & NVS_STS_SPEW_MSG)
3170                 dev_info(&client->dev, "%s err=%d\n", __func__, ret);
3171         return ret;
3172 }
3173
3174 static const struct dev_pm_ops nvi_pm_ops = {
3175         .suspend = nvi_suspend,
3176         .resume = nvi_resume,
3177 };
3178
3179 static void nvi_shutdown(struct i2c_client *client)
3180 {
3181         struct nvi_state *st = i2c_get_clientdata(client);
3182         unsigned int i;
3183
3184         st->sts |= NVS_STS_SHUTDOWN;
3185         if (st->nvs) {
3186                 for (i = 0; i < DEV_N; i++) {
3187                         if (st->nvs_st[i])
3188                                 st->nvs->shutdown(st->nvs_st[i]);
3189                 }
3190         }
3191         nvi_disable_irq(st);
3192         if (st->hal)
3193                 nvi_pm(st, NVI_PM_OFF);
3194         if (st->sts & NVS_STS_SPEW_MSG)
3195                 dev_info(&client->dev, "%s\n", __func__);
3196 }
3197
3198 static int nvi_remove(struct i2c_client *client)
3199 {
3200         struct nvi_state *st = i2c_get_clientdata(client);
3201         unsigned int i;
3202
3203         if (st != NULL) {
3204                 nvi_shutdown(client);
3205                 if (st->nvs) {
3206                         for (i = 0; i < DEV_N; i++) {
3207                                 if (st->nvs_st[i])
3208                                         st->nvs->remove(st->nvs_st[i]);
3209                         }
3210                 }
3211                 nvi_pm_exit(st);
3212         }
3213         dev_info(&client->dev, "%s\n", __func__);
3214         return 0;
3215 }
3216
3217 static const unsigned int nvi_lpf_us_tbl_6050[] = {
3218         0, /* WAR: disabled 3906, 256Hz */
3219         5319,   /* 188Hz */
3220         10204,  /* 98Hz */
3221         23810,  /* 42Hz */
3222         50000,  /* 20Hz */
3223         100000, /* 10Hz */
3224         /* 200000, 5Hz */
3225 };
3226
3227 static const unsigned long nvi_lpa_delay_us_tbl_6050[] = {
3228         800000, /* 800ms */
3229         200000, /* 200ms */
3230         50000,  /* 50ms */
3231         /* 25000, 25ms */
3232 };
3233
3234 static const unsigned int smplrt_6050_dev_delays[] = {
3235         DEV_ACCEL,
3236         DEV_ANGLVEL,
3237         DEV_TEMP,
3238         DEV_AUX,
3239 };
3240
3241 static const struct nvi_smplrt smplrt_6050 = {
3242         .dev                            = DEV_ACCEL,
3243         .delay_us_min                   = 10000,
3244         .delay_us_max                   = 256000,
3245         .delay_us_dflt                  = 100000,
3246         .dev_delays_n                   = ARRAY_SIZE(smplrt_6050_dev_delays),
3247         .dev_delays                     = smplrt_6050_dev_delays,
3248         .lpf_us_tbl_n                   = ARRAY_SIZE(nvi_lpf_us_tbl_6050),
3249         .lpf_us_tbl                     = nvi_lpf_us_tbl_6050,
3250         .base_hz                        = 1000,
3251         .lpf_wr                         = nvi_wr_gyro_config,
3252 };
3253
3254 static struct nvi_rr nvi_rr_accel[] = {
3255         /* all accelerometer values are in g's  fval = NVS_FLOAT_NANO */
3256         {
3257                 .max_range              = {
3258                         .ival           = 19,
3259                         .fval           = 613300000,
3260                 },
3261                 .resolution             = {
3262                         .ival           = 0,
3263                         .fval           = 598550,
3264                 },
3265         },
3266         {
3267                 .max_range              = {
3268                         .ival           = 39,
3269                         .fval           = 226600000,
3270                 },
3271                 .resolution             = {
3272                         .ival           = 0,
3273                         .fval           = 1197101,
3274                 },
3275         },
3276         {
3277                 .max_range              = {
3278                         .ival           = 78,
3279                         .fval           = 453200000,
3280                 },
3281                 .resolution             = {
3282                         .ival           = 0,
3283                         .fval           = 2394202,
3284                 },
3285         },
3286         {
3287                 .max_range              = {
3288                         .ival           = 156,
3289                         .fval           = 906400000,
3290                 },
3291                 .resolution             = {
3292                         .ival           = 0,
3293                         .fval           = 4788403,
3294                 },
3295         },
3296 };
3297
3298 static struct nvi_rr nvi_rr_anglvel[] = {
3299         /* rad / sec  fval = NVS_FLOAT_NANO */
3300         {
3301                 .max_range              = {
3302                         .ival           = 4,
3303                         .fval           = 363323130,
3304                 },
3305                 .resolution             = {
3306                         .ival           = 0,
3307                         .fval           = 133231,
3308                 },
3309         },
3310         {
3311                 .max_range              = {
3312                         .ival           = 8,
3313                         .fval           = 726646260,
3314                 },
3315                 .resolution             = {
3316                         .ival           = 0,
3317                         .fval           = 266462,
3318                 },
3319         },
3320         {
3321                 .max_range              = {
3322                         .ival           = 17,
3323                         .fval           = 453292520,
3324                 },
3325                 .resolution             = {
3326                         .ival           = 0,
3327                         .fval           = 532113,
3328                 },
3329         },
3330         {
3331                 .max_range              = {
3332                         .ival           = 34,
3333                         .fval           = 906585040,
3334                 },
3335                 .resolution             = {
3336                         .ival           = 0,
3337                         .fval           = 1064225,
3338                 },
3339         },
3340 };
3341
3342 static struct nvi_rr nvi_rr_temp[] = {
3343         {
3344                 .max_range              = {
3345                         .ival           = 125,
3346                         .fval           = 0,
3347                 },
3348                 .resolution             = {
3349                         .ival           = 1,
3350                         .fval           = 0,
3351                 },
3352         },
3353 };
3354
3355 static const struct nvi_hal_dev nvi_hal_6050_accel = {
3356         .version                        = 3,
3357         .selftest_scale                 = 8,
3358         .rr                             = nvi_rr_accel,
3359         .milliamp                       = {
3360                 .ival                   = 0,
3361                 .fval                   = 500000000, /* NVS_FLOAT_NANO */
3362         },
3363 };
3364
3365 static const struct nvi_hal_dev nvi_hal_6050_anglvel = {
3366         .version                        = 3,
3367         .selftest_scale                 = 250,
3368         .rr                             = nvi_rr_anglvel,
3369         .milliamp                       = {
3370                 .ival                   = 3,
3371                 .fval                   = 700000000, /* NVS_FLOAT_NANO */
3372         },
3373 };
3374
3375 static const struct nvi_hal_dev nvi_hal_6050_temp = {
3376         .version                        = 2,
3377         .rr                             = nvi_rr_temp,
3378         .scale                          = {
3379                 .ival                   = 0,
3380                 .fval                   = 315806400, /* NVS_FLOAT_MICRO */
3381         },
3382         .offset                         = {
3383                 .ival                   = 0,
3384                 .fval                   = 239418400, /* NVS_FLOAT_MICRO */
3385         },
3386         .milliamp                       = {
3387                 .ival                   = 3,
3388                 .fval                   = 700000000, /* NVS_FLOAT_MICRO */
3389         },
3390 };
3391
3392 static const struct nvi_hal_reg nvi_hal_reg_6050 = {
3393         .a_offset_h[AXIS_X]             = {
3394                 .reg                    = 0x06,
3395         },
3396         .a_offset_h[AXIS_Y]             = {
3397                 .reg                    = 0x08,
3398         },
3399         .a_offset_h[AXIS_Z]             = {
3400                 .reg                    = 0x0A,
3401         },
3402         .xg_offset_h                    = {
3403                 .reg                    = 0x13,
3404         },
3405         .smplrt_div[DEV_ACCEL]          = {
3406                 .reg                    = 0x19,
3407         },
3408         .gyro_config1                   = {
3409                 .reg                    = 0x1A,
3410         },
3411         .gyro_config2                   = {
3412                 .reg                    = 0x1B,
3413         },
3414         .accel_config                   = {
3415                 .reg                    = 0x1C,
3416         },
3417         .accel_config2                  = {
3418                 .reg                    = 0x1D,
3419         },
3420         .lp_config                      = {
3421                 .reg                    = 0x1E,
3422         },
3423         .fifo_en                        = {
3424                 .reg                    = 0x23,
3425                 .dflt                   = 0x4000,
3426         },
3427         .i2c_mst_ctrl                   = {
3428                 .reg                    = 0x24,
3429         },
3430         .i2c_slv0_addr                  = {
3431                 .reg                    = 0x25,
3432         },
3433         .i2c_slv0_reg                   = {
3434                 .reg                    = 0x26,
3435         },
3436         .i2c_slv0_ctrl                  = {
3437                 .reg                    = 0x27,
3438         },
3439         .i2c_slv4_ctrl                  = {
3440                 .reg                    = 0x34,
3441         },
3442         .i2c_slv4_di                    = {
3443                 .reg                    = 0x35,
3444         },
3445         .i2c_mst_status                 = {
3446                 .reg                    = 0x36,
3447         },
3448         .int_pin_cfg                    = {
3449                 .reg                    = 0x37,
3450         },
3451         .int_enable                     = {
3452                 .reg                    = 0x38,
3453         },
3454         .int_status                     = {
3455                 .reg                    = 0x3A,
3456         },
3457         .accel_xout_h                   = {
3458                 .reg                    = 0x3B,
3459         },
3460         .temp_out_h                     = {
3461                 .reg                    = 0x41,
3462         },
3463         .gyro_xout_h                    = {
3464                 .reg                    = 0x43,
3465         },
3466         .ext_sens_data_00               = {
3467                 .reg                    = 0x49,
3468         },
3469         .i2c_slv0_do                    = {
3470                 .reg                    = 0x63,
3471         },
3472         .i2c_slv4_do                    = {
3473                 .reg                    = 0x33,
3474         },
3475         .i2c_mst_odr_config             = {
3476                 .bank                   = -1,
3477         },
3478         .i2c_mst_delay_ctrl             = {
3479                 .reg                    = 0x67,
3480         },
3481         .signal_path_reset              = {
3482                 .reg                    = 0x68,
3483         },
3484         .accel_intel_ctrl               = {
3485                 .reg                    = 0x69,
3486         },
3487         .user_ctrl                      = {
3488                 .reg                    = 0x6A,
3489         },
3490         .pwr_mgmt_1                     = {
3491                 .reg                    = 0x6B,
3492         },
3493         .pwr_mgmt_2                     = {
3494                 .reg                    = 0x6C,
3495         },
3496         .fifo_count_h                   = {
3497                 .reg                    = 0x72,
3498         },
3499         .fifo_r_w                       = {
3500                 .reg                    = 0x74,
3501         },
3502         .who_am_i                       = {
3503                 .reg                    = 0x75,
3504         },
3505 };
3506
3507 static const struct nvi_hal_bit nvi_hal_bit_6050 = {
3508         .smplrt_div_n[DEV_ACCEL]        = 8,
3509         .i2c_mst_int_en                 = 0,
3510         .dmp_int_en                     = 1,
3511         .pll_rdy_en                     = 7,
3512         .wom_int_en                     = 6,
3513         .reg_wof_en                     = 3,
3514         .raw_data_0_rdy_en              = 0,
3515         .raw_data_1_rdy_en              = 0,
3516         .raw_data_2_rdy_en              = 0,
3517         .raw_data_3_rdy_en              = 0,
3518         .fifo_overflow_en               = 4,
3519         .fifo_wm_en                     = 6,
3520         .bit_int_enable_max             = 8,
3521         .slv_fifo_en[0]                 = 0,
3522         .slv_fifo_en[1]                 = 1,
3523         .slv_fifo_en[2]                 = 2,
3524         .slv_fifo_en[3]                 = 13,
3525         .temp_fifo_en                   = 7,
3526         .gyro_x_fifo_en                 = 6,
3527         .gyro_y_fifo_en                 = 5,
3528         .gyro_z_fifo_en                 = 4,
3529         .accel_fifo_en                  = 3,
3530         .bit_fifo_en_max                = 16,
3531 };
3532
3533 static unsigned int (*fifo_read_6050[])(struct nvi_state *st,
3534                                         unsigned int buf_i, s64 ts) = {
3535         nvi_fifo_read_accel,
3536         nvi_fifo_read_temp,
3537         nvi_fifo_read_anglvel,
3538 };
3539
3540 static const struct nvi_hal nvi_hal_6050 = {
3541         .part                           = MPU6050,
3542         .regs_n                         = 118,
3543         .reg_bank_n                     = 1,
3544         .fifo_size                      = 1024,
3545         .lpa_tbl                        = nvi_lpa_delay_us_tbl_6050,
3546         .lpa_tbl_n                     = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6050),
3547         .smplrt[DEV_ACCEL]              = &smplrt_6050,
3548         .smplrt[DEV_ANGLVEL]            = &smplrt_6050,
3549         .smplrt[DEV_TEMP]               = &smplrt_6050,
3550         .smplrt[DEV_AUX]                = &smplrt_6050,
3551         .dev[DEV_ACCEL]                 = &nvi_hal_6050_accel,
3552         .dev[DEV_ANGLVEL]               = &nvi_hal_6050_anglvel,
3553         .dev[DEV_TEMP]                  = &nvi_hal_6050_temp,
3554         .reg                            = &nvi_hal_reg_6050,
3555         .bit                            = &nvi_hal_bit_6050,
3556         .fifo_read_n                    = ARRAY_SIZE(fifo_read_6050),
3557         .fifo_read                      = fifo_read_6050,
3558         .init                           = inv_get_silicon_rev_mpu6050,
3559 };
3560
3561 static const unsigned long nvi_lpa_delay_us_tbl_6500[] = {
3562         4096000,/* 4096ms */
3563         2048000,/* 2048ms */
3564         1024000,/* 1024ms */
3565         512000, /* 512ms */
3566         256000, /* 256ms */
3567         128000, /* 128ms */
3568         64000,  /* 64ms */
3569         32000,  /* 32ms */
3570         16000,  /* 16ms */
3571         8000,   /* 8ms */
3572         4000,   /* 4ms */
3573         /* 2000, 2ms */
3574 };
3575
3576 static const struct nvi_hal_dev nvi_hal_6500_accel = {
3577         .version                        = 3,
3578         .selftest_scale                 = 2,
3579         .rr                             = nvi_rr_accel,
3580         .milliamp                       = {
3581                 .ival                   = 0,
3582                 .fval                   = 500000000, /* NVS_FLOAT_NANO */
3583         },
3584 };
3585
3586 static const struct nvi_hal_dev nvi_hal_6500_temp = {
3587         .version                        = 2,
3588         .rr                             = nvi_rr_temp,
3589         .scale                          = {
3590                 .ival                   = 0,
3591                 .fval                   = 334082700, /* NVS_FLOAT_NANO */
3592         },
3593         .offset                         = {
3594                 .ival                   = 0,
3595                 .fval                   = 137625600, /* NVS_FLOAT_NANO */
3596         },
3597         .milliamp                       = {
3598                 .ival                   = 3,
3599                 .fval                   = 700000000, /* NVS_FLOAT_NANO */
3600         },
3601 };
3602
3603 static const struct nvi_hal_reg nvi_hal_reg_6500 = {
3604         .a_offset_h[AXIS_X]             = {
3605                 .reg                    = 0x77,
3606         },
3607         .a_offset_h[AXIS_Y]             = {
3608                 .reg                    = 0x7A,
3609         },
3610         .a_offset_h[AXIS_Z]             = {
3611                 .reg                    = 0x7D,
3612         },
3613         .xg_offset_h                    = {
3614                 .reg                    = 0x13,
3615         },
3616         .smplrt_div[DEV_ACCEL]          = {
3617                 .reg                    = 0x19,
3618         },
3619         .gyro_config1                   = {
3620                 .reg                    = 0x1A,
3621         },
3622         .gyro_config2                   = {
3623                 .reg                    = 0x1B,
3624         },
3625         .accel_config                   = {
3626                 .reg                    = 0x1C,
3627         },
3628         .accel_config2                  = {
3629                 .reg                    = 0x1D,
3630         },
3631         .lp_config                      = {
3632                 .reg                    = 0x1E,
3633         },
3634         .fifo_en                        = {
3635                 .reg                    = 0x23,
3636                 .dflt                   = 0x4000,
3637         },
3638         .i2c_mst_ctrl                   = {
3639                 .reg                    = 0x24,
3640         },
3641         .i2c_slv0_addr                  = {
3642                 .reg                    = 0x25,
3643         },
3644         .i2c_slv0_reg                   = {
3645                 .reg                    = 0x26,
3646         },
3647         .i2c_slv0_ctrl                  = {
3648                 .reg                    = 0x27,
3649         },
3650         .i2c_slv4_ctrl                  = {
3651                 .reg                    = 0x34,
3652         },
3653         .i2c_slv4_di                    = {
3654                 .reg                    = 0x35,
3655         },
3656         .i2c_mst_status                 = {
3657                 .reg                    = 0x36,
3658         },
3659         .int_pin_cfg                    = {
3660                 .reg                    = 0x37,
3661         },
3662         .int_enable                     = {
3663                 .reg                    = 0x38,
3664         },
3665         .int_status                     = {
3666                 .reg                    = 0x3A,
3667         },
3668         .accel_xout_h                   = {
3669                 .reg                    = 0x3B,
3670         },
3671         .temp_out_h                     = {
3672                 .reg                    = 0x41,
3673         },
3674         .gyro_xout_h                    = {
3675                 .reg                    = 0x43,
3676         },
3677         .ext_sens_data_00               = {
3678                 .reg                    = 0x49,
3679         },
3680         .i2c_slv0_do                    = {
3681                 .reg                    = 0x63,
3682         },
3683         .i2c_slv4_do                    = {
3684                 .reg                    = 0x33,
3685         },
3686         .i2c_mst_odr_config             = {
3687                 .bank                   = -1,
3688         },
3689         .i2c_mst_delay_ctrl             = {
3690                 .reg                    = 0x67,
3691         },
3692         .signal_path_reset              = {
3693                 .reg                    = 0x68,
3694         },
3695         .accel_intel_ctrl               = {
3696                 .reg                    = 0x69,
3697         },
3698         .user_ctrl                      = {
3699                 .reg                    = 0x6A,
3700         },
3701         .pwr_mgmt_1                     = {
3702                 .reg                    = 0x6B,
3703         },
3704         .pwr_mgmt_2                     = {
3705                 .reg                    = 0x6C,
3706         },
3707         .fifo_count_h                   = {
3708                 .reg                    = 0x72,
3709         },
3710         .fifo_r_w                       = {
3711                 .reg                    = 0x74,
3712         },
3713         .who_am_i                       = {
3714                 .reg                    = 0x75,
3715         },
3716 };
3717
3718 static const struct nvi_hal nvi_hal_6500 = {
3719         .part                           = MPU6500,
3720         .regs_n                         = 128,
3721         .reg_bank_n                     = 1,
3722         .fifo_size                      = 4096,
3723         .lpa_tbl                        = nvi_lpa_delay_us_tbl_6500,
3724         .lpa_tbl_n                     = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6500),
3725         .smplrt[DEV_ACCEL]              = &smplrt_6050,
3726         .smplrt[DEV_ANGLVEL]            = &smplrt_6050,
3727         .smplrt[DEV_TEMP]               = &smplrt_6050,
3728         .smplrt[DEV_AUX]                = &smplrt_6050,
3729         .dev[DEV_ACCEL]                 = &nvi_hal_6500_accel,
3730         .dev[DEV_ANGLVEL]               = &nvi_hal_6050_anglvel,
3731         .dev[DEV_TEMP]                  = &nvi_hal_6500_temp,
3732         .reg                            = &nvi_hal_reg_6500,
3733         .bit                            = &nvi_hal_bit_6050,
3734         .fifo_read_n                    = ARRAY_SIZE(fifo_read_6050),
3735         .fifo_read                      = fifo_read_6050,
3736         .init                           = inv_get_silicon_rev_mpu6500,
3737 };
3738
3739 static const struct nvi_hal nvi_hal_6515 = {
3740         .part                           = MPU6515,
3741         .regs_n                         = 128,
3742         .reg_bank_n                     = 1,
3743         .fifo_size                      = 4096,
3744         .lpa_tbl                        = nvi_lpa_delay_us_tbl_6500,
3745         .lpa_tbl_n                     = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6500),
3746         .smplrt[DEV_ACCEL]              = &smplrt_6050,
3747         .smplrt[DEV_ANGLVEL]            = &smplrt_6050,
3748         .smplrt[DEV_TEMP]               = &smplrt_6050,
3749         .smplrt[DEV_AUX]                = &smplrt_6050,
3750         .dev[DEV_ACCEL]                 = &nvi_hal_6500_accel,
3751         .dev[DEV_ANGLVEL]               = &nvi_hal_6050_anglvel,
3752         .dev[DEV_TEMP]                  = &nvi_hal_6500_temp,
3753         .reg                            = &nvi_hal_reg_6500,
3754         .bit                            = &nvi_hal_bit_6050,
3755         .fifo_read_n                    = ARRAY_SIZE(fifo_read_6050),
3756         .fifo_read                      = fifo_read_6050,
3757         .init                           = inv_get_silicon_rev_mpu6500,
3758 };
3759
3760 static const unsigned int smplrt_20628_accel_dd[] = {
3761         DEV_ACCEL,
3762         DEV_AUX,
3763 };
3764
3765 static const struct nvi_smplrt smplrt_20628_accel = {
3766         .dev                            = DEV_ACCEL,
3767         .delay_us_min                   = 10000,
3768         .delay_us_max                   = 256000,
3769         .delay_us_dflt                  = 100000,
3770         .dev_delays_n                   = ARRAY_SIZE(smplrt_20628_accel_dd),
3771         .dev_delays                     = smplrt_20628_accel_dd,
3772         .base_hz                        = 1125,
3773         .lpf_wr                         = nvi_wr_accel_config,
3774 };
3775
3776 static const unsigned int smplrt_20628_anglvel_dd[] = {
3777         DEV_ANGLVEL,
3778         DEV_TEMP,
3779 };
3780
3781 static const struct nvi_smplrt smplrt_20628_anglvel = {
3782         .dev                            = DEV_ANGLVEL,
3783         .delay_us_min                   = 10000,
3784         .delay_us_max                   = 256000,
3785         .delay_us_dflt                  = 100000,
3786         .dev_delays_n                   = ARRAY_SIZE(smplrt_20628_anglvel_dd),
3787         .dev_delays                     = smplrt_20628_anglvel_dd,
3788         .base_hz                        = 1125,
3789         .lpf_wr                         = nvi_wr_gyro_config,
3790 };
3791
3792 static const unsigned int smplrt_20628_aux_dd[] = {
3793         DEV_AUX,
3794 };
3795
3796 static const struct nvi_smplrt smplrt_20628_aux = {
3797         .dev                            = DEV_AUX,
3798         .delay_us_min                   = 10000,
3799         .delay_us_max                   = 256000,
3800         .delay_us_dflt                  = 100000,
3801         .dev_delays_n                   = ARRAY_SIZE(smplrt_20628_aux_dd),
3802         .dev_delays                     = smplrt_20628_aux_dd,
3803         .base_hz                        = 1125,
3804         .lpf_wr                         = nvi_wr_gyro_config,
3805 };
3806
3807 static const struct nvi_hal_reg nvi_hal_reg_20628 = {
3808 /* register bank 0 */
3809         .lp_config                      = {
3810                 .bank                   = 0,
3811                 .reg                    = 0x05,
3812                 .dflt                   = 0x70,
3813         },
3814         .fifo_en                        = {
3815                 .bank                   = 0,
3816                 .reg                    = 0x66,
3817         },
3818         .i2c_mst_status                 = {
3819                 .bank                   = 0,
3820                 .reg                    = 0x17,
3821         },
3822         .int_pin_cfg                    = {
3823                 .bank                   = 0,
3824                 .reg                    = 0x0F,
3825         },
3826         .int_enable                     = {
3827                 .bank                   = 0,
3828                 .reg                    = 0x10,
3829         },
3830         .int_status                     = {
3831                 .bank                   = 0,
3832                 .reg                    = 0x19,
3833         },
3834         .accel_xout_h                   = {
3835                 .bank                   = 0,
3836                 .reg                    = 0x2D,
3837         },
3838         .temp_out_h                     = {
3839                 .bank                   = 0,
3840                 .reg                    = 0x39,
3841         },
3842         .gyro_xout_h                    = {
3843                 .bank                   = 0,
3844                 .reg                    = 0x33,
3845         },
3846         .ext_sens_data_00               = {
3847                 .bank                   = 0,
3848                 .reg                    = 0x3B,
3849         },
3850         .signal_path_reset              = {
3851                 .bank                   = 0,
3852                 .reg                    = 0x04,
3853         },
3854         .user_ctrl                      = {
3855                 .bank                   = 0,
3856                 .reg                    = 0x03,
3857         },
3858         .pwr_mgmt_1                     = {
3859                 .bank                   = 0,
3860                 .reg                    = 0x06,
3861                 .dflt                   = 0x01,
3862         },
3863         .pwr_mgmt_2                     = {
3864                 .bank                   = 0,
3865                 .reg                    = 0x07,
3866                 .dflt                   = 0x40,
3867         },
3868         .fifo_count_h                   = {
3869                 .bank                   = 0,
3870                 .reg                    = 0x70,
3871         },
3872         .fifo_r_w                       = {
3873                 .bank                   = 0,
3874                 .reg                    = 0x72,
3875         },
3876         .who_am_i                       = {
3877                 .bank                   = 0,
3878                 .reg                    = 0x00,
3879         },
3880         .reg_bank_sel                   = {
3881                 .bank                   = 0,
3882                 .reg                    = 0x7F,
3883         },
3884 /* register bank 1 */
3885         .self_test_x_gyro               = {
3886                 .bank                   = 1,
3887                 .reg                    = 0x02,
3888         },
3889         .self_test_y_gyro               = {
3890                 .bank                   = 1,
3891                 .reg                    = 0x03,
3892         },
3893         .self_test_z_gyro               = {
3894                 .bank                   = 1,
3895                 .reg                    = 0x04,
3896         },
3897         .self_test_x_accel              = {
3898                 .bank                   = 1,
3899                 .reg                    = 0x0E,
3900         },
3901         .self_test_y_accel              = {
3902                 .bank                   = 1,
3903                 .reg                    = 0x0F,
3904         },
3905         .self_test_z_accel              = {
3906                 .bank                   = 1,
3907                 .reg                    = 0x10,
3908         },
3909         .a_offset_h[AXIS_X]             = {
3910                 .bank                   = 1,
3911                 .reg                    = 0x14,
3912         },
3913         .a_offset_h[AXIS_Y]             = {
3914                 .bank                   = 1,
3915                 .reg                    = 0x17,
3916         },
3917         .a_offset_h[AXIS_Z]             = {
3918                 .bank                   = 1,
3919                 .reg                    = 0x1A,
3920         },
3921 /* register bank 2 */
3922         .smplrt_div[DEV_ANGLVEL]        = {
3923                 .bank                   = 2,
3924                 .reg                    = 0x00,
3925         },
3926         .gyro_config1                   = {
3927                 .bank                   = 2,
3928                 .reg                    = 0x01,
3929                 .dflt                   = 0x01,
3930         },
3931         .gyro_config2                   = {
3932                 .bank                   = 2,
3933                 .reg                    = 0x02,
3934         },
3935         .xg_offset_h                    = {
3936                 .bank                   = 2,
3937                 .reg                    = 0x03,
3938         },
3939         .smplrt_div[DEV_ACCEL]          = {
3940                 .bank                   = 2,
3941                 .reg                    = 0x10,
3942         },
3943         .accel_intel_ctrl               = {
3944                 .bank                   = 2,
3945                 .reg                    = 0x12,
3946         },
3947         .accel_config                   = {
3948                 .bank                   = 2,
3949                 .reg                    = 0x14,
3950         },
3951         .accel_config2                  = {
3952                 .bank                   = 2,
3953                 .reg                    = 0x15,
3954         },
3955 /* register bank 3 */
3956         .i2c_mst_odr_config             = {
3957                 .bank                   = 3,
3958                 .reg                    = 0x00,
3959         },
3960         .i2c_mst_ctrl                   = {
3961                 .bank                   = 3,
3962                 .reg                    = 0x01,
3963         },
3964         .i2c_mst_delay_ctrl             = {
3965                 .bank                   = 3,
3966                 .reg                    = 0x02,
3967         },
3968         .i2c_slv0_addr                  = {
3969                 .bank                   = 3,
3970                 .reg                    = 0x03,
3971         },
3972         .i2c_slv0_reg                   = {
3973                 .bank                   = 3,
3974                 .reg                    = 0x04,
3975         },
3976         .i2c_slv0_ctrl                  = {
3977                 .bank                   = 3,
3978                 .reg                    = 0x05,
3979         },
3980         .i2c_slv0_do                    = {
3981                 .bank                   = 3,
3982                 .reg                    = 0x06,
3983         },
3984         .i2c_slv4_ctrl                  = {
3985                 .bank                   = 3,
3986                 .reg                    = 0x15,
3987         },
3988         .i2c_slv4_do                    = {
3989                 .bank                   = 3,
3990                 .reg                    = 0x16,
3991         },
3992         .i2c_slv4_di                    = {
3993                 .bank                   = 3,
3994                 .reg                    = 0x17,
3995         },
3996 };
3997
3998 static const struct nvi_hal_bit nvi_hal_bit_20628 = {
3999         .smplrt_div_n[DEV_ACCEL]        = 12,
4000         .smplrt_div_n[DEV_ANGLVEL]      = 8,
4001         .i2c_mst_int_en                 = 0,
4002         .dmp_int_en                     = 1,
4003         .pll_rdy_en                     = 2,
4004         .wom_int_en                     = 3,
4005         .reg_wof_en                     = 7,
4006         .raw_data_0_rdy_en              = 8,
4007         .raw_data_1_rdy_en              = 9,
4008         .raw_data_2_rdy_en              = 10,
4009         .raw_data_3_rdy_en              = 11,
4010         .fifo_overflow_en               = 16,
4011         .fifo_wm_en                     = 24,
4012         .bit_int_enable_max             = 32,
4013         .slv_fifo_en[0]                 = 0,
4014         .slv_fifo_en[1]                 = 1,
4015         .slv_fifo_en[2]                 = 2,
4016         .slv_fifo_en[3]                 = 3,
4017         .temp_fifo_en                   = 8,
4018         .gyro_x_fifo_en                 = 9,
4019         .gyro_y_fifo_en                 = 10,
4020         .gyro_z_fifo_en                 = 11,
4021         .accel_fifo_en                  = 12,
4022         .bit_fifo_en_max                = 16,
4023 };
4024
4025 static unsigned int (*fifo_read_20628[])(struct nvi_state *st,
4026                                          unsigned int buf_i, s64 ts) = {
4027         nvi_fifo_read_accel,
4028         nvi_fifo_read_anglvel,
4029         nvi_fifo_read_temp,
4030 };
4031
4032 static void nvi_por2rc_20628(struct nvi_state *st)
4033 {
4034         st->rc.lp_config = 0x40;
4035         st->rc.pwr_mgmt_1 = 0x41;
4036         st->rc.gyro_config1 = 0x01;
4037         st->rc.accel_config = 0x01;
4038 }
4039
4040 static const struct nvi_hal nvi_hal_20628 = {
4041         .part                           = ICM20628,
4042         .regs_n                         = 128,
4043         .reg_bank_n                     = 4,
4044         .fifo_size                      = 4096,
4045         .lpa_tbl                        = nvi_lpa_delay_us_tbl_6500,
4046         .lpa_tbl_n                     = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6500),
4047         .smplrt[DEV_ACCEL]              = &smplrt_20628_accel,
4048         .smplrt[DEV_ANGLVEL]            = &smplrt_20628_anglvel,
4049         .smplrt[DEV_TEMP]               = &smplrt_20628_anglvel,
4050         .smplrt[DEV_AUX]                = &smplrt_20628_accel,
4051         .dev[DEV_ACCEL]                 = &nvi_hal_6500_accel,
4052         .dev[DEV_ANGLVEL]               = &nvi_hal_6050_anglvel,
4053         .dev[DEV_TEMP]                  = &nvi_hal_6500_temp,
4054         .reg                            = &nvi_hal_reg_20628,
4055         .bit                            = &nvi_hal_bit_20628,
4056         .fifo_read_n                    = ARRAY_SIZE(fifo_read_20628),
4057         .fifo_read                      = fifo_read_20628,
4058         .por2rc                         = nvi_por2rc_20628,
4059         .init                           = inv_icm_init,
4060 };
4061
4062 struct sensor_cfg nvi_cfg_dflt[] = {
4063         {
4064                 .name                   = "accelerometer",
4065                 .snsr_id                = DEV_ACCEL,
4066                 .kbuf_sz                = KBUF_SZ,
4067                 .snsr_data_n            = 7,
4068                 .ch_n                   = AXIS_N,
4069                 .ch_sz                  = -2,
4070                 .vendor                 = NVI_VENDOR,
4071                 .matrix                 = { 1, 0, 0, 0, 1, 0, 0, 0, 1 },
4072                 .float_significance     = NVS_FLOAT_NANO,
4073                 .ch_n_max               = AXIS_N,
4074         },
4075         {
4076                 .name                   = "gyroscope",
4077                 .snsr_id                = DEV_ANGLVEL,
4078                 .kbuf_sz                = KBUF_SZ,
4079                 .snsr_data_n            = 7,
4080                 .ch_n                   = AXIS_N,
4081                 .ch_sz                  = -2,
4082                 .vendor                 = NVI_VENDOR,
4083                 .matrix                 = { 1, 0, 0, 0, 1, 0, 0, 0, 1 },
4084                 .float_significance     = NVS_FLOAT_NANO,
4085                 .ch_n_max               = AXIS_N,
4086         },
4087         {
4088                 .name                   = "gyro_temp",
4089                 .snsr_id                = SENSOR_TYPE_TEMPERATURE,
4090                 .ch_n                   = 1,
4091                 .ch_sz                  = -2,
4092                 .vendor                 = NVI_VENDOR,
4093                 .float_significance     = NVS_FLOAT_NANO,
4094         },
4095 };
4096
4097 static void nvi_init_config(struct nvi_state *st)
4098 {
4099         unsigned int i;
4100
4101         memcpy(st->cfg, nvi_cfg_dflt, sizeof(st->cfg));
4102         st->hal = &nvi_hal_6050;
4103         for (i = 0; i < DEV_N_AUX; i++) {
4104                 st->enabled[i] = 0;
4105                 st->delay_us[i] = 0;
4106         }
4107         st->master_enable = (1 << EN_STDBY);
4108         st->chip_config.lpa_delay_us = 0; /* disabled */
4109         st->chip_config.gyro_start_delay_ns = GYRO_STARTUP_DELAY_NS;
4110         st->chip_config.bypass_timeout_ms = NVI_BYPASS_TIMEOUT_MS;
4111         st->chip_config.temp_fifo_en = 0;
4112         st->chip_config.fifo_thr = FIFO_THRESHOLD;
4113
4114         st->chip_config.fsr = INV_FSR_2000DPS;
4115         st->chip_config.lpf = INV_FS_02G;
4116 }
4117
4118 static int nvi_id_hal(struct nvi_state *st, u8 dev_id)
4119 {
4120         const char *part_name;
4121         unsigned int i;
4122
4123         switch (dev_id) {
4124         case MPU6050_ID:
4125                 st->hal = &nvi_hal_6050;
4126                 part_name = NVI_NAME_MPU6050;
4127                 break;
4128
4129         case MPU6500_ID:
4130                 st->hal = &nvi_hal_6500;
4131                 part_name = NVI_NAME_MPU6500;
4132                 break;
4133
4134         case MPU9250_ID:
4135                 st->hal = &nvi_hal_6500;
4136                 part_name = NVI_NAME_MPU9250;
4137                 break;
4138
4139         case MPU6515_ID:
4140                 st->hal = &nvi_hal_6515;
4141                 part_name = NVI_NAME_MPU6515;
4142                 break;
4143
4144         case MPU9350_ID:
4145                 st->hal = &nvi_hal_6515;
4146                 part_name = NVI_NAME_MPU9350;
4147                 break;
4148
4149         case ICM20628_ID:
4150                 st->hal = &nvi_hal_20628;
4151                 part_name = NVI_NAME_ICM20628;
4152                 break;
4153
4154         case ICM20630_ID:
4155                 st->hal = &nvi_hal_20628;
4156                 part_name = NVI_NAME_ICM20630;
4157                 break;
4158
4159         case ICM20632_ID:
4160                 st->hal = &nvi_hal_20628;
4161                 part_name = NVI_NAME_ICM20632;
4162                 break;
4163
4164         default:
4165                 return -ENODEV;
4166         }
4167
4168         /* populate st->cfg based on max_range setting */
4169         nvi_max_range(st, DEV_ACCEL, INV_FS_02G);
4170         nvi_max_range(st, DEV_ANGLVEL, INV_FSR_2000DPS);
4171         nvi_max_range(st, DEV_TEMP, 0);
4172         /* populate the rest of st->cfg */
4173         for (i = 0; i < DEV_N; i++) {
4174                 st->cfg[i].part = part_name;
4175                 st->cfg[i].version = st->hal->dev[i]->version;
4176                 st->cfg[i].milliamp.ival = st->hal->dev[i]->milliamp.ival;
4177                 st->cfg[i].milliamp.fval = st->hal->dev[i]->milliamp.fval;
4178                 st->cfg[i].delay_us_min = st->hal->smplrt[i]->delay_us_min;
4179                 st->cfg[i].delay_us_max = st->hal->smplrt[i]->delay_us_max;
4180         }
4181         return 0;
4182 }
4183
4184 static int nvi_id_dev(struct nvi_state *st, const char *name)
4185 {
4186         u8 dev_id;
4187         unsigned int i;
4188         int ret;
4189
4190         if (!strcmp(name, "mpu6xxx"))
4191                 dev_id = 0xFF;
4192         else if (!strcmp(name, "mpu6050"))
4193                 dev_id = MPU6050_ID;
4194         else if (!strcmp(name, "mpu6500"))
4195                 dev_id = MPU6500_ID;
4196         else if (!strcmp(name, "mpu6515"))
4197                 dev_id = MPU6515_ID;
4198         else if (!strcmp(name, "mpu9150"))
4199                 dev_id = MPU6050_ID;
4200         else if (!strcmp(name, "mpu9250"))
4201                 dev_id = MPU9250_ID;
4202         else if (!strcmp(name, "mpu9350"))
4203                 dev_id = MPU9350_ID;
4204         else if (!strcmp(name, "icm20628"))
4205                 dev_id = ICM20628_ID;
4206         else if (!strcmp(name, "icm20630"))
4207                 dev_id = ICM20630_ID;
4208         else if (!strcmp(name, "icm20632"))
4209                 dev_id = ICM20632_ID;
4210         else
4211                 return -ENODEV;
4212
4213         if (dev_id == 0xFF) {
4214                 st->hal = &nvi_hal_6050;
4215                 nvi_pm_wr(st, 0, 0, 0);
4216                 ret = nvi_i2c_read(st, st->i2c_addr,
4217                                    nvi_hal_reg_6050.who_am_i.reg, 1, &dev_id);
4218                 if (ret) {
4219                         dev_err(&st->i2c->dev, "%s AUTO ID FAILED\n",
4220                                 __func__);
4221                         return -ENODEV;
4222                 }
4223
4224                 ret = nvi_id_hal(st, dev_id);
4225                 if (ret) {
4226                         st->hal = &nvi_hal_20628;
4227                         /* cause a master reset by disabling regulators */
4228                         nvs_vregs_disable(&st->i2c->dev, st->vreg,
4229                                           ARRAY_SIZE(nvi_vregs));
4230                         ret = nvi_pm_wr(st, 0, 0, 0);
4231                         ret = nvi_i2c_read(st, st->i2c_addr,
4232                                            nvi_hal_reg_20628.who_am_i.reg,
4233                                            1, &dev_id);
4234                         if (ret) {
4235                                 dev_err(&st->i2c->dev, "%s AUTO ID FAILED\n",
4236                                         __func__);
4237                                 return -ENODEV;
4238                         }
4239
4240                         ret = nvi_id_hal(st, dev_id);
4241                         if (ret) {
4242                                 dev_err(&st->i2c->dev, "%s AUTO ID FAILED\n",
4243                                         __func__);
4244                                 return -ENODEV;
4245                         }
4246                 }
4247         } else {
4248                 nvi_id_hal(st, dev_id);
4249                 /* cause a master reset by disabling regulators */
4250                 nvs_vregs_disable(&st->i2c->dev, st->vreg,
4251                                   ARRAY_SIZE(nvi_vregs));
4252                 nvi_pm_wr(st, 0, 0, 0);
4253         }
4254         ret = nvs_vregs_sts(st->vreg, ARRAY_SIZE(nvi_vregs));
4255         if (ret < 0)
4256                 /* regulators aren't supported so manually do master reset */
4257                 nvi_wr_pwr_mgmt_1(st, BIT_H_RESET);
4258         ret = st->hal->init(st);
4259         for (i = 0; i < AXIS_N; i++) {
4260                 st->rom_accel_offset[i] = (s16)st->rc.accel_offset[i];
4261                 st->rom_gyro_offset[i] = (s16)st->rc.gyro_offset[i];
4262                 st->input_accel_offset[i] = 0;
4263                 st->input_gyro_offset[i] = 0;
4264         }
4265         dev_info(&st->i2c->dev, "%s: DT=%s ID=%x USING: %s\n",
4266                  __func__, name, dev_id, st->cfg[0].part);
4267         return ret;
4268 }
4269
4270 static int nvi_id_i2c(struct nvi_state *st, const struct i2c_device_id *id)
4271 {
4272         int i;
4273         int ret;
4274
4275         for (i = 0; i < ARRAY_SIZE(nvi_i2c_addrs); i++) {
4276                 if (st->i2c->addr == nvi_i2c_addrs[i])
4277                         break;
4278         }
4279
4280         if (i < ARRAY_SIZE(nvi_i2c_addrs)) {
4281                 st->i2c_addr = st->i2c->addr;
4282                 ret = nvi_id_dev(st, id->name);
4283         } else {
4284                 for (i = 0; i < ARRAY_SIZE(nvi_i2c_addrs); i++) {
4285                         st->i2c_addr = nvi_i2c_addrs[i];
4286                         ret = nvi_id_dev(st, NVI_NAME);
4287                         if (!ret)
4288                                 break;
4289                 }
4290         }
4291         return ret;
4292 }
4293
4294 static int nvi_of_dt(struct nvi_state *st, struct device_node *dn)
4295 {
4296         u32 tmp;
4297         int ret;
4298
4299         /* just test if global disable */
4300         ret = nvs_of_dt(dn, NULL, NULL);
4301         if (ret == -ENODEV)
4302                 return -ENODEV;
4303
4304         /* device specific parameters */
4305         if (!of_property_read_u32(dn, "invensense,standby_en", &tmp)) {
4306                 if (tmp)
4307                         st->master_enable |= (1 << EN_STDBY);
4308                 else
4309                         st->master_enable &= ~(1 << EN_STDBY);
4310         }
4311         of_property_read_u32(dn, "invensense,lpa_delay_us",
4312                              &st->chip_config.lpa_delay_us);
4313         if (st->chip_config.lpa_delay_us)
4314                 st->master_enable |= (1 << EN_LPA);
4315         else
4316                 st->master_enable &= ~(1 << EN_LPA);
4317         of_property_read_u32(dn, "invensense,gyro_start_delay_ns",
4318                              (u32 *)&st->chip_config.gyro_start_delay_ns);
4319         of_property_read_u32(dn, "invensense,bypass_timeout_ms",
4320                              &st->chip_config.bypass_timeout_ms);
4321         of_property_read_u32(dn, "invensense,temp_fifo_en",
4322                              &st->chip_config.temp_fifo_en);
4323         of_property_read_u32(dn, "invensense,fifo_threshold",
4324                              &st->chip_config.fifo_thr);
4325         if (!of_property_read_u32(dn, "invensense,fsr", &tmp))
4326                 st->chip_config.fsr = tmp;
4327         if (!of_property_read_u32(dn, "invensense,lpf", &tmp))
4328                 st->chip_config.lpf = tmp;
4329         return 0;
4330 }
4331
4332 static int nvi_probe(struct i2c_client *client,
4333                      const struct i2c_device_id *id)
4334 {
4335         struct nvi_state *st;
4336         struct mpu_platform_data *pdata;
4337         unsigned int i;
4338         unsigned int n;
4339         int ret;
4340
4341         dev_info(&client->dev, "%s %s\n", __func__, id->name);
4342         if (!client->irq) {
4343                 dev_err(&client->dev, "%s ERR: no interrupt\n", __func__);
4344                 return -ENODEV;
4345         }
4346
4347         st = devm_kzalloc(&client->dev, sizeof(*st), GFP_KERNEL);
4348         if (st == NULL) {
4349                 dev_err(&client->dev, "%s devm_kzalloc ERR\n", __func__);
4350                 return -ENOMEM;
4351         }
4352
4353         i2c_set_clientdata(client, st);
4354         st->i2c = client;
4355         nvi_init_config(st);
4356         if (client->dev.of_node) {
4357                 ret = nvi_of_dt(st, client->dev.of_node);
4358                 if (ret) {
4359                         if (ret == -ENODEV) {
4360                                 dev_info(&client->dev, "%s DT disabled\n",
4361                                          __func__);
4362                         } else {
4363                                 dev_err(&client->dev, "%s _of_dt ERR\n",
4364                                         __func__);
4365                                 ret = -ENODEV;
4366                         }
4367                         goto nvi_probe_err;
4368                 }
4369         } else {
4370                 pdata = (struct mpu_platform_data *)
4371                                                 dev_get_platdata(&client->dev);
4372                 if (pdata) {
4373                         memcpy(&st->cfg[DEV_ACCEL].matrix,
4374                                &pdata->orientation, sizeof(st->cfg[0].matrix));
4375                         memcpy(&st->cfg[DEV_ANGLVEL].matrix,
4376                                &pdata->orientation, sizeof(st->cfg[0].matrix));
4377                 } else {
4378                         dev_err(&client->dev, "%s dev_get_platdata ERR\n",
4379                                 __func__);
4380                         ret = -EINVAL;
4381                         goto nvi_probe_err;
4382                 }
4383         }
4384
4385         nvi_pm_init(st);
4386         ret = nvi_id_i2c(st, id);
4387         if (ret) {
4388                 dev_err(&client->dev, "%s ERR: nvi_id_i2c\n", __func__);
4389                 goto nvi_probe_err;
4390         }
4391
4392         nvi_fn_dev.sts = &st->sts;
4393         nvi_fn_dev.errs = &st->errs;
4394         st->nvs = nvs_iio();
4395         if (st->nvs == NULL) {
4396                 ret = -ENODEV;
4397                 goto nvi_probe_err;
4398         }
4399
4400         n = 0;
4401         for (i = 0; i < DEV_N; i++) {
4402                 nvs_of_dt(client->dev.of_node, &st->cfg[i], NULL);
4403                 ret = st->nvs->probe(&st->nvs_st[i], st, &client->dev,
4404                                      &nvi_fn_dev, &st->cfg[i]);
4405                 if (!ret) {
4406                         n++;
4407                         if (st->cfg[i].snsr_id == SENSOR_TYPE_TEMPERATURE)
4408                                 /* SENSOR_TYPE_TEMPERATURE is only used to tell
4409                                  * NVS what type of sensor it is since this is
4410                                  * an unknown sensor name.  snsr_id can now be
4411                                  * changed to DEV_TEMP for any future NVS calls
4412                                  */
4413                                 st->cfg[i].snsr_id = DEV_TEMP;
4414                 }
4415         }
4416         if (!n) {
4417                 dev_err(&client->dev, "%s nvs_probe ERR\n", __func__);
4418                 ret = -ENODEV;
4419                 goto nvi_probe_err;
4420         }
4421
4422         ret = request_threaded_irq(st->i2c->irq,
4423                                    nvi_irq_handler, nvi_irq_thread,
4424                                    IRQF_TRIGGER_RISING, NVI_NAME, st);
4425         if (ret) {
4426                 dev_err(&client->dev, "%s req_threaded_irq ERR %d\n",
4427                         __func__, ret);
4428                 ret = -ENOMEM;
4429                 goto nvi_probe_err;
4430         }
4431
4432         nvi_pm(st, NVI_PM_AUTO);
4433         nvi_state_local = st;
4434         dev_info(&client->dev, "%s done\n", __func__);
4435         return 0;
4436
4437 nvi_probe_err:
4438         dev_err(&client->dev, "%s ERR %d\n", __func__, ret);
4439         nvi_remove(client);
4440         return ret;
4441 }
4442
4443 static struct i2c_device_id nvi_i2c_device_id[] = {
4444         { NVI_NAME, 0 },
4445         { "mpu6050", 0 },
4446         { "mpu6500", 0 },
4447         { "mpu6515", 0 },
4448         { "mpu9150", 0 },
4449         { "mpu9250", 0 },
4450         { "mpu9350", 0 },
4451         { "icm20628", 0 },
4452         { "icm20632", 0 },
4453         {}
4454 };
4455
4456 MODULE_DEVICE_TABLE(i2c, nvi_i2c_device_id);
4457
4458 static const struct of_device_id nvi_of_match[] = {
4459         { .compatible = "invensense,mpu6xxx", },
4460         { .compatible = "invensense,mpu6050", },
4461         { .compatible = "invensense,mpu6500", },
4462         { .compatible = "invensense,mpu6515", },
4463         { .compatible = "invensense,mpu9150", },
4464         { .compatible = "invensense,mpu9250", },
4465         { .compatible = "invensense,mpu9350", },
4466         { .compatible = "invensense,icm20628", },
4467         { .compatible = "invensense,icm20632", },
4468         {}
4469 };
4470
4471 MODULE_DEVICE_TABLE(of, nvi_of_match);
4472
4473 static struct i2c_driver nvi_driver = {
4474         .class                          = I2C_CLASS_HWMON,
4475         .probe                          = nvi_probe,
4476         .remove                         = nvi_remove,
4477         .shutdown                       = nvi_shutdown,
4478         .driver                         = {
4479                 .name                   = NVI_NAME,
4480                 .owner                  = THIS_MODULE,
4481                 .of_match_table         = of_match_ptr(nvi_of_match),
4482                 .pm                     = &nvi_pm_ops,
4483         },
4484         .id_table                       = nvi_i2c_device_id,
4485 };
4486
4487 module_i2c_driver(nvi_driver);
4488
4489 MODULE_LICENSE("GPL");
4490 MODULE_DESCRIPTION("NVidiaInvensense driver");
4491 MODULE_AUTHOR("NVIDIA Corporation");
4492