drivers/iio/imu/nvi_mpu/nvi.c

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