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