iio: sensors: NVInvensense drivers
Erik Lilliebjerg [Thu, 8 May 2014 16:38:59 +0000 (09:38 -0700)]
IIO sensor drivers with Invensense MPU auxiliary support for:
- MPU6xxx and MPU9xxx MPU
- AK89xx and AK09911 compass
- BMPx80 pressure
IIO sensor drivers for ALS:
- cm3217
- cm3218x
- max4400x

Bug 200013566
Bug 1528478

Change-Id: I1e29a329026b81d5d1a20273d1d6f0faef9c1a44
Signed-off-by: Erik Lilliebjerg <elilliebjerg@nvidia.com>
Reviewed-on: http://git-master/r/423364
(cherry picked from commit 138016fe3c6ff3626859648ae06bdcbeb71c95e8)
Reviewed-on: http://git-master/r/406997
Reviewed-by: Xiaohui Tao <xtao@nvidia.com>
Tested-by: Xiaohui Tao <xtao@nvidia.com>
Reviewed-by: Thomas Cherry <tcherry@nvidia.com>

27 files changed:
arch/arm/configs/tegra12_android_defconfig
arch/arm/mach-tegra/board-ardbeg-sensors.c
arch/arm64/configs/tegra13_android_defconfig
drivers/iio/Kconfig
drivers/iio/Makefile
drivers/iio/imu/Kconfig
drivers/iio/imu/Makefile
drivers/iio/imu/nvi_mpu/Kconfig [new file with mode: 0644]
drivers/iio/imu/nvi_mpu/Makefile [new file with mode: 0644]
drivers/iio/imu/nvi_mpu/dmpDefaultMPU6050.c [new file with mode: 0644]
drivers/iio/imu/nvi_mpu/dmpKey.h [new file with mode: 0644]
drivers/iio/imu/nvi_mpu/dmpmap.h [new file with mode: 0644]
drivers/iio/imu/nvi_mpu/invensense.c [new file with mode: 0644]
drivers/iio/imu/nvi_mpu/nvi.c [new file with mode: 0644]
drivers/iio/imu/nvi_mpu/nvi.h [new file with mode: 0644]
drivers/iio/light/Kconfig
drivers/iio/light/Makefile
drivers/iio/light/nvs_cm3217.c [new file with mode: 0644]
drivers/iio/light/nvs_cm3218.c [new file with mode: 0644]
drivers/iio/light/nvs_max4400x.c [new file with mode: 0644]
drivers/iio/magnetometer/Kconfig
drivers/iio/magnetometer/Makefile
drivers/iio/magnetometer/nvi_ak89xx.c [new file with mode: 0644]
drivers/iio/pressure/Kconfig [new file with mode: 0644]
drivers/iio/pressure/Makefile [new file with mode: 0644]
drivers/iio/pressure/nvi_bmpX80.c [new file with mode: 0644]
include/linux/mpu_iio.h

index d29ca73..d1553af 100644 (file)
@@ -267,9 +267,6 @@ CONFIG_INPUT_MISC=y
 CONFIG_INPUT_KEYCHORD=y
 CONFIG_INPUT_UINPUT=y
 CONFIG_INPUT_GPIO=y
-CONFIG_INV_AK8975=y
-CONFIG_INV_MPU=y
-CONFIG_INV_BMP180=y
 CONFIG_SERIO_LIBPS2=y
 # CONFIG_VT is not set
 # CONFIG_LEGACY_PTYS is not set
@@ -522,12 +519,9 @@ CONFIG_STAGING=y
 CONFIG_MAX77660_ADC=y
 CONFIG_AS3722_ADC_EXTCON=y
 CONFIG_PALMAS_GPADC=y
-CONFIG_SENSORS_CM3218=y
 CONFIG_SENSORS_JSA1127=y
 CONFIG_SENSORS_LTR558=y
-CONFIG_SENSORS_MAX44005=y
 CONFIG_SENSORS_STM8T143=y
-CONFIG_SENSORS_CM3217=y
 CONFIG_LS_SYSFS=y
 CONFIG_SENSORS_IQS253=y
 CONFIG_INA219=y
@@ -554,6 +548,15 @@ CONFIG_PM_DEVFREQ=y
 CONFIG_EXTCON=y
 CONFIG_EXTCON_PALMAS=y
 CONFIG_IIO=y
+CONFIG_IIO_BUFFER=y
+CONFIG_IIO_KFIFO_BUF=y
+CONFIG_IIO_TRIGGER=y
+CONFIG_NVI_MPU=m
+CONFIG_NVI_AK89XX=m
+CONFIG_NVI_BMPX80=m
+CONFIG_NVS_CM3217=m
+CONFIG_NVS_CM3218=m
+CONFIG_NVS_MAX4400X=m
 CONFIG_PWM=y
 CONFIG_PWM_TEGRA=y
 CONFIG_GK20A_PMU=y
index 9e01912..be89d54 100644 (file)
@@ -15,7 +15,7 @@
 
 #include <linux/i2c.h>
 #include <linux/gpio.h>
-#include <linux/mpu.h>
+#include <linux/mpu_iio.h>
 #include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/nct1008.h>
@@ -58,65 +58,70 @@ static struct i2c_board_info ardbeg_i2c_board_info_cm32181[] = {
 
 /* MPU board file definition    */
 static struct mpu_platform_data mpu9250_gyro_data = {
-       .int_config     = 0x10,
-       .level_shifter  = 0,
-       /* Located in board_[platformname].h */
-       .orientation    = MPU_GYRO_ORIENTATION,
-       .sec_slave_type = SECONDARY_SLAVE_TYPE_NONE,
-       .key            = {0x4E, 0xCC, 0x7E, 0xEB, 0xF6, 0x1E, 0x35, 0x22,
-                       0x00, 0x34, 0x0D, 0x65, 0x32, 0xE9, 0x94, 0x89},
+       .orientation    = MTMAT_TOP_CCW_0,
 };
 
 static struct mpu_platform_data mpu9250_gyro_data_e1762 = {
-       .int_config     = 0x10,
-       .level_shifter  = 0,
-       /* Located in board_[platformname].h */
-       .orientation    = MPU_GYRO_ORIENTATION_E1762,
-       .sec_slave_type = SECONDARY_SLAVE_TYPE_NONE,
-       .key            = {0x4E, 0xCC, 0x7E, 0xEB, 0xF6, 0x1E, 0x35, 0x22,
-                       0x00, 0x34, 0x0D, 0x65, 0x32, 0xE9, 0x94, 0x89},
+       .orientation    = MTMAT_TOP_CCW_270,
 };
 
 static struct mpu_platform_data mpu_compass_data = {
-       .orientation    = MPU_COMPASS_ORIENTATION,
-       .config         = NVI_CONFIG_BOOT_MPU,
-};
-
-static struct mpu_platform_data mpu_bmp_pdata = {
-       .config         = NVI_CONFIG_BOOT_MPU,
+       .orientation = MTMAT_TOP_CCW_270,
 };
 
 static struct i2c_board_info __initdata inv_mpu9250_i2c0_board_info[] = {
        {
-               I2C_BOARD_INFO(MPU_GYRO_NAME, MPU_GYRO_ADDR),
+               I2C_BOARD_INFO("mpu6xxx", 0x69),
                .platform_data = &mpu9250_gyro_data,
        },
        {
-               /* The actual BMP180 address is 0x77 but because this conflicts
-                * with another device, this address is hacked so Linux will
-                * call the driver.  The conflict is technically okay since the
-                * BMP180 is behind the MPU.  Also, the BMP180 driver uses a
-                * hard-coded address of 0x77 since it can't be changed anyway.
-                */
-               I2C_BOARD_INFO(MPU_BMP_NAME, MPU_BMP_ADDR),
-               .platform_data = &mpu_bmp_pdata,
+               I2C_BOARD_INFO("bmpX80", 0x77),
        },
        {
-               I2C_BOARD_INFO(MPU_COMPASS_NAME, MPU_COMPASS_ADDR),
+               I2C_BOARD_INFO("ak89xx", 0x0C),
                .platform_data = &mpu_compass_data,
        },
+       {
+               I2C_BOARD_INFO("cm3217", 0x10),
+       },
+       {
+               I2C_BOARD_INFO("cm3218x", 0x48),
+       },
+       {
+               I2C_BOARD_INFO("max4400x", 0x44),
+       },
 };
 
 static void mpuirq_init(void)
 {
        int ret = 0;
-       unsigned gyro_irq_gpio = MPU_GYRO_IRQ_GPIO;
-       unsigned gyro_bus_num = MPU_GYRO_BUS_NUM;
+/*     unsigned gyro_irq_gpio = TEGRA_GPIO_PR3; oldest platforms */
+/*     unsigned gyro_irq_gpio = TEGRA_GPIO_PO7; older platforms */
+       unsigned gyro_irq_gpio = TEGRA_GPIO_PS0;
+       unsigned als_irq_gpio = TEGRA_GPIO_PX3;
        char *gyro_name = MPU_GYRO_NAME;
        struct board_info board_info;
 
        pr_info("*** MPU START *** mpuirq_init...\n");
 
+       ret = gpio_request(als_irq_gpio, "als");
+       if (ret < 0) {
+               pr_err("%s: gpio_request %d failed %d\n",
+                      __func__, als_irq_gpio, ret);
+       } else {
+               ret = gpio_direction_input(als_irq_gpio);
+               if (ret < 0) {
+                       pr_err("%s: gpio_direction_input %d failed %d\n",
+                               __func__, als_irq_gpio, ret);
+                       gpio_free(als_irq_gpio);
+               } else {
+                       inv_mpu9250_i2c0_board_info[4].irq =
+                                                    gpio_to_irq(als_irq_gpio);
+                       inv_mpu9250_i2c0_board_info[5].irq =
+                                                    gpio_to_irq(als_irq_gpio);
+               }
+       }
+
        tegra_get_board_info(&board_info);
 
        ret = gpio_request(gyro_irq_gpio, gyro_name);
@@ -140,8 +145,8 @@ static void mpuirq_init(void)
        if (board_info.board_id == BOARD_E1762)
                inv_mpu9250_i2c0_board_info[0].platform_data =
                                        &mpu9250_gyro_data_e1762;
-       inv_mpu9250_i2c0_board_info[0].irq = gpio_to_irq(MPU_GYRO_IRQ_GPIO);
-       i2c_register_board_info(gyro_bus_num, inv_mpu9250_i2c0_board_info,
+       inv_mpu9250_i2c0_board_info[0].irq = gpio_to_irq(gyro_irq_gpio);
+       i2c_register_board_info(0, inv_mpu9250_i2c0_board_info,
                ARRAY_SIZE(inv_mpu9250_i2c0_board_info));
 }
 
index 6a92afd..97ed13f 100644 (file)
@@ -253,9 +253,6 @@ CONFIG_INPUT_MISC=y
 CONFIG_INPUT_KEYCHORD=y
 CONFIG_INPUT_UINPUT=y
 CONFIG_INPUT_GPIO=y
-CONFIG_INV_AK8975=y
-CONFIG_INV_MPU=y
-CONFIG_INV_BMP180=y
 # CONFIG_SERIO_I8042 is not set
 # CONFIG_VT is not set
 # CONFIG_LEGACY_PTYS is not set
@@ -513,12 +510,9 @@ CONFIG_STAGING=y
 CONFIG_MAX77660_ADC=y
 CONFIG_AS3722_ADC_EXTCON=y
 CONFIG_PALMAS_GPADC=y
-CONFIG_SENSORS_CM3218=y
 CONFIG_SENSORS_JSA1127=y
 CONFIG_SENSORS_LTR558=y
-CONFIG_SENSORS_MAX44005=y
 CONFIG_SENSORS_STM8T143=y
-CONFIG_SENSORS_CM3217=y
 CONFIG_INA219=y
 CONFIG_INA230=y
 CONFIG_INA3221=y
@@ -540,6 +534,15 @@ CONFIG_TEGRA_IOMMU_SMMU=y
 CONFIG_EXTCON=y
 CONFIG_EXTCON_PALMAS=y
 CONFIG_IIO=y
+CONFIG_IIO_BUFFER=y
+CONFIG_IIO_KFIFO_BUF=y
+CONFIG_IIO_TRIGGER=y
+CONFIG_NVI_MPU=m
+CONFIG_NVI_AK89XX=m
+CONFIG_NVI_BMPX80=m
+CONFIG_NVS_CM3217=m
+CONFIG_NVS_CM3218=m
+CONFIG_NVS_MAX4400X=m
 CONFIG_PWM=y
 CONFIG_PWM_TEGRA=y
 CONFIG_EXT2_FS=y
index b2f963b..b45b070 100644 (file)
@@ -70,5 +70,6 @@ source "drivers/iio/gyro/Kconfig"
 source "drivers/iio/imu/Kconfig"
 source "drivers/iio/light/Kconfig"
 source "drivers/iio/magnetometer/Kconfig"
+source "drivers/iio/pressure/Kconfig"
 
 endif # IIO
index a0e8cdd..909ced7 100644 (file)
@@ -21,3 +21,5 @@ obj-y += frequency/
 obj-y += imu/
 obj-y += light/
 obj-y += magnetometer/
+obj-y += pressure/
+
index 26e1341..13c362a 100644 (file)
@@ -39,3 +39,4 @@ config IIO_ADIS_LIB_BUFFER
 
 source "drivers/iio/imu/inv_mpu/Kconfig"
 source "drivers/iio/imu/inv_mpu6050/Kconfig"
+source "drivers/iio/imu/nvi_mpu/Kconfig"
index e29f6d5..9d981d9 100644 (file)
@@ -14,3 +14,4 @@ obj-$(CONFIG_IIO_ADIS_LIB) += adis_lib.o
 
 obj-y += inv_mpu6050/
 obj-y += inv_mpu/
+obj-y += nvi_mpu/
diff --git a/drivers/iio/imu/nvi_mpu/Kconfig b/drivers/iio/imu/nvi_mpu/Kconfig
new file mode 100644 (file)
index 0000000..46cc9a1
--- /dev/null
@@ -0,0 +1,14 @@
+#
+# nvi-mpu driver for Invensense MPU devices and combos
+#
+
+config NVI_MPU
+       tristate "Invensense MPU devices"
+       depends on I2C && SYSFS && IIO && IIO_KFIFO_BUF && IIO_TRIGGER && !INV_MPU
+       default n
+       help
+         This driver supports the Invensense MPU devices.
+         This includes MPU6050/MPU65XX/MPU9X50
+         This driver can be built as a module. The module will be called
+         nvi-mpu.
+
diff --git a/drivers/iio/imu/nvi_mpu/Makefile b/drivers/iio/imu/nvi_mpu/Makefile
new file mode 100644 (file)
index 0000000..ef55daf
--- /dev/null
@@ -0,0 +1,14 @@
+#
+# Makefile for NVidia Invensense nvi-mpu device.
+#
+
+obj-$(CONFIG_NVI_MPU) += nvi-mpu.o
+
+nvi-mpu-objs := nvi.o
+nvi-mpu-objs += invensense.o
+nvi-mpu-objs += dmpDefaultMPU6050.o
+
+CFLAGS_nvi.o                   += -Idrivers/iio
+CFLAGS_invensense.o            += -Idrivers/iio
+CFLAGS_dmpDefaultMPU6050.o     += -Idrivers/iio
+
diff --git a/drivers/iio/imu/nvi_mpu/dmpDefaultMPU6050.c b/drivers/iio/imu/nvi_mpu/dmpDefaultMPU6050.c
new file mode 100644 (file)
index 0000000..f016580
--- /dev/null
@@ -0,0 +1,381 @@
+/* Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (C) 2012 Invensense, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+#include <linux/printk.h>
+#include "dmpKey.h"
+#include "dmpmap.h"
+
+#define CFG_OUT_PRESS               (1823)
+#define CFG_PED_ENABLE              (1936)
+#define CFG_OUT_GYRO                (1755)
+#define CFG_PEDSTEP_DET             (2417)
+#define OUT_GYRO_DAT                (1764)
+#define CFG_FIFO_INT                (1934)
+#define OUT_CPASS_DAT               (1798)
+#define CFG_AUTH                    (1051)
+#define OUT_ACCL_DAT                (1730)
+#define FCFG_1                      (1078)
+#define FCFG_3                      (1103)
+#define FCFG_2                      (1082)
+#define CFG_OUT_CPASS               (1789)
+#define FCFG_7                      (1089)
+#define CFG_OUT_3QUAT               (1617)
+#define OUT_PRESS_DAT               (1832)
+#define OUT_3QUAT_DAT               (1627)
+#define CFG_7                       (1300)
+#define OUT_PQUAT_DAT               (1696)
+#define CFG_OUT_6QUAT               (1652)
+#define CFG_PED_INT                 (2406)
+#define SMD_TP2                     (1265)
+#define SMD_TP1                     (1244)
+#define CFG_MOTION_BIAS             (1302)
+#define CFG_OUT_ACCL                (1721)
+#define CFG_OUT_STEPDET             (1587)
+#define OUT_6QUAT_DAT               (1662)
+#define CFG_OUT_PQUAT               (1687)
+
+#define D_0_22                  (22+512)
+#define D_0_24                  (24+512)
+
+#define D_0_36                  (36)
+#define D_0_52                  (52)
+#define D_0_96                  (96)
+#define D_0_104                 (104)
+#define D_0_108                 (108)
+#define D_0_163                 (163)
+#define D_0_188                 (188)
+#define D_0_192                 (192)
+#define D_0_224                 (224)
+#define D_0_228                 (228)
+#define D_0_232                 (232)
+#define D_0_236                 (236)
+
+#define D_1_2                   (256 + 2)
+#define D_1_4                   (256 + 4)
+#define D_1_8                   (256 + 8)
+#define D_1_10                  (256 + 10)
+#define D_1_24                  (256 + 24)
+#define D_1_28                  (256 + 28)
+#define D_1_36                  (256 + 36)
+#define D_1_40                  (256 + 40)
+#define D_1_44                  (256 + 44)
+#define D_1_72                  (256 + 72)
+#define D_1_74                  (256 + 74)
+#define D_1_79                  (256 + 79)
+#define D_1_88                  (256 + 88)
+#define D_1_90                  (256 + 90)
+#define D_1_92                  (256 + 92)
+#define D_1_96                  (256 + 96)
+#define D_1_98                  (256 + 98)
+#define D_1_106                 (256 + 106)
+#define D_1_108                 (256 + 108)
+#define D_1_112                 (256 + 112)
+#define D_1_128                 (256 + 144)
+#define D_1_152                 (256 + 12)
+#define D_1_160                 (256 + 160)
+#define D_1_176                 (256 + 176)
+#define D_1_178                 (256 + 178)
+#define D_1_218                 (256 + 218)
+#define D_1_232                 (256 + 232)
+#define D_1_236                 (256 + 236)
+#define D_1_240                 (256 + 240)
+#define D_1_244                 (256 + 244)
+#define D_1_250                 (256 + 250)
+#define D_1_252                 (256 + 252)
+#define D_2_12                  (512 + 12)
+#define D_2_96                  (512 + 96)
+#define D_2_108                 (512 + 108)
+#define D_2_208                 (512 + 208)
+#define D_2_224                 (512 + 224)
+#define D_2_236                 (512 + 236)
+#define D_2_244                 (512 + 244)
+#define D_2_248                 (512 + 248)
+#define D_2_252                 (512 + 252)
+#define CPASS_BIAS_X            (30 * 16 + 4)
+#define CPASS_BIAS_Y            (30 * 16 + 8)
+#define CPASS_BIAS_Z            (30 * 16 + 12)
+#define CPASS_MTX_00            (32 * 16 + 4)
+#define CPASS_MTX_01            (32 * 16 + 8)
+#define CPASS_MTX_02            (36 * 16 + 12)
+#define CPASS_MTX_10            (33 * 16)
+#define CPASS_MTX_11            (33 * 16 + 4)
+#define CPASS_MTX_12            (33 * 16 + 8)
+#define CPASS_MTX_20            (33 * 16 + 12)
+#define CPASS_MTX_21            (34 * 16 + 4)
+#define CPASS_MTX_22            (34 * 16 + 8)
+#define D_EXT_GYRO_BIAS_X       (61 * 16)
+#define D_EXT_GYRO_BIAS_Y       (61 * 16 + 4)
+#define D_EXT_GYRO_BIAS_Z       (61 * 16 + 8)
+#define D_ACT0                  (40 * 16)
+#define D_ACSX                  (40 * 16 + 4)
+#define D_ACSY                  (40 * 16 + 8)
+#define D_ACSZ                  (40 * 16 + 12)
+
+#define FLICK_MSG               (45 * 16 + 4)
+#define FLICK_COUNTER           (45 * 16 + 8)
+#define FLICK_LOWER             (45 * 16 + 12)
+#define FLICK_UPPER             (46 * 16 + 12)
+
+#define D_SMD_ENABLE            (18 * 16)
+#define D_SMD_MOT_THLD          (21 * 16 + 8)
+#define D_SMD_DELAY_THLD        (21 * 16 + 4)
+#define D_SMD_DELAY2_THLD       (21 * 16 + 12)
+#define D_SMD_EXE_STATE         (22 * 16)
+#define D_SMD_DELAY_CNTR        (21 * 16)
+
+#define D_WF_GESTURE_TIME_THRSH        (25 * 16 + 8)
+#define D_WF_GESTURE_TILT_ERROR        (25 * 16 + 12)
+#define D_WF_GESTURE_TILT_THRSH        (26 * 16 + 8)
+#define D_WF_GESTURE_TILT_REJECT_THRSH (26 * 16 + 12)
+
+#define D_AUTH_OUT              (992)
+#define D_AUTH_IN               (996)
+#define D_AUTH_A                (1000)
+#define D_AUTH_B                (1004)
+
+#define D_PEDSTD_BP_B           (768 + 0x1C)
+#define D_PEDSTD_BP_A4          (768 + 0x40)
+#define D_PEDSTD_BP_A3          (768 + 0x44)
+#define D_PEDSTD_BP_A2          (768 + 0x48)
+#define D_PEDSTD_BP_A1          (768 + 0x4C)
+#define D_PEDSTD_SB             (768 + 0x28)
+#define D_PEDSTD_SB_TIME        (768 + 0x2C)
+#define D_PEDSTD_PEAKTHRSH      (768 + 0x98)
+#define D_PEDSTD_TIML           (768 + 0x2A)
+#define D_PEDSTD_TIMH           (768 + 0x2E)
+#define D_PEDSTD_PEAK           (768 + 0X94)
+#define D_PEDSTD_STEPCTR        (768 + 0x60)
+#define D_PEDSTD_STEPCTR2       (58 * 16 + 8)
+#define D_PEDSTD_TIMECTR        (964)
+#define D_PEDSTD_DECI           (768 + 0xA0)
+#define D_PEDSTD_SB2                   (60 * 16 + 14)
+#define D_STPDET_TIMESTAMP      (28 * 16 + 8)
+#define D_PEDSTD_DRIVE_STATE    (58)
+
+#define D_HOST_NO_MOT           (976)
+#define D_ACCEL_BIAS            (660)
+
+#define D_BM_BATCH_CNTR         (27 * 16 + 4)
+#define D_BM_BATCH_THLD         (27 * 16 + 12)
+#define D_BM_ENABLE             (28 * 16 + 6)
+#define D_BM_NUMWORD_TOFILL     (28 * 16 + 4)
+
+#define D_SO_DATA               (4 * 16 + 2)
+
+#define D_P_HW_ID               (22 * 16 + 10)
+#define D_P_INIT                (22 * 16 + 2)
+
+#define D_DMP_RUN_CNTR          (24*16)
+
+#define D_ODR_S0                (45*16+8)
+#define D_ODR_S1                (45*16+12)
+#define D_ODR_S2                (45*16+10)
+#define D_ODR_S3                (45*16+14)
+#define D_ODR_S4                (46*16+8)
+#define D_ODR_S5                (46*16+12)
+#define D_ODR_S6                (46*16+10)
+#define D_ODR_S7                (46*16+14)
+#define D_ODR_S8                (42*16+8)
+#define D_ODR_S9                (42*16+12)
+
+#define D_ODR_CNTR_S0           (45*16)
+#define D_ODR_CNTR_S1           (45*16+4)
+#define D_ODR_CNTR_S2           (45*16+2)
+#define D_ODR_CNTR_S3           (45*16+6)
+#define D_ODR_CNTR_S4           (46*16)
+#define D_ODR_CNTR_S5           (46*16+4)
+#define D_ODR_CNTR_S6           (46*16+2)
+#define D_ODR_CNTR_S7           (46*16+6)
+#define D_ODR_CNTR_S8           (42*16)
+#define D_ODR_CNTR_S9           (42*16+4)
+
+#define D_FS_LPQ0               (59*16)
+#define D_FS_LPQ1               (59*16 + 4)
+#define D_FS_LPQ2               (59*16 + 8)
+#define D_FS_LPQ3               (59*16 + 12)
+
+#define D_FS_Q0                 (12*16)
+#define D_FS_Q1                 (12*16 + 4)
+#define D_FS_Q2                 (12*16 + 8)
+#define D_FS_Q3                 (12*16 + 12)
+
+#define D_FS_9Q0                (2*16)
+#define D_FS_9Q1                (2*16 + 4)
+#define D_FS_9Q2                (2*16 + 8)
+#define D_FS_9Q3                (2*16 + 12)
+
+static const struct t_key_label dmp_t_config[] = {
+       {KEY_CFG_OUT_ACCL,              CFG_OUT_ACCL},
+       {KEY_CFG_OUT_GYRO,              CFG_OUT_GYRO},
+       {KEY_CFG_OUT_3QUAT,             CFG_OUT_3QUAT},
+       {KEY_CFG_OUT_6QUAT,             CFG_OUT_6QUAT},
+       {KEY_CFG_OUT_PQUAT,             CFG_OUT_PQUAT},
+       {KEY_CFG_PED_ENABLE,            CFG_PED_ENABLE},
+       {KEY_CFG_FIFO_INT,              CFG_FIFO_INT},
+       {KEY_CFG_AUTH,                  CFG_AUTH},
+       {KEY_FCFG_1,                    FCFG_1},
+       {KEY_FCFG_3,                    FCFG_3},
+       {KEY_FCFG_2,                    FCFG_2},
+       {KEY_FCFG_7,                    FCFG_7},
+       {KEY_CFG_7,                     CFG_7},
+       {KEY_CFG_MOTION_BIAS,           CFG_MOTION_BIAS},
+       {KEY_CFG_PEDSTEP_DET,           CFG_PEDSTEP_DET},
+       {KEY_D_0_22,                    D_0_22},
+       {KEY_D_0_96,                    D_0_96},
+       {KEY_D_0_104,                   D_0_104},
+       {KEY_D_0_108,                   D_0_108},
+       {KEY_D_1_36,                    D_1_36},
+       {KEY_D_1_40,                    D_1_40},
+       {KEY_D_1_44,                    D_1_44},
+       {KEY_D_1_72,                    D_1_72},
+       {KEY_D_1_74,                    D_1_74},
+       {KEY_D_1_79,                    D_1_79},
+       {KEY_D_1_88,                    D_1_88},
+       {KEY_D_1_90,                    D_1_90},
+       {KEY_D_1_92,                    D_1_92},
+       {KEY_D_1_160,                   D_1_160},
+       {KEY_D_1_176,                   D_1_176},
+       {KEY_D_1_218,                   D_1_218},
+       {KEY_D_1_232,                   D_1_232},
+       {KEY_D_1_250,                   D_1_250},
+       {KEY_DMP_SH_TH_Y,               DMP_SH_TH_Y},
+       {KEY_DMP_SH_TH_X,               DMP_SH_TH_X},
+       {KEY_DMP_SH_TH_Z,               DMP_SH_TH_Z},
+       {KEY_DMP_ORIENT,                DMP_ORIENT},
+       {KEY_D_AUTH_OUT,                D_AUTH_OUT},
+       {KEY_D_AUTH_IN,                 D_AUTH_IN},
+       {KEY_D_AUTH_A,                  D_AUTH_A},
+       {KEY_D_AUTH_B,                  D_AUTH_B},
+       {KEY_CPASS_BIAS_X,          CPASS_BIAS_X},
+       {KEY_CPASS_BIAS_Y,          CPASS_BIAS_Y},
+       {KEY_CPASS_BIAS_Z,          CPASS_BIAS_Z},
+       {KEY_CPASS_MTX_00,          CPASS_MTX_00},
+       {KEY_CPASS_MTX_01,          CPASS_MTX_01},
+       {KEY_CPASS_MTX_02,          CPASS_MTX_02},
+       {KEY_CPASS_MTX_10,          CPASS_MTX_10},
+       {KEY_CPASS_MTX_11,          CPASS_MTX_11},
+       {KEY_CPASS_MTX_12,          CPASS_MTX_12},
+       {KEY_CPASS_MTX_20,          CPASS_MTX_20},
+       {KEY_CPASS_MTX_21,          CPASS_MTX_21},
+       {KEY_CPASS_MTX_22,          CPASS_MTX_22},
+       {KEY_D_ACT0,                    D_ACT0},
+       {KEY_D_ACSX,                    D_ACSX},
+       {KEY_D_ACSY,                    D_ACSY},
+       {KEY_D_ACSZ,                    D_ACSZ},
+       {KEY_D_PEDSTD_BP_B,             D_PEDSTD_BP_B},
+       {KEY_D_PEDSTD_BP_A4,            D_PEDSTD_BP_A4},
+       {KEY_D_PEDSTD_BP_A3,            D_PEDSTD_BP_A3},
+       {KEY_D_PEDSTD_BP_A2,            D_PEDSTD_BP_A2},
+       {KEY_D_PEDSTD_BP_A1,            D_PEDSTD_BP_A1},
+       {KEY_D_PEDSTD_SB,               D_PEDSTD_SB},
+       {KEY_D_PEDSTD_SB_TIME,          D_PEDSTD_SB_TIME},
+       {KEY_D_PEDSTD_PEAKTHRSH,        D_PEDSTD_PEAKTHRSH},
+       {KEY_D_PEDSTD_TIML,             D_PEDSTD_TIML},
+       {KEY_D_PEDSTD_TIMH,             D_PEDSTD_TIMH},
+       {KEY_D_PEDSTD_PEAK,             D_PEDSTD_PEAK},
+       {KEY_D_PEDSTD_STEPCTR,          D_PEDSTD_STEPCTR},
+       {KEY_D_PEDSTD_STEPCTR2,         D_PEDSTD_STEPCTR2},
+       {KEY_D_PEDSTD_TIMECTR,          D_PEDSTD_TIMECTR},
+       {KEY_D_PEDSTD_DECI,             D_PEDSTD_DECI},
+       {KEY_D_PEDSTD_SB2,                              D_PEDSTD_SB2},
+       {KEY_D_PEDSTD_DRIVE_STATE,      D_PEDSTD_DRIVE_STATE},
+       {KEY_D_STPDET_TIMESTAMP,                D_STPDET_TIMESTAMP},
+       {KEY_D_HOST_NO_MOT,             D_HOST_NO_MOT},
+       {KEY_D_ACCEL_BIAS,              D_ACCEL_BIAS},
+       {KEY_CFG_EXT_GYRO_BIAS_X,       D_EXT_GYRO_BIAS_X},
+       {KEY_CFG_EXT_GYRO_BIAS_Y,       D_EXT_GYRO_BIAS_Y},
+       {KEY_CFG_EXT_GYRO_BIAS_Z,       D_EXT_GYRO_BIAS_Z},
+       {KEY_CFG_PED_INT,               CFG_PED_INT},
+       {KEY_SMD_ENABLE,                D_SMD_ENABLE},
+       {KEY_SMD_ACCEL_THLD,            D_SMD_MOT_THLD},
+       {KEY_SMD_DELAY_THLD,            D_SMD_DELAY_THLD},
+       {KEY_SMD_DELAY2_THLD,           D_SMD_DELAY2_THLD},
+       {KEY_SMD_ENABLE_TESTPT1,        SMD_TP1},
+       {KEY_SMD_ENABLE_TESTPT2,        SMD_TP2},
+       {KEY_SMD_EXE_STATE,             D_SMD_EXE_STATE},
+       {KEY_SMD_DELAY_CNTR,            D_SMD_DELAY_CNTR},
+       {KEY_BM_ENABLE,                 D_BM_ENABLE},
+       {KEY_BM_BATCH_CNTR,             D_BM_BATCH_CNTR},
+       {KEY_BM_BATCH_THLD,             D_BM_BATCH_THLD},
+       {KEY_BM_NUMWORD_TOFILL,         D_BM_NUMWORD_TOFILL},
+       {KEY_SO_DATA,                   D_SO_DATA},
+       {KEY_P_INIT,                    D_P_INIT},
+       {KEY_CFG_OUT_CPASS,             CFG_OUT_CPASS},
+       {KEY_CFG_OUT_PRESS,             CFG_OUT_PRESS},
+       {KEY_CFG_OUT_STEPDET,           CFG_OUT_STEPDET},
+       {KEY_CFG_3QUAT_ODR,             D_ODR_S1},
+       {KEY_CFG_6QUAT_ODR,             D_ODR_S2},
+       {KEY_CFG_PQUAT6_ODR,            D_ODR_S3},
+       {KEY_CFG_ACCL_ODR,              D_ODR_S4},
+       {KEY_CFG_GYRO_ODR,              D_ODR_S5},
+       {KEY_CFG_CPASS_ODR,             D_ODR_S6},
+       {KEY_CFG_PRESS_ODR,             D_ODR_S7},
+       {KEY_CFG_9QUAT_ODR,             D_ODR_S8},
+       {KEY_CFG_PQUAT9_ODR,            D_ODR_S9},
+       {KEY_ODR_CNTR_3QUAT,            D_ODR_CNTR_S1},
+       {KEY_ODR_CNTR_6QUAT,            D_ODR_CNTR_S2},
+       {KEY_ODR_CNTR_PQUAT,            D_ODR_CNTR_S3},
+       {KEY_ODR_CNTR_ACCL,             D_ODR_CNTR_S4},
+       {KEY_ODR_CNTR_GYRO,             D_ODR_CNTR_S5},
+       {KEY_ODR_CNTR_CPASS,            D_ODR_CNTR_S6},
+       {KEY_ODR_CNTR_PRESS,            D_ODR_CNTR_S7},
+       {KEY_ODR_CNTR_9QUAT,                    D_ODR_CNTR_S8},
+       {KEY_ODR_CNTR_PQUAT9,                   D_ODR_CNTR_S9},
+       {KEY_DMP_RUN_CNTR,              D_DMP_RUN_CNTR},
+       {KEY_DMP_LPQ0,                  D_FS_LPQ0},
+       {KEY_DMP_LPQ1,                  D_FS_LPQ1},
+       {KEY_DMP_LPQ2,                  D_FS_LPQ2},
+       {KEY_DMP_LPQ3,                  D_FS_LPQ3},
+       {KEY_DMP_Q0,                    D_FS_Q0},
+       {KEY_DMP_Q1,                    D_FS_Q1},
+       {KEY_DMP_Q2,                    D_FS_Q2},
+       {KEY_DMP_Q3,                    D_FS_Q3},
+       {KEY_DMP_9Q0,                                   D_FS_9Q0},
+       {KEY_DMP_9Q1,                                   D_FS_9Q1},
+       {KEY_DMP_9Q2,                                   D_FS_9Q2},
+       {KEY_DMP_9Q3,                                   D_FS_9Q3},
+       {KEY_TEST_01,                   OUT_ACCL_DAT},
+       {KEY_TEST_02,                   OUT_GYRO_DAT},
+       {KEY_TEST_03,                   OUT_CPASS_DAT},
+       {KEY_TEST_04,                   OUT_PRESS_DAT},
+       {KEY_TEST_05,                   OUT_3QUAT_DAT},
+       {KEY_TEST_06,                   OUT_6QUAT_DAT},
+       {KEY_TEST_07,                   OUT_PQUAT_DAT}
+};
+#define NUM_LOCAL_KEYS (sizeof(dmp_t_config)/sizeof(dmp_t_config[0]))
+
+static struct t_key_label keys[NUM_KEYS];
+
+unsigned short inv_dmp_get_address(unsigned short key)
+{
+       static int is_sorted;
+
+       if (!is_sorted) {
+               int kk;
+               for (kk = 0; kk < NUM_KEYS; ++kk) {
+                       keys[kk].addr = 0xffff;
+                       keys[kk].key = kk;
+               }
+               for (kk = 0; kk < NUM_LOCAL_KEYS; ++kk)
+                       keys[dmp_t_config[kk].key].addr = dmp_t_config[kk].addr;
+               is_sorted = 1;
+       }
+       if (key >= NUM_KEYS) {
+               pr_err("ERROR!! key not exist=%d!\n", key);
+               return 0xffff;
+       }
+       if (0xffff == keys[key].addr)
+               pr_err("ERROR!!key not local=%d!\n", key);
+       return keys[key].addr;
+}
diff --git a/drivers/iio/imu/nvi_mpu/dmpKey.h b/drivers/iio/imu/nvi_mpu/dmpKey.h
new file mode 100644 (file)
index 0000000..e9f4bb8
--- /dev/null
@@ -0,0 +1,607 @@
+/* Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (C) 2012 Invensense, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef DMPKEY_H__
+#define DMPKEY_H__
+
+#define KEY_CFG_25                  (0)
+#define KEY_CFG_24                  (KEY_CFG_25 + 1)
+#define KEY_CFG_26                  (KEY_CFG_24 + 1)
+#define KEY_CFG_27                  (KEY_CFG_26 + 1)
+#define KEY_CFG_21                  (KEY_CFG_27 + 1)
+#define KEY_CFG_20                  (KEY_CFG_21 + 1)
+#define KEY_CFG_TAP4                (KEY_CFG_20 + 1)
+#define KEY_CFG_TAP5                (KEY_CFG_TAP4 + 1)
+#define KEY_CFG_TAP6                (KEY_CFG_TAP5 + 1)
+#define KEY_CFG_TAP7                (KEY_CFG_TAP6 + 1)
+#define KEY_CFG_TAP0                (KEY_CFG_TAP7 + 1)
+#define KEY_CFG_TAP1                (KEY_CFG_TAP0 + 1)
+#define KEY_CFG_TAP2                (KEY_CFG_TAP1 + 1)
+#define KEY_CFG_TAP3                (KEY_CFG_TAP2 + 1)
+#define KEY_CFG_TAP_QUANTIZE        (KEY_CFG_TAP3 + 1)
+#define KEY_CFG_TAP_JERK            (KEY_CFG_TAP_QUANTIZE + 1)
+#define KEY_CFG_DR_INT              (KEY_CFG_TAP_JERK + 1)
+#define KEY_CFG_AUTH                (KEY_CFG_DR_INT + 1)
+#define KEY_CFG_TAP_SAVE_ACCB       (KEY_CFG_AUTH + 1)
+#define KEY_CFG_TAP_CLEAR_STICKY    (KEY_CFG_TAP_SAVE_ACCB + 1)
+#define KEY_CFG_FIFO_ON_EVENT       (KEY_CFG_TAP_CLEAR_STICKY + 1)
+#define KEY_FCFG_ACCEL_INPUT        (KEY_CFG_FIFO_ON_EVENT + 1)
+#define KEY_FCFG_ACCEL_INIT         (KEY_FCFG_ACCEL_INPUT + 1)
+#define KEY_CFG_23                  (KEY_FCFG_ACCEL_INIT + 1)
+#define KEY_FCFG_1                  (KEY_CFG_23 + 1)
+#define KEY_FCFG_3                  (KEY_FCFG_1 + 1)
+#define KEY_FCFG_2                  (KEY_FCFG_3 + 1)
+#define KEY_CFG_3D                  (KEY_FCFG_2 + 1)
+#define KEY_CFG_3B                  (KEY_CFG_3D + 1)
+#define KEY_CFG_3C                  (KEY_CFG_3B + 1)
+#define KEY_FCFG_5                  (KEY_CFG_3C + 1)
+#define KEY_FCFG_4                  (KEY_FCFG_5 + 1)
+#define KEY_FCFG_7                  (KEY_FCFG_4 + 1)
+#define KEY_FCFG_FSCALE             (KEY_FCFG_7 + 1)
+#define KEY_FCFG_AZ                 (KEY_FCFG_FSCALE + 1)
+#define KEY_FCFG_6                  (KEY_FCFG_AZ + 1)
+#define KEY_FCFG_LSB4               (KEY_FCFG_6 + 1)
+#define KEY_CFG_12                  (KEY_FCFG_LSB4 + 1)
+#define KEY_CFG_14                  (KEY_CFG_12 + 1)
+#define KEY_CFG_15                  (KEY_CFG_14 + 1)
+#define KEY_CFG_16                  (KEY_CFG_15 + 1)
+#define KEY_CFG_18                  (KEY_CFG_16 + 1)
+#define KEY_CFG_6                   (KEY_CFG_18 + 1)
+#define KEY_CFG_7                   (KEY_CFG_6 + 1)
+#define KEY_CFG_4                   (KEY_CFG_7 + 1)
+#define KEY_CFG_5                   (KEY_CFG_4 + 1)
+#define KEY_CFG_2                   (KEY_CFG_5 + 1)
+#define KEY_CFG_3                   (KEY_CFG_2 + 1)
+#define KEY_CFG_1                   (KEY_CFG_3 + 1)
+#define KEY_CFG_EXTERNAL            (KEY_CFG_1 + 1)
+#define KEY_CFG_8                   (KEY_CFG_EXTERNAL + 1)
+#define KEY_CFG_9                   (KEY_CFG_8 + 1)
+#define KEY_CFG_ORIENT_3            (KEY_CFG_9 + 1)
+#define KEY_CFG_ORIENT_2            (KEY_CFG_ORIENT_3 + 1)
+#define KEY_CFG_ORIENT_1            (KEY_CFG_ORIENT_2 + 1)
+#define KEY_CFG_GYRO_SOURCE         (KEY_CFG_ORIENT_1 + 1)
+#define KEY_CFG_ORIENT_IRQ_1        (KEY_CFG_GYRO_SOURCE + 1)
+#define KEY_CFG_ORIENT_IRQ_2        (KEY_CFG_ORIENT_IRQ_1 + 1)
+#define KEY_CFG_ORIENT_IRQ_3        (KEY_CFG_ORIENT_IRQ_2 + 1)
+#define KEY_FCFG_MAG_VAL            (KEY_CFG_ORIENT_IRQ_3 + 1)
+#define KEY_FCFG_MAG_MOV            (KEY_FCFG_MAG_VAL + 1)
+#define KEY_CFG_LP_QUAT             (KEY_FCFG_MAG_MOV + 1)
+#define KEY_CFG_GYRO_RAW_DATA       (KEY_CFG_LP_QUAT + 1)
+#define KEY_CFG_EXT_GYRO_BIAS       (KEY_CFG_GYRO_RAW_DATA + 1)
+#define KEY_CFG_EXT_GYRO_BIAS_X     (KEY_CFG_EXT_GYRO_BIAS + 1)
+#define KEY_CFG_EXT_GYRO_BIAS_Y     (KEY_CFG_EXT_GYRO_BIAS_X + 1)
+#define KEY_CFG_EXT_GYRO_BIAS_Z     (KEY_CFG_EXT_GYRO_BIAS_Y + 1)
+#define KEY_BAD_COMPASS             (KEY_CFG_EXT_GYRO_BIAS_Z + 1)
+#define KEY_COMPASS_CHG_SENSITIVITY (KEY_BAD_COMPASS + 1)
+#define KEY_CCS_HEADING_THLD        (KEY_COMPASS_CHG_SENSITIVITY + 1)
+#define KEY_CCS_TIME_THLD           (KEY_CCS_HEADING_THLD + 1)
+#define KEY_CCS_DOTP_THLD           (KEY_CCS_TIME_THLD + 1)
+#define KEY_CCS_COMP_CNTR           (KEY_CCS_DOTP_THLD + 1)
+#define KEY_CFG_NM_DET              (KEY_CCS_COMP_CNTR + 1)
+#define KEY_SMD_ENABLE              (KEY_CFG_NM_DET + 1)
+#define KEY_SMD_ACCEL_THLD          (KEY_SMD_ENABLE + 1)
+#define KEY_SMD_DELAY_THLD          (KEY_SMD_ACCEL_THLD + 1)
+#define KEY_SMD_DELAY2_THLD         (KEY_SMD_DELAY_THLD + 1)
+#define KEY_SMD_ENABLE_TESTPT1      (KEY_SMD_DELAY2_THLD + 1)
+#define KEY_SMD_ENABLE_TESTPT2      (KEY_SMD_ENABLE_TESTPT1 + 1)
+#define KEY_SMD_EXE_STATE           (KEY_SMD_ENABLE_TESTPT2 + 1)
+#define KEY_SMD_DELAY_CNTR          (KEY_SMD_EXE_STATE + 1)
+
+#define KEY_BREAK (81)
+#if KEY_SMD_DELAY_CNTR != KEY_BREAK
+#error
+#endif
+
+/* MPU6050 keys */
+#define KEY_CFG_ACCEL_FILTER        (KEY_BREAK + 1)
+#define KEY_CFG_MOTION_BIAS         (KEY_CFG_ACCEL_FILTER + 1)
+#define KEY_TEMPLABEL               (KEY_CFG_MOTION_BIAS + 1)
+
+#define KEY_D_0_22                  (KEY_TEMPLABEL + 1)
+#define KEY_D_0_24                  (KEY_D_0_22 + 1)
+#define KEY_D_0_36                  (KEY_D_0_24 + 1)
+#define KEY_D_0_52                  (KEY_D_0_36 + 1)
+#define KEY_D_0_96                  (KEY_D_0_52 + 1)
+#define KEY_D_0_104                 (KEY_D_0_96 + 1)
+#define KEY_D_0_108                 (KEY_D_0_104 + 1)
+#define KEY_D_0_163                 (KEY_D_0_108 + 1)
+#define KEY_D_0_188                 (KEY_D_0_163 + 1)
+#define KEY_D_0_192                 (KEY_D_0_188 + 1)
+#define KEY_D_0_224                 (KEY_D_0_192 + 1)
+#define KEY_D_0_228                 (KEY_D_0_224 + 1)
+#define KEY_D_0_232                 (KEY_D_0_228 + 1)
+#define KEY_D_0_236                 (KEY_D_0_232 + 1)
+
+#define KEY_DMP_PREVPTAT            (KEY_D_0_236 + 1)
+#define KEY_D_1_2                   (KEY_DMP_PREVPTAT + 1)
+#define KEY_D_1_4                   (KEY_D_1_2 + 1)
+#define KEY_D_1_8                   (KEY_D_1_4 + 1)
+#define KEY_D_1_10                  (KEY_D_1_8 + 1)
+#define KEY_D_1_24                  (KEY_D_1_10 + 1)
+#define KEY_D_1_28                  (KEY_D_1_24 + 1)
+#define KEY_D_1_36                  (KEY_D_1_28 + 1)
+#define KEY_D_1_40                  (KEY_D_1_36 + 1)
+#define KEY_D_1_44                  (KEY_D_1_40 + 1)
+#define KEY_D_1_72                  (KEY_D_1_44 + 1)
+#define KEY_D_1_74                  (KEY_D_1_72 + 1)
+#define KEY_D_1_79                  (KEY_D_1_74 + 1)
+#define KEY_D_1_88                  (KEY_D_1_79 + 1)
+#define KEY_D_1_90                  (KEY_D_1_88 + 1)
+#define KEY_D_1_92                  (KEY_D_1_90 + 1)
+#define KEY_D_1_96                  (KEY_D_1_92 + 1)
+#define KEY_D_1_98                  (KEY_D_1_96 + 1)
+#define KEY_D_1_100                 (KEY_D_1_98 + 1)
+#define KEY_D_1_106                 (KEY_D_1_100 + 1)
+#define KEY_D_1_108                 (KEY_D_1_106 + 1)
+#define KEY_D_1_112                 (KEY_D_1_108 + 1)
+#define KEY_D_1_128                 (KEY_D_1_112 + 1)
+#define KEY_D_1_152                 (KEY_D_1_128 + 1)
+#define KEY_D_1_160                 (KEY_D_1_152 + 1)
+#define KEY_D_1_168                 (KEY_D_1_160 + 1)
+#define KEY_D_1_175                 (KEY_D_1_168 + 1)
+#define KEY_D_1_176                 (KEY_D_1_175 + 1)
+#define KEY_D_1_178                 (KEY_D_1_176 + 1)
+#define KEY_D_1_179                 (KEY_D_1_178 + 1)
+#define KEY_D_1_218                 (KEY_D_1_179 + 1)
+#define KEY_D_1_232                 (KEY_D_1_218 + 1)
+#define KEY_D_1_236                 (KEY_D_1_232 + 1)
+#define KEY_D_1_240                 (KEY_D_1_236 + 1)
+#define KEY_D_1_244                 (KEY_D_1_240 + 1)
+#define KEY_D_1_250                 (KEY_D_1_244 + 1)
+#define KEY_D_1_252                 (KEY_D_1_250 + 1)
+#define KEY_D_2_12                  (KEY_D_1_252 + 1)
+#define KEY_D_2_96                  (KEY_D_2_12 + 1)
+#define KEY_D_2_108                 (KEY_D_2_96 + 1)
+#define KEY_D_2_208                 (KEY_D_2_108 + 1)
+#define KEY_FLICK_MSG               (KEY_D_2_208 + 1)
+#define KEY_FLICK_COUNTER           (KEY_FLICK_MSG + 1)
+#define KEY_FLICK_LOWER             (KEY_FLICK_COUNTER + 1)
+#define KEY_CFG_FLICK_IN            (KEY_FLICK_LOWER + 1)
+#define KEY_FLICK_UPPER             (KEY_CFG_FLICK_IN + 1)
+#define KEY_CGNOTICE_INTR           (KEY_FLICK_UPPER + 1)
+#define KEY_D_2_224                 (KEY_CGNOTICE_INTR + 1)
+#define KEY_D_2_244                 (KEY_D_2_224 + 1)
+#define KEY_D_2_248                 (KEY_D_2_244 + 1)
+#define KEY_D_2_252                 (KEY_D_2_248 + 1)
+
+#define KEY_D_GYRO_BIAS_X               (KEY_D_2_252 + 1)
+#define KEY_D_GYRO_BIAS_Y               (KEY_D_GYRO_BIAS_X + 1)
+#define KEY_D_GYRO_BIAS_Z               (KEY_D_GYRO_BIAS_Y + 1)
+#define KEY_D_ACC_BIAS_X                (KEY_D_GYRO_BIAS_Z + 1)
+#define KEY_D_ACC_BIAS_Y                (KEY_D_ACC_BIAS_X + 1)
+#define KEY_D_ACC_BIAS_Z                (KEY_D_ACC_BIAS_Y + 1)
+#define KEY_D_GYRO_ENABLE               (KEY_D_ACC_BIAS_Z + 1)
+#define KEY_D_ACCEL_ENABLE              (KEY_D_GYRO_ENABLE + 1)
+#define KEY_D_QUAT_ENABLE               (KEY_D_ACCEL_ENABLE + 1)
+#define KEY_D_OUTPUT_ENABLE             (KEY_D_QUAT_ENABLE + 1)
+#define KEY_D_ACCEL_CNTR                (KEY_D_OUTPUT_ENABLE + 1)
+#define KEY_D_GYRO_CNTR                 (KEY_D_ACCEL_CNTR + 1)
+#define KEY_D_QUAT0_CNTR                (KEY_D_GYRO_CNTR + 1)
+#define KEY_D_QUAT1_CNTR                (KEY_D_QUAT0_CNTR + 1)
+#define KEY_D_QUAT2_CNTR                (KEY_D_QUAT1_CNTR + 1)
+#define KEY_D_CR_TIME_G                 (KEY_D_QUAT2_CNTR + 1)
+#define KEY_D_CR_TIME_A                 (KEY_D_CR_TIME_G + 1)
+#define KEY_D_CR_TIME_Q                 (KEY_D_CR_TIME_A + 1)
+#define KEY_D_CS_TAX                    (KEY_D_CR_TIME_Q + 1)
+#define KEY_D_CS_TAY                    (KEY_D_CS_TAX + 1)
+#define KEY_D_CS_TAZ                    (KEY_D_CS_TAY + 1)
+#define KEY_D_CS_TGX                    (KEY_D_CS_TAZ + 1)
+#define KEY_D_CS_TGY                    (KEY_D_CS_TGX + 1)
+#define KEY_D_CS_TGZ                    (KEY_D_CS_TGY + 1)
+#define KEY_D_CS_TQ0                    (KEY_D_CS_TGZ + 1)
+#define KEY_D_CS_TQ1                    (KEY_D_CS_TQ0 + 1)
+#define KEY_D_CS_TQ2                    (KEY_D_CS_TQ1 + 1)
+#define KEY_D_CS_TQ3                    (KEY_D_CS_TQ2 + 1)
+
+/* Compass keys */
+#define KEY_CPASS_GAIN              (KEY_D_CS_TQ3 + 1)
+#define KEY_CPASS_BIAS_X            (KEY_CPASS_GAIN + 1)
+#define KEY_CPASS_BIAS_Y            (KEY_CPASS_BIAS_X + 1)
+#define KEY_CPASS_BIAS_Z            (KEY_CPASS_BIAS_Y + 1)
+#define KEY_CPASS_MTX_00            (KEY_CPASS_BIAS_Z + 1)
+#define KEY_CPASS_MTX_01            (KEY_CPASS_MTX_00 + 1)
+#define KEY_CPASS_MTX_02            (KEY_CPASS_MTX_01 + 1)
+#define KEY_CPASS_MTX_10            (KEY_CPASS_MTX_02 + 1)
+#define KEY_CPASS_MTX_11            (KEY_CPASS_MTX_10 + 1)
+#define KEY_CPASS_MTX_12            (KEY_CPASS_MTX_11 + 1)
+#define KEY_CPASS_MTX_20            (KEY_CPASS_MTX_12 + 1)
+#define KEY_CPASS_MTX_21            (KEY_CPASS_MTX_20 + 1)
+#define KEY_CPASS_MTX_22            (KEY_CPASS_MTX_21 + 1)
+
+/* Tap Keys */
+#define KEY_DMP_TAP_GATE              (KEY_CPASS_MTX_22 + 1)
+#define KEY_DMP_TAPW_MIN              (KEY_DMP_TAP_GATE + 1)
+#define KEY_DMP_TAP_THR_Z             (KEY_DMP_TAPW_MIN + 1)
+#define KEY_DMP_TAP_PREV_JERK_Z       (KEY_DMP_TAP_THR_Z + 1)
+#define KEY_DMP_TAP_MIN_TAPS          (KEY_DMP_TAP_PREV_JERK_Z + 1)
+#define KEY_DMP_TAP_NEXT_TAP_THRES    (KEY_DMP_TAP_MIN_TAPS + 1)
+#define KEY_DMP_TAP_SHAKE_REJECT      (KEY_DMP_TAP_NEXT_TAP_THRES + 1)
+#define KEY_DMP_TAP_SHAKE_COUNT_MAX   (KEY_DMP_TAP_SHAKE_REJECT + 1)
+#define KEY_DMP_TAP_SHAKE_TIMEOUT_MAX (KEY_DMP_TAP_SHAKE_COUNT_MAX + 1)
+#define KEY_DMP_TAP_DIRECTION         (KEY_DMP_TAP_SHAKE_TIMEOUT_MAX + 1)
+#define KEY_DMP_TAP_COUNT             (KEY_DMP_TAP_DIRECTION + 1)
+/* Gesture Keys */
+#define KEY_DMP_SH_TH_Y             (KEY_DMP_TAP_COUNT + 1)
+#define KEY_DMP_SH_TH_X             (KEY_DMP_SH_TH_Y + 1)
+#define KEY_DMP_SH_TH_Z             (KEY_DMP_SH_TH_X + 1)
+#define KEY_DMP_ORIENT              (KEY_DMP_SH_TH_Z + 1)
+#define KEY_D_ACT0                  (KEY_DMP_ORIENT + 1)
+#define KEY_D_ACSX                  (KEY_D_ACT0 + 1)
+#define KEY_D_ACSY                  (KEY_D_ACSX + 1)
+#define KEY_D_ACSZ                  (KEY_D_ACSY + 1)
+
+#define KEY_CFG_DISPLAY_ORIENT_INT  (KEY_D_ACSZ + 1)
+#define KEY_NO_ORIENT_INTERRUPT     (KEY_CFG_DISPLAY_ORIENT_INT + 1)
+#define KEY_X_GRT_Y_TMP2            (KEY_NO_ORIENT_INTERRUPT + 1)
+
+/*Shake Keys */
+#define KEY_D_0_64                  (KEY_X_GRT_Y_TMP2 + 1)
+#define KEY_D_2_4                   (KEY_D_0_64 + 1)
+#define KEY_D_2_8                   (KEY_D_2_4 + 1)
+#define KEY_D_2_48                  (KEY_D_2_8 + 1)
+#define KEY_D_2_92                  (KEY_D_2_48 + 1)
+#define KEY_D_2_94                  (KEY_D_2_92 + 1)
+#define KEY_D_2_160                 (KEY_D_2_94 + 1)
+#define KEY_D_3_180                 (KEY_D_2_160 + 1)
+#define KEY_D_3_184                 (KEY_D_3_180 + 1)
+#define KEY_D_3_188                 (KEY_D_3_184 + 1)
+#define KEY_D_3_208                 (KEY_D_3_188 + 1)
+#define KEY_D_3_240                 (KEY_D_3_208 + 1)
+#define KEY_RETRACTION_1            (KEY_D_3_240 + 1)
+#define KEY_RETRACTION_2            (KEY_RETRACTION_1 + 1)
+#define KEY_RETRACTION_3            (KEY_RETRACTION_2 + 1)
+#define KEY_RETRACTION_4            (KEY_RETRACTION_3 + 1)
+#define KEY_CFG_SHAKE_INT           (KEY_RETRACTION_4 + 1)
+
+/* Authenticate Keys */
+#define KEY_D_AUTH_OUT              (KEY_CFG_SHAKE_INT + 1)
+#define KEY_D_AUTH_IN               (KEY_D_AUTH_OUT + 1)
+#define KEY_D_AUTH_A                (KEY_D_AUTH_IN + 1)
+#define KEY_D_AUTH_B                (KEY_D_AUTH_A + 1)
+
+/* Pedometer standalone only keys */
+#define KEY_D_PEDSTD_BP_B           (KEY_D_AUTH_B + 1)
+#define KEY_D_PEDSTD_HP_A           (KEY_D_PEDSTD_BP_B + 1)
+#define KEY_D_PEDSTD_HP_B           (KEY_D_PEDSTD_HP_A + 1)
+#define KEY_D_PEDSTD_BP_A4          (KEY_D_PEDSTD_HP_B + 1)
+#define KEY_D_PEDSTD_BP_A3          (KEY_D_PEDSTD_BP_A4 + 1)
+#define KEY_D_PEDSTD_BP_A2          (KEY_D_PEDSTD_BP_A3 + 1)
+#define KEY_D_PEDSTD_BP_A1          (KEY_D_PEDSTD_BP_A2 + 1)
+#define KEY_D_PEDSTD_INT_THRSH      (KEY_D_PEDSTD_BP_A1 + 1)
+#define KEY_D_PEDSTD_CLIP           (KEY_D_PEDSTD_INT_THRSH + 1)
+#define KEY_D_PEDSTD_SB             (KEY_D_PEDSTD_CLIP + 1)
+#define KEY_D_PEDSTD_SB_TIME        (KEY_D_PEDSTD_SB + 1)
+#define KEY_D_PEDSTD_PEAKTHRSH      (KEY_D_PEDSTD_SB_TIME + 1)
+#define KEY_D_PEDSTD_TIML           (KEY_D_PEDSTD_PEAKTHRSH + 1)
+#define KEY_D_PEDSTD_TIMH           (KEY_D_PEDSTD_TIML + 1)
+#define KEY_D_PEDSTD_PEAK           (KEY_D_PEDSTD_TIMH + 1)
+#define KEY_D_PEDSTD_TIMECTR        (KEY_D_PEDSTD_PEAK + 1)
+#define KEY_D_PEDSTD_STEPCTR        (KEY_D_PEDSTD_TIMECTR + 1)
+#define KEY_D_PEDSTD_STEPCTR2          (KEY_D_PEDSTD_STEPCTR + 1)
+#define KEY_D_PEDSTD_WALKTIME       (KEY_D_PEDSTD_STEPCTR2 + 1)
+#define KEY_D_PEDSTD_DECI           (KEY_D_PEDSTD_WALKTIME + 1)
+#define KEY_D_PEDSTD_SB2                       (KEY_D_PEDSTD_DECI + 1)
+#define KEY_D_PEDSTD_DRIVE_STATE    (KEY_D_PEDSTD_SB2 + 1)
+#define KEY_CFG_PED_INT             (KEY_D_PEDSTD_DRIVE_STATE + 1)
+#define KEY_CFG_PED_ENABLE          (KEY_CFG_PED_INT + 1)
+#define KEY_D_STPDET_TIMESTAMP      (KEY_CFG_PED_ENABLE + 1)
+
+/*Host Based No Motion*/
+#define KEY_D_HOST_NO_MOT           (KEY_D_STPDET_TIMESTAMP + 1)
+
+/*Host Based Accel Bias*/
+#define KEY_D_ACCEL_BIAS            (KEY_D_HOST_NO_MOT + 1)
+
+/*Screen/Display Orientation Keys*/
+#define KEY_D_ORIENT_GAP            (KEY_D_ACCEL_BIAS + 1)
+#define KEY_D_TILT0_H               (KEY_D_ORIENT_GAP + 1)
+#define KEY_D_TILT0_L               (KEY_D_TILT0_H + 1)
+#define KEY_D_TILT1_H               (KEY_D_TILT0_L + 1)
+#define KEY_D_TILT1_L               (KEY_D_TILT1_H + 1)
+#define KEY_D_TILT2_H               (KEY_D_TILT1_L + 1)
+#define KEY_D_TILT2_L               (KEY_D_TILT2_H  + 1)
+#define KEY_D_TILT3_H               (KEY_D_TILT2_L + 1)
+#define KEY_D_TILT3_L               (KEY_D_TILT3_H + 1)
+
+#define KEY_STREAM_P_ACCEL_Z        (KEY_D_TILT3_L + 1)
+
+/* Batch mode */
+#define KEY_BM_ENABLE               (KEY_STREAM_P_ACCEL_Z + 1)
+#define KEY_BM_BATCH_THLD           (KEY_BM_ENABLE + 1)
+#define KEY_BM_BATCH_CNTR           (KEY_BM_BATCH_THLD + 1)
+#define KEY_BM_NUMWORD_TOFILL       (KEY_BM_BATCH_CNTR + 1)
+
+/* Watermark */
+#define KEY_CFG_WATERMARK_H         (KEY_BM_NUMWORD_TOFILL + 1)
+#define KEY_CFG_WATERMARK_L         (KEY_CFG_WATERMARK_H + 1)
+
+/* FIFO output control */
+#define KEY_CFG_OUT_ACCL            (KEY_CFG_WATERMARK_L + 1)
+#define KEY_CFG_OUT_GYRO            (KEY_CFG_OUT_ACCL + 1)
+#define KEY_CFG_OUT_3QUAT           (KEY_CFG_OUT_GYRO + 1)
+#define KEY_CFG_OUT_6QUAT           (KEY_CFG_OUT_3QUAT + 1)
+#define KEY_CFG_OUT_9QUAT           (KEY_CFG_OUT_6QUAT + 1)
+#define KEY_CFG_OUT_PQUAT           (KEY_CFG_OUT_9QUAT + 1)
+#define KEY_CFG_OUT_PQUAT9          (KEY_CFG_OUT_PQUAT + 1)
+#define KEY_CFG_OUT_CPASS           (KEY_CFG_OUT_PQUAT9 + 1)
+#define KEY_CFG_OUT_PRESS           (KEY_CFG_OUT_CPASS + 1)
+#define KEY_CFG_OUT_STEPDET         (KEY_CFG_OUT_PRESS + 1)
+#define KEY_CFG_FIFO_INT            (KEY_CFG_OUT_STEPDET + 1)
+
+/* Ped Step detection */
+#define KEY_CFG_PEDSTEP_DET         (KEY_CFG_FIFO_INT + 1)
+
+/* Screen Orientation data */
+#define KEY_SO_DATA                 (KEY_CFG_PEDSTEP_DET + 1)
+
+/* MPU for DMP Android K */
+#define KEY_P_INIT                  (KEY_SO_DATA + 1)
+#define KEY_P_HW_ID                 (KEY_P_INIT + 1)
+
+/* DMP running counter */
+#define KEY_DMP_RUN_CNTR            (KEY_P_HW_ID + 1)
+
+/* Sensor's ODR */
+#define KEY_CFG_3QUAT_ODR           (KEY_DMP_RUN_CNTR + 1)
+#define KEY_CFG_6QUAT_ODR           (KEY_CFG_3QUAT_ODR + 1)
+#define KEY_CFG_9QUAT_ODR           (KEY_CFG_6QUAT_ODR + 1)
+#define KEY_CFG_PQUAT6_ODR          (KEY_CFG_9QUAT_ODR + 1)
+#define KEY_CFG_PQUAT9_ODR          (KEY_CFG_PQUAT6_ODR + 1)
+#define KEY_CFG_ACCL_ODR            (KEY_CFG_PQUAT9_ODR + 1)
+#define KEY_CFG_GYRO_ODR            (KEY_CFG_ACCL_ODR + 1)
+#define KEY_CFG_CPASS_ODR           (KEY_CFG_GYRO_ODR + 1)
+#define KEY_CFG_PRESS_ODR           (KEY_CFG_CPASS_ODR + 1)
+
+#define KEY_ODR_CNTR_3QUAT          (KEY_CFG_PRESS_ODR + 1)
+#define KEY_ODR_CNTR_6QUAT          (KEY_ODR_CNTR_3QUAT + 1)
+#define KEY_ODR_CNTR_9QUAT          (KEY_ODR_CNTR_6QUAT + 1)
+#define KEY_ODR_CNTR_PQUAT          (KEY_ODR_CNTR_9QUAT + 1)
+#define KEY_ODR_CNTR_PQUAT9         (KEY_ODR_CNTR_PQUAT + 1)
+#define KEY_ODR_CNTR_ACCL           (KEY_ODR_CNTR_PQUAT9 + 1)
+#define KEY_ODR_CNTR_GYRO           (KEY_ODR_CNTR_ACCL + 1)
+#define KEY_ODR_CNTR_CPASS          (KEY_ODR_CNTR_GYRO + 1)
+#define KEY_ODR_CNTR_PRESS          (KEY_ODR_CNTR_CPASS + 1)
+
+/* DMP fusion LP-Quat */
+#define KEY_DMP_LPQ0                (KEY_ODR_CNTR_PRESS + 1)
+#define KEY_DMP_LPQ1                (KEY_DMP_LPQ0 + 1)
+#define KEY_DMP_LPQ2                (KEY_DMP_LPQ1 + 1)
+#define KEY_DMP_LPQ3                (KEY_DMP_LPQ2 + 1)
+
+/* DMP fusion 6-axis Quat */
+#define KEY_DMP_Q0                  (KEY_DMP_LPQ3 + 1)
+#define KEY_DMP_Q1                  (KEY_DMP_Q0 + 1)
+#define KEY_DMP_Q2                  (KEY_DMP_Q1 + 1)
+#define KEY_DMP_Q3                  (KEY_DMP_Q2 + 1)
+
+/* 9-axis fusion */
+#define KEY_CPASS_VALID             (KEY_DMP_Q3 + 1)
+#define KEY_9AXIS_ACCURACY          (KEY_CPASS_VALID + 1)
+#define KEY_DMP_9Q0                 (KEY_9AXIS_ACCURACY + 1)
+#define KEY_DMP_9Q1                 (KEY_DMP_9Q0 + 1)
+#define KEY_DMP_9Q2                 (KEY_DMP_9Q1 + 1)
+#define KEY_DMP_9Q3                 (KEY_DMP_9Q2 + 1)
+
+/* Test key */
+#define KEY_TEST_01                 (KEY_9AXIS_ACCURACY + 1)
+#define KEY_TEST_02                 (KEY_TEST_01 + 1)
+#define KEY_TEST_03                 (KEY_TEST_02 + 1)
+#define KEY_TEST_04                 (KEY_TEST_03 + 1)
+#define KEY_TEST_05                 (KEY_TEST_04 + 1)
+#define KEY_TEST_06                 (KEY_TEST_05 + 1)
+#define KEY_TEST_07                 (KEY_TEST_06 + 1)
+#define KEY_TEST_XX                 (KEY_TEST_07 + 1)
+
+#define NUM_KEYS                    (KEY_TEST_XX + 1)
+
+struct t_key_label {
+       unsigned short key;
+       unsigned short addr;
+};
+
+#define DINA0A 0x0a
+#define DINA22 0x22
+#define DINA42 0x42
+#define DINA5A 0x5a
+
+#define DINA06 0x06
+#define DINA0E 0x0e
+#define DINA16 0x16
+#define DINA1E 0x1e
+#define DINA26 0x26
+#define DINA2E 0x2e
+#define DINA36 0x36
+#define DINA3E 0x3e
+#define DINA46 0x46
+#define DINA4E 0x4e
+#define DINA56 0x56
+#define DINA5E 0x5e
+#define DINA66 0x66
+#define DINA6E 0x6e
+#define DINA76 0x76
+#define DINA7E 0x7e
+
+#define DINA00 0x00
+#define DINA08 0x08
+#define DINA10 0x10
+#define DINA18 0x18
+#define DINA20 0x20
+#define DINA28 0x28
+#define DINA30 0x30
+#define DINA38 0x38
+#define DINA40 0x40
+#define DINA48 0x48
+#define DINA50 0x50
+#define DINA58 0x58
+#define DINA60 0x60
+#define DINA68 0x68
+#define DINA70 0x70
+#define DINA78 0x78
+
+#define DINA04 0x04
+#define DINA0C 0x0c
+#define DINA14 0x14
+#define DINA1C 0x1C
+#define DINA24 0x24
+#define DINA2C 0x2c
+#define DINA34 0x34
+#define DINA3C 0x3c
+#define DINA44 0x44
+#define DINA4C 0x4c
+#define DINA54 0x54
+#define DINA5C 0x5c
+#define DINA64 0x64
+#define DINA6C 0x6c
+#define DINA74 0x74
+#define DINA7C 0x7c
+
+#define DINA01 0x01
+#define DINA09 0x09
+#define DINA11 0x11
+#define DINA19 0x19
+#define DINA21 0x21
+#define DINA29 0x29
+#define DINA31 0x31
+#define DINA39 0x39
+#define DINA41 0x41
+#define DINA49 0x49
+#define DINA51 0x51
+#define DINA59 0x59
+#define DINA61 0x61
+#define DINA69 0x69
+#define DINA71 0x71
+#define DINA79 0x79
+
+#define DINA25 0x25
+#define DINA2D 0x2d
+#define DINA35 0x35
+#define DINA3D 0x3d
+#define DINA4D 0x4d
+#define DINA55 0x55
+#define DINA5D 0x5D
+#define DINA6D 0x6d
+#define DINA75 0x75
+#define DINA7D 0x7d
+
+#define DINADC 0xdc
+#define DINAF2 0xf2
+#define DINAAB 0xab
+#define DINAAA 0xaa
+#define DINAF1 0xf1
+#define DINADF 0xdf
+#define DINADA 0xda
+#define DINAB1 0xb1
+#define DINAB9 0xb9
+#define DINAF3 0xf3
+#define DINA8B 0x8b
+#define DINAA3 0xa3
+#define DINA91 0x91
+#define DINAB6 0xb6
+#define DINAB4 0xb4
+
+#define DINC00 0x00
+#define DINC01 0x01
+#define DINC02 0x02
+#define DINC03 0x03
+#define DINC08 0x08
+#define DINC09 0x09
+#define DINC0A 0x0a
+#define DINC0B 0x0b
+#define DINC10 0x10
+#define DINC11 0x11
+#define DINC12 0x12
+#define DINC13 0x13
+#define DINC18 0x18
+#define DINC19 0x19
+#define DINC1A 0x1a
+#define DINC1B 0x1b
+
+#define DINC20 0x20
+#define DINC21 0x21
+#define DINC22 0x22
+#define DINC23 0x23
+#define DINC28 0x28
+#define DINC29 0x29
+#define DINC2A 0x2a
+#define DINC2B 0x2b
+#define DINC30 0x30
+#define DINC31 0x31
+#define DINC32 0x32
+#define DINC33 0x33
+#define DINC38 0x38
+#define DINC39 0x39
+#define DINC3A 0x3a
+#define DINC3B 0x3b
+
+#define DINC40 0x40
+#define DINC41 0x41
+#define DINC42 0x42
+#define DINC43 0x43
+#define DINC48 0x48
+#define DINC49 0x49
+#define DINC4A 0x4a
+#define DINC4B 0x4b
+#define DINC50 0x50
+#define DINC51 0x51
+#define DINC52 0x52
+#define DINC53 0x53
+#define DINC58 0x58
+#define DINC59 0x59
+#define DINC5A 0x5a
+#define DINC5B 0x5b
+
+#define DINC60 0x60
+#define DINC61 0x61
+#define DINC62 0x62
+#define DINC63 0x63
+#define DINC68 0x68
+#define DINC69 0x69
+#define DINC6A 0x6a
+#define DINC6B 0x6b
+#define DINC70 0x70
+#define DINC71 0x71
+#define DINC72 0x72
+#define DINC73 0x73
+#define DINC78 0x78
+#define DINC79 0x79
+#define DINC7A 0x7a
+#define DINC7B 0x7b
+#define DIND40 0x40
+#define DINA80 0x80
+#define DINA90 0x90
+#define DINAA0 0xa0
+#define DINAC9 0xc9
+#define DINACB 0xcb
+#define DINACD 0xcd
+#define DINACF 0xcf
+#define DINAC8 0xc8
+#define DINACA 0xca
+#define DINACC 0xcc
+#define DINACE 0xce
+#define DINAD8 0xd8
+#define DINADD 0xdd
+#define DINAF8 0xf0
+#define DINAFE 0xfe
+
+#define DINBF8 0xf8
+#define DINAC0 0xb0
+#define DINAC1 0xb1
+#define DINAC2 0xb4
+#define DINAC3 0xb5
+#define DINAC4 0xb8
+#define DINAC5 0xb9
+#define DINBC0 0xc0
+#define DINBC2 0xc2
+#define DINBC4 0xc4
+#define DINBC6 0xc6
+
+#endif
diff --git a/drivers/iio/imu/nvi_mpu/dmpmap.h b/drivers/iio/imu/nvi_mpu/dmpmap.h
new file mode 100644 (file)
index 0000000..f19f3f2
--- /dev/null
@@ -0,0 +1,263 @@
+/* Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+* Copyright (C) 2012 Invensense, Inc.
+*
+* This software is licensed under the terms of the GNU General Public
+* License version 2, as published by the Free Software Foundation, and
+* may be copied, distributed, and modified under those terms.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*/
+
+#ifndef DMPMAP_H
+#define DMPMAP_H
+
+#define DMP_PTAT    0
+#define DMP_XGYR    2
+#define DMP_YGYR    4
+#define DMP_ZGYR    6
+#define DMP_XACC    8
+#define DMP_YACC    10
+#define DMP_ZACC    12
+#define DMP_ADC1    14
+#define DMP_ADC2    16
+#define DMP_ADC3    18
+#define DMP_BIASUNC    20
+#define DMP_FIFORT    22
+#define DMP_INVGSFH    24
+#define DMP_INVGSFL    26
+#define DMP_1H    28
+#define DMP_1L    30
+#define DMP_BLPFSTCH    32
+#define DMP_BLPFSTCL    34
+#define DMP_BLPFSXH    36
+#define DMP_BLPFSXL    38
+#define DMP_BLPFSYH    40
+#define DMP_BLPFSYL    42
+#define DMP_BLPFSZH    44
+#define DMP_BLPFSZL    46
+#define DMP_BLPFMTC    48
+#define DMP_SMC    50
+#define DMP_BLPFMXH    52
+#define DMP_BLPFMXL    54
+#define DMP_BLPFMYH    56
+#define DMP_BLPFMYL    58
+#define DMP_BLPFMZH    60
+#define DMP_BLPFMZL    62
+#define DMP_BLPFC    64
+#define DMP_SMCTH    66
+#define DMP_TAP_DIRECTION 68
+#define DMP_TAP_COUNT 70
+#define DMP_TAP_GATE    72
+#define DMP_TAP_MIN_TAPS    74
+#define DMP_BERR2NH    76
+#define DMP_SMCINC    78
+#define DMP_TAP_SHAKE_REJECT    80
+#define DMP_ANGVBXL    82
+#define DMP_TAP_SHAKE_COUNT_MAX    84
+#define DMP_TAP_SHAKE_TIMEOUT_MAX    86
+#define DMP_TAP_THZ    88
+#define DMP_TAPW_MIN    90
+#define DMP_TAP_PREV_JERK_Z    92
+#define DMP_TAP_NEXT_TAP_THRES    94
+#define DMP_ATCH    96
+#define DMP_BIASUNCSF    98
+#define DMP_ACT2H    100
+#define DMP_ACT2L    102
+#define DMP_GSFH    104
+#define DMP_GSFL    106
+#define DMP_GH    108
+#define DMP_GL    110
+#define DMP_0_5H    112
+#define DMP_0_5L    114
+#define DMP_0_0H    116
+#define DMP_0_0L    118
+#define DMP_1_0H    120
+#define DMP_1_0L    122
+#define DMP_1_5H    124
+#define DMP_1_5L    126
+#define DMP_TMP1AH    128
+#define DMP_TMP1AL    130
+#define DMP_TMP2AH    132
+#define DMP_TMP2AL    134
+#define DMP_TMP3AH    136
+#define DMP_TMP3AL    138
+#define DMP_TMP4AH    140
+#define DMP_TMP4AL    142
+#define DMP_XACCW    144
+#define DMP_TMP5    146
+#define DMP_XACCB    148
+#define DMP_TMP8    150
+#define DMP_YACCB    152
+#define DMP_TMP9    154
+#define DMP_ZACCB    156
+#define DMP_TMP10    158
+#define DMP_DZH    160
+#define DMP_DZL    162
+#define DMP_XGCH    164
+#define DMP_XGCL    166
+#define DMP_YGCH    168
+#define DMP_YGCL    170
+#define DMP_ZGCH    172
+#define DMP_ZGCL    174
+#define DMP_YACCW    176
+#define DMP_TMP7    178
+#define DMP_AFB1H    180
+#define DMP_AFB1L    182
+#define DMP_AFB2H    184
+#define DMP_AFB2L    186
+#define DMP_MAGFBH    188
+#define DMP_MAGFBL    190
+#define DMP_QT1H    192
+#define DMP_QT1L    194
+#define DMP_QT2H    196
+#define DMP_QT2L    198
+#define DMP_QT3H    200
+#define DMP_QT3L    202
+#define DMP_QT4H    204
+#define DMP_QT4L    206
+#define DMP_CTRL1H    208
+#define DMP_CTRL1L    210
+#define DMP_CTRL2H    212
+#define DMP_CTRL2L    214
+#define DMP_CTRL3H    216
+#define DMP_CTRL3L    218
+#define DMP_CTRL4H    220
+#define DMP_CTRL4L    222
+#define DMP_CTRLS1    224
+#define DMP_CTRLSF1    226
+#define DMP_CTRLS2    228
+#define DMP_CTRLSF2    230
+#define DMP_CTRLS3    232
+#define DMP_CTRLSFNLL    234
+#define DMP_CTRLS4    236
+#define DMP_CTRLSFNL2    238
+#define DMP_CTRLSFNL    240
+#define DMP_TMP30    242
+#define DMP_CTRLSFJT    244
+#define DMP_TMP31    246
+#define DMP_TMP11    248
+#define DMP_CTRLSF2_2    250
+#define DMP_TMP12    252
+#define DMP_CTRLSF1_2    254
+#define DMP_PREVPTAT    256
+#define DMP_ACCZB    258
+#define DMP_ACCXB    264
+#define DMP_ACCYB    266
+#define DMP_1HB    272
+#define DMP_1LB    274
+#define DMP_0H    276
+#define DMP_0L    278
+#define DMP_ASR22H    280
+#define DMP_ASR22L    282
+#define DMP_ASR6H    284
+#define DMP_ASR6L    286
+#define DMP_TMP13    288
+#define DMP_TMP14    290
+#define DMP_FINTXH    292
+#define DMP_FINTXL    294
+#define DMP_FINTYH    296
+#define DMP_FINTYL    298
+#define DMP_FINTZH    300
+#define DMP_FINTZL    302
+#define DMP_TMP1BH    304
+#define DMP_TMP1BL    306
+#define DMP_TMP2BH    308
+#define DMP_TMP2BL    310
+#define DMP_TMP3BH    312
+#define DMP_TMP3BL    314
+#define DMP_TMP4BH    316
+#define DMP_TMP4BL    318
+#define DMP_STXG    320
+#define DMP_ZCTXG    322
+#define DMP_STYG    324
+#define DMP_ZCTYG    326
+#define DMP_STZG    328
+#define DMP_ZCTZG    330
+#define DMP_CTRLSFJT2    332
+#define DMP_CTRLSFJTCNT    334
+#define DMP_PVXG    336
+#define DMP_TMP15    338
+#define DMP_PVYG    340
+#define DMP_TMP16    342
+#define DMP_PVZG    344
+#define DMP_TMP17    346
+#define DMP_MNMFLAGH    352
+#define DMP_MNMFLAGL    354
+#define DMP_MNMTMH    356
+#define DMP_MNMTML    358
+#define DMP_MNMTMTHRH    360
+#define DMP_MNMTMTHRL    362
+#define DMP_MNMTHRH    364
+#define DMP_MNMTHRL    366
+#define DMP_ACCQD4H    368
+#define DMP_ACCQD4L    370
+#define DMP_ACCQD5H    372
+#define DMP_ACCQD5L    374
+#define DMP_ACCQD6H    376
+#define DMP_ACCQD6L    378
+#define DMP_ACCQD7H    380
+#define DMP_ACCQD7L    382
+#define DMP_ACCQD0H    384
+#define DMP_ACCQD0L    386
+#define DMP_ACCQD1H    388
+#define DMP_ACCQD1L    390
+#define DMP_ACCQD2H    392
+#define DMP_ACCQD2L    394
+#define DMP_ACCQD3H    396
+#define DMP_ACCQD3L    398
+#define DMP_XN2H    400
+#define DMP_XN2L    402
+#define DMP_XN1H    404
+#define DMP_XN1L    406
+#define DMP_YN2H    408
+#define DMP_YN2L    410
+#define DMP_YN1H    412
+#define DMP_YN1L    414
+#define DMP_YH    416
+#define DMP_YL    418
+#define DMP_B0H    420
+#define DMP_B0L    422
+#define DMP_A1H    424
+#define DMP_A1L    426
+#define DMP_A2H    428
+#define DMP_A2L    430
+#define DMP_SEM1    432
+#define DMP_FIFOCNT    434
+#define DMP_SH_TH_X    436
+#define DMP_PACKET    438
+#define DMP_SH_TH_Y    440
+#define DMP_FOOTER    442
+#define DMP_SH_TH_Z    444
+#define DMP_TEMP29    448
+#define DMP_TEMP30    450
+#define DMP_XACCB_PRE    452
+#define DMP_XACCB_PREL    454
+#define DMP_YACCB_PRE    456
+#define DMP_YACCB_PREL    458
+#define DMP_ZACCB_PRE    460
+#define DMP_ZACCB_PREL    462
+#define DMP_TMP22    464
+#define DMP_TAP_TIMER    466
+#define DMP_TAP_THX    468
+#define DMP_TAP_THY    472
+#define DMP_TMP25    480
+#define DMP_TMP26    482
+#define DMP_TMP27    484
+#define DMP_TMP28    486
+#define DMP_ORIENT    488
+#define DMP_THRSH    490
+#define DMP_ENDIANH    492
+#define DMP_ENDIANL    494
+#define DMP_BLPFNMTCH    496
+#define DMP_BLPFNMTCL    498
+#define DMP_BLPFNMXH    500
+#define DMP_BLPFNMXL    502
+#define DMP_BLPFNMYH    504
+#define DMP_BLPFNMYL    506
+#define DMP_BLPFNMZH    508
+#define DMP_BLPFNMZL    510
+
+#endif
diff --git a/drivers/iio/imu/nvi_mpu/invensense.c b/drivers/iio/imu/nvi_mpu/invensense.c
new file mode 100644 (file)
index 0000000..1345955
--- /dev/null
@@ -0,0 +1,2186 @@
+/* Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+* Copyright (C) 2012 Invensense, Inc.
+*
+* This software is licensed under the terms of the GNU General Public
+* License version 2, as published by the Free Software Foundation, and
+* may be copied, distributed, and modified under those terms.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*/
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/sysfs.h>
+#include <linux/jiffies.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/kfifo.h>
+#include <linux/poll.h>
+#include <linux/miscdevice.h>
+#include <linux/crc32.h>
+#include <linux/mpu_iio.h>
+#include <linux/iio/iio.h>
+
+#include "nvi.h"
+#include "dmpKey.h"
+
+#define mem_w(a, b, c) \
+       mpu_memory_write(st, st->i2c_addr, a, b, c)
+#define mem_w_key(key, b, c) mpu_memory_write_unaligned(st, key, b, c)
+
+enum inv_filter_e {
+       INV_FILTER_256HZ_NOLPF2 = 0,
+       INV_FILTER_188HZ,
+       INV_FILTER_98HZ,
+       INV_FILTER_42HZ,
+       INV_FILTER_20HZ,
+       INV_FILTER_10HZ,
+       INV_FILTER_5HZ,
+       INV_FILTER_2100HZ_NOLPF,
+       NUM_FILTER
+};
+
+/*==== MPU6050B1 MEMORY ====*/
+enum MPU_MEMORY_BANKS {
+       MEM_RAM_BANK_0 = 0,
+       MEM_RAM_BANK_1,
+       MEM_RAM_BANK_2,
+       MEM_RAM_BANK_3,
+       MEM_RAM_BANK_4,
+       MEM_RAM_BANK_5,
+       MEM_RAM_BANK_6,
+       MEM_RAM_BANK_7,
+       MEM_RAM_BANK_8,
+       MEM_RAM_BANK_9,
+       MEM_RAM_BANK_10,
+       MEM_RAM_BANK_11,
+       MPU_MEM_NUM_RAM_BANKS,
+       MPU_MEM_OTP_BANK_0 = 16
+};
+
+/* produces an unique identifier for each device based on the
+   combination of product version and product revision */
+struct prod_rev_map_t {
+       u16 mpl_product_key;
+       u8 silicon_rev;
+       u16 gyro_trim;
+       u16 accel_trim;
+};
+
+/* registers */
+#define REG_XA_OFFS_L_TC               (0x07)
+#define REG_PRODUCT_ID                 (0x0C)
+#define REG_ST_GCT_X                   (0x0D)
+#define REG_BANK_SEL                   (0x6D)
+#define REG_MEM_START_ADDR             (0x6E)
+#define REG_MEM_RW                     (0x6F)
+#define REG_PRGM_STRT_ADDRH            (0x70)
+#define REG_PRGM_STRT_ADDRL            (0x71)
+#define REG_FIFO_COUNT_H               (0x72)
+#define REG_FIFO_COUNT_L               (0x73)
+#define REG_FIFO_R_W                   (0x74)
+#define REG_WHO_AM_I                   (0x75)
+
+/* data definitions */
+#define DMP_START_ADDR           0x400
+#define DMP_MASK_TAP             0x3f
+#define DMP_MASK_DIS_ORIEN       0xC0
+#define DMP_DIS_ORIEN_SHIFT      6
+
+#define BYTES_FOR_DMP            8
+#define BYTES_FOR_EVENTS         4
+#define QUATERNION_BYTES         16
+#define BYTES_PER_SENSOR         6
+#define MPU3050_FOOTER_SIZE      2
+#define FIFO_COUNT_BYTE          2
+#define FIFO_SIZE                800
+#define HARDWARE_FIFO_SIZE       1024
+#define MAX_READ_SIZE            64
+#define SENSOR_UP_TIME           30
+#define INV_MPU_SAMPLE_RATE_CHANGE_STABLE 50
+#define MPU_MEM_BANK_SIZE        256
+
+/* data header defines */
+#define PRESSURE_HDR             0x8000
+#define ACCEL_HDR                0x4000
+#define GYRO_HDR                 0x2000
+#define COMPASS_HDR              0x1000
+#define LPQUAT_HDR               0x0800
+#define SIXQUAT_HDR              0x0400
+#define PEDQUAT_HDR              0x0200
+#define STEP_DETECTOR_HDR        0x0100
+#define STEP_INDICATOR_MASK      0xf
+
+#define MAX_BYTES_PER_SAMPLE     80
+#define MAX_HW_FIFO_BYTES        (BYTES_PER_SENSOR * 2)
+#define IIO_BUFFER_BYTES         8
+#define HEADERED_NORMAL_BYTES    8
+#define HEADERED_Q_BYTES         16
+
+#define MPU6XXX_MAX_MOTION_THRESH (255*4)
+#define MPU6050_MOTION_THRESH_SHIFT 5
+#define MPU6500_MOTION_THRESH_SHIFT 2
+#define MPU6050_MOTION_DUR_DEFAULT  1
+#define MPU6050_MAX_MOTION_DUR   255
+#define MPU_TEMP_SHIFT           16
+#define LPA_FREQ_SHIFT           6
+#define COMPASS_RATE_SCALE       10
+#define MAX_GYRO_FS_PARAM        3
+#define MAX_ACCEL_FS_PARAM        3
+#define MAX_LPA_FREQ_PARAM       3
+#define MPU_MAX_A_OFFSET_VALUE     16383
+#define MPU_MIN_A_OFFSET_VALUE     -16384
+#define MPU_MAX_G_OFFSET_VALUE     32767
+#define MPU_MIN_G_OFFSET_VALUE     -32767
+#define MPU6XXX_MAX_MPU_MEM      (256 * 12)
+
+#define INIT_MOT_DUR             128
+#define INIT_MOT_THR             128
+#define INIT_ZMOT_DUR            128
+#define INIT_ZMOT_THR            128
+#define INIT_ST_SAMPLES          200
+#define INIT_ST_MPU6050_SAMPLES  600
+#define INIT_ST_THRESHOLD        50
+#define INIT_PED_INT_THRESH      2
+#define INIT_PED_THRESH          7
+#define ST_THRESHOLD_MULTIPLIER  10
+#define ST_MAX_SAMPLES           500
+#define ST_MAX_THRESHOLD         100
+#define DMP_INTERVAL_INIT       (5 * NSEC_PER_MSEC)
+#define DMP_INTERVAL_MIN_ADJ    (50 * NSEC_PER_USEC)
+
+/*---- MPU6500 ----*/
+#define MPU6500_PRODUCT_REVISION 1
+#define MPU6500_MEM_REV_ADDR     0x16
+#define INV_MPU_REV_MASK         0x0F
+#define MPU6500_REV              2
+#define MPU_DMP_LOAD_START       0x20
+
+#define THREE_AXIS               3
+#define GYRO_CONFIG_FSR_SHIFT    3
+#define ACCEL_CONFIG_FSR_SHIFT    3
+#define GYRO_DPS_SCALE           250
+#define MEM_ADDR_PROD_REV        0x6
+#define SOFT_PROD_VER_BYTES      5
+#define CRC_FIRMWARE_SEED        0
+#define SELF_TEST_SUCCESS        1
+#define MS_PER_DMP_TICK          20
+#define DMP_IMAGE_SIZE           2463
+
+/* init parameters */
+#define INIT_FIFO_RATE           50
+#define INIT_DMP_OUTPUT_RATE     25
+#define INIT_DUR_TIME           (NSEC_PER_SEC / INIT_FIFO_RATE)
+#define INIT_TAP_THRESHOLD       100
+#define INIT_TAP_TIME            100
+#define INIT_TAP_MIN_COUNT       2
+#define INIT_SAMPLE_DIVIDER      4
+#define MPU_INIT_SMD_DELAY_THLD  3
+#define MPU_INIT_SMD_DELAY2_THLD 1
+#define MPU_INIT_SMD_THLD        1500
+#define MPU_DEFAULT_DMP_FREQ     200
+#define MPL_PROD_KEY(ver, rev)  (ver * 100 + rev)
+#define NUM_OF_PROD_REVS (ARRAY_SIZE(prod_rev_map))
+/*---- MPU6050 Silicon Revisions ----*/
+#define MPU_SILICON_REV_A2                    1       /* MPU6050A2 Device */
+#define MPU_SILICON_REV_B1                    2       /* MPU6050B1 Device */
+
+#define BIT_PRFTCH_EN                         0x40
+#define BIT_CFG_USER_BANK                     0x20
+#define BITS_MEM_SEL                          0x1f
+
+#define TIME_STAMP_TOR                        5
+#define MAX_CATCH_UP                          5
+#define DEFAULT_ACCEL_TRIM                    16384
+#define DEFAULT_GYRO_TRIM                     131
+#define MAX_FIFO_RATE                         1000
+#define MAX_DMP_OUTPUT_RATE                   200
+#define MIN_FIFO_RATE                         4
+#define ONE_K_HZ                              1000
+#define NS_PER_MS_SHIFT                       20
+#define END_MARKER                            0x0010
+#define EMPTY_MARKER                          0x0020
+
+/*tap related defines */
+#define INV_TAP                               0x08
+#define INV_NUM_TAP_AXES                      3
+
+#define INV_TAP_AXIS_X_POS                    0x20
+#define INV_TAP_AXIS_X_NEG                    0x10
+#define INV_TAP_AXIS_Y_POS                    0x08
+#define INV_TAP_AXIS_Y_NEG                    0x04
+#define INV_TAP_AXIS_Z_POS                    0x02
+#define INV_TAP_AXIS_Z_NEG                    0x01
+#define INV_TAP_ALL_DIRECTIONS                0x3f
+
+#define INV_TAP_AXIS_X                        0x1
+#define INV_TAP_AXIS_Y                        0x2
+#define INV_TAP_AXIS_Z                        0x4
+
+#define INV_TAP_AXIS_ALL               \
+               (INV_TAP_AXIS_X            |   \
+               INV_TAP_AXIS_Y             |   \
+               INV_TAP_AXIS_Z)
+
+#define INT_SRC_TAP             0x01
+#define INT_SRC_DISPLAY_ORIENT  0x08
+#define INT_SRC_SHAKE           0x10
+
+#define INV_X_AXIS_INDEX                  0x00
+#define INV_Y_AXIS_INDEX                  0x01
+#define INV_Z_AXIS_INDEX                  0x02
+
+#define INV_ELEMENT_1                     0x0001
+#define INV_ELEMENT_2                     0x0002
+#define INV_ELEMENT_3                     0x0004
+#define INV_ELEMENT_4                     0x0008
+#define INV_ELEMENT_5                     0x0010
+#define INV_ELEMENT_6                     0x0020
+#define INV_ELEMENT_7                     0x0040
+#define INV_ELEMENT_8                     0x0080
+#define INV_ALL                           0xFFFF
+#define INV_ELEMENT_MASK                  0x00FF
+#define INV_GYRO_ACC_MASK                 0x007E
+#define INV_ACCEL_MASK                    0x70
+#define INV_GYRO_MASK                     0xE
+
+
+/* DMP defines */
+#define DMP_ORIENTATION_TIME            500
+#define DMP_ORIENTATION_ANGLE           60
+#define DMP_DEFAULT_FIFO_RATE           200
+#define DMP_TAP_SCALE                   (767603923 / 5)
+#define DMP_MULTI_SHIFT                 30
+#define DMP_MULTI_TAP_TIME              500
+#define DMP_SHAKE_REJECT_THRESH         100
+#define DMP_SHAKE_REJECT_TIME           10
+#define DMP_SHAKE_REJECT_TIMEOUT        10
+#define DMP_ANGLE_SCALE                 15
+#define DMP_PRECISION                   1000
+#define DMP_MAX_DIVIDER                 4
+#define DMP_MAX_MIN_TAPS                4
+#define DMP_IMAGE_CRC_VALUE             0x972aae92
+
+/*--- Test parameters defaults --- */
+#define DEF_OLDEST_SUPP_PROD_REV        8
+#define DEF_OLDEST_SUPP_SW_REV          2
+
+/* sample rate */
+#define DEF_SELFTEST_SAMPLE_RATE        0
+/* full scale setting dps */
+#define DEF_SELFTEST_GYRO_FS            (0 << 3)
+#define DEF_SELFTEST_ACCEL_FS           (2 << 3)
+#define DEF_SELFTEST_GYRO_SENS          (32768 / 250)
+/* wait time before collecting data */
+#define DEF_GYRO_WAIT_TIME              10
+#define DEF_ST_STABLE_TIME              20
+#define DEF_ST_6500_STABLE_TIME         20
+#define DEF_GYRO_SCALE                  131
+#define DEF_ST_PRECISION                1000
+#define DEF_ST_ACCEL_FS_MG              8000UL
+#define DEF_ST_SCALE                    (1L << 15)
+#define DEF_ST_TRY_TIMES                2
+#define DEF_ST_COMPASS_RESULT_SHIFT     2
+#define DEF_ST_ACCEL_RESULT_SHIFT       1
+#define DEF_ST_OTP0_THRESH              60
+#define DEF_ST_ABS_THRESH               20
+#define DEF_ST_TOR                      2
+
+#define X                               0
+#define Y                               1
+#define Z                               2
+/*---- MPU6050 notable product revisions ----*/
+#define MPU_PRODUCT_KEY_B1_E1_5         105
+#define MPU_PRODUCT_KEY_B2_F1           431
+/* accelerometer Hw self test min and max bias shift (mg) */
+#define DEF_ACCEL_ST_SHIFT_MIN          300
+#define DEF_ACCEL_ST_SHIFT_MAX          950
+
+#define DEF_ACCEL_ST_SHIFT_DELTA        500
+#define DEF_GYRO_CT_SHIFT_DELTA         500
+/* gyroscope Coriolis self test min and max bias shift (dps) */
+#define DEF_GYRO_CT_SHIFT_MIN           10
+#define DEF_GYRO_CT_SHIFT_MAX           105
+
+/*---- MPU6500 Self Test Pass/Fail Criteria ----*/
+/* Gyro Offset Max Value (dps) */
+#define DEF_GYRO_OFFSET_MAX             20
+/* Gyro Self Test Absolute Limits ST_AL (dps) */
+#define DEF_GYRO_ST_AL                  60
+/* Accel Self Test Absolute Limits ST_AL (mg) */
+#define DEF_ACCEL_ST_AL_MIN             225
+#define DEF_ACCEL_ST_AL_MAX             675
+#define DEF_6500_ACCEL_ST_SHIFT_DELTA   500
+#define DEF_6500_GYRO_CT_SHIFT_DELTA    500
+#define DEF_ST_MPU6500_ACCEL_LPF        2
+#define DEF_ST_6500_ACCEL_FS_MG         2000UL
+#define DEF_SELFTEST_6500_ACCEL_FS      (0 << 3)
+
+/* Note: The ST_AL values are only used when ST_OTP = 0,
+ * i.e no factory self test values for reference
+ */
+
+/* NOTE: product entries are in chronological order */
+static const struct prod_rev_map_t prod_rev_map[] = {
+       /* prod_ver = 0 */
+       {MPL_PROD_KEY(0,   1), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,   2), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,   3), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,   4), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,   5), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,   6), MPU_SILICON_REV_A2, 131, 16384},
+       /* prod_ver = 1 */
+       {MPL_PROD_KEY(0,   7), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,   8), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,   9), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,  10), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,  11), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,  12), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,  13), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,  14), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,  15), MPU_SILICON_REV_A2, 131, 16384},
+       {MPL_PROD_KEY(0,  27), MPU_SILICON_REV_A2, 131, 16384},
+       /* prod_ver = 1 */
+       {MPL_PROD_KEY(1,  16), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,  17), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,  18), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,  19), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,  20), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,  28), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,   1), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,   2), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,   3), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,   4), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,   5), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(1,   6), MPU_SILICON_REV_B1, 131, 16384},
+       /* prod_ver = 2 */
+       {MPL_PROD_KEY(2,   7), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(2,   8), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(2,   9), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(2,  10), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(2,  11), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(2,  12), MPU_SILICON_REV_B1, 131, 16384},
+       {MPL_PROD_KEY(2,  29), MPU_SILICON_REV_B1, 131, 16384},
+       /* prod_ver = 3 */
+       {MPL_PROD_KEY(3,  30), MPU_SILICON_REV_B1, 131, 16384},
+       /* prod_ver = 4 */
+       {MPL_PROD_KEY(4,  31), MPU_SILICON_REV_B1, 131,  8192},
+       {MPL_PROD_KEY(4,   1), MPU_SILICON_REV_B1, 131,  8192},
+       {MPL_PROD_KEY(4,   3), MPU_SILICON_REV_B1, 131,  8192},
+       /* prod_ver = 5 */
+       {MPL_PROD_KEY(5,   3), MPU_SILICON_REV_B1, 131, 16384},
+       /* prod_ver = 6 */
+       {MPL_PROD_KEY(6,  19), MPU_SILICON_REV_B1, 131, 16384},
+       /* prod_ver = 7 */
+       {MPL_PROD_KEY(7,  19), MPU_SILICON_REV_B1, 131, 16384},
+       /* prod_ver = 8 */
+       {MPL_PROD_KEY(8,  19), MPU_SILICON_REV_B1, 131, 16384},
+       /* prod_ver = 9 */
+       {MPL_PROD_KEY(9,  19), MPU_SILICON_REV_B1, 131, 16384},
+       /* prod_ver = 10 */
+       {MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384}
+};
+
+/*
+*   List of product software revisions
+*
+*   NOTE :
+*   software revision 0 falls back to the old detection method
+*   based off the product version and product revision per the
+*   table above
+*/
+static const struct prod_rev_map_t sw_rev_map[] = {
+       {0,                  0,   0,     0},
+       {1, MPU_SILICON_REV_B1, 131,  8192},    /* rev C */
+       {2, MPU_SILICON_REV_B1, 131, 16384}     /* rev D */
+};
+
+static const u16 mpu_6500_st_tb[256] = {
+       2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
+       2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
+       3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
+       3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
+       3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
+       3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
+       4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
+       4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
+       4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
+       5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
+       5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
+       6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
+       6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
+       7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
+       7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
+       8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
+       9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
+       10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
+       10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
+       11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
+       12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
+       13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
+       15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
+       16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
+       17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
+       19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
+       20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
+       22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
+       24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
+       26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
+       28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
+       30903, 31212, 31524, 31839, 32157, 32479, 32804
+};
+
+static const int accel_st_tb[31] = {
+       340, 351, 363, 375, 388, 401, 414, 428,
+       443, 458, 473, 489, 506, 523, 541, 559,
+       578, 597, 617, 638, 660, 682, 705, 729,
+       753, 779, 805, 832, 860, 889, 919
+};
+
+static const int gyro_6050_st_tb[31] = {
+       3275, 3425, 3583, 3748, 3920, 4100, 4289, 4486,
+       4693, 4909, 5134, 5371, 5618, 5876, 6146, 6429,
+       6725, 7034, 7358, 7696, 8050, 8421, 8808, 9213,
+       9637, 10080, 10544, 11029, 11537, 12067, 12622
+};
+
+char *wr_pr_debug_begin(u8 const *data, u32 len, char *string)
+{
+       int ii;
+       string = kmalloc(len * 2 + 1, GFP_KERNEL);
+       for (ii = 0; ii < len; ii++)
+               sprintf(&string[ii * 2], "%02X", data[ii]);
+       string[len * 2] = 0;
+       return string;
+}
+
+char *wr_pr_debug_end(char *string)
+{
+       kfree(string);
+       return "";
+}
+
+int mpu_memory_write(struct nvi_state *st, u8 mpu_addr, u16 mem_addr,
+                    u32 len, u8 const *data)
+{
+       u8 bank[2];
+       u8 addr[2];
+       u8 buf[513];
+
+       struct i2c_msg msgs[3];
+       int res;
+
+       if (!data || !st)
+               return -EINVAL;
+
+       if (len >= (sizeof(buf) - 1))
+               return -ENOMEM;
+
+       bank[0] = REG_BANK_SEL;
+       bank[1] = mem_addr >> 8;
+
+       addr[0] = REG_MEM_START_ADDR;
+       addr[1] = mem_addr & 0xFF;
+
+       buf[0] = REG_MEM_RW;
+       memcpy(buf + 1, data, len);
+
+       /* write message */
+       msgs[0].addr = mpu_addr;
+       msgs[0].flags = 0;
+       msgs[0].buf = bank;
+       msgs[0].len = sizeof(bank);
+
+       msgs[1].addr = mpu_addr;
+       msgs[1].flags = 0;
+       msgs[1].buf = addr;
+       msgs[1].len = sizeof(addr);
+
+       msgs[2].addr = mpu_addr;
+       msgs[2].flags = 0;
+       msgs[2].buf = (u8 *)buf;
+       msgs[2].len = len + 1;
+
+#if CONFIG_DYNAMIC_DEBUG
+       {
+               char *write = 0;
+               pr_debug("%s WM%02X%02X%02X%s%s - %d\n", st->hal->part_name,
+                        mpu_addr, bank[1], addr[1],
+                        wr_pr_debug_begin(data, len, write),
+                        wr_pr_debug_end(write),
+                        len);
+       }
+#endif
+
+       res = i2c_transfer(st->i2c->adapter, msgs, 3);
+       if (res != 3) {
+               if (res >= 0)
+                       res = -EIO;
+               return res;
+       } else {
+               return 0;
+       }
+}
+
+int mpu_memory_read(struct nvi_state *st, u8 mpu_addr, u16 mem_addr,
+                   u32 len, u8 *data)
+{
+       u8 bank[2];
+       u8 addr[2];
+       u8 buf;
+
+       struct i2c_msg msgs[4];
+       int res;
+
+       if (!data || !st)
+               return -EINVAL;
+
+       bank[0] = REG_BANK_SEL;
+       bank[1] = mem_addr >> 8;
+
+       addr[0] = REG_MEM_START_ADDR;
+       addr[1] = mem_addr & 0xFF;
+
+       buf = REG_MEM_RW;
+
+       /* write message */
+       msgs[0].addr = mpu_addr;
+       msgs[0].flags = 0;
+       msgs[0].buf = bank;
+       msgs[0].len = sizeof(bank);
+
+       msgs[1].addr = mpu_addr;
+       msgs[1].flags = 0;
+       msgs[1].buf = addr;
+       msgs[1].len = sizeof(addr);
+
+       msgs[2].addr = mpu_addr;
+       msgs[2].flags = 0;
+       msgs[2].buf = &buf;
+       msgs[2].len = 1;
+
+       msgs[3].addr = mpu_addr;
+       msgs[3].flags = I2C_M_RD;
+       msgs[3].buf = data;
+       msgs[3].len = len;
+
+       res = i2c_transfer(st->i2c->adapter, msgs, 4);
+       if (res != 4) {
+               if (res >= 0)
+                       res = -EIO;
+       } else
+               res = 0;
+
+#if CONFIG_DYNAMIC_DEBUG
+       {
+               char *read = 0;
+               pr_debug("%s RM%02X%02X%02X%02X - %s%s\n", st->hal->part_name,
+                        mpu_addr, bank[1], addr[1], len,
+                        wr_pr_debug_begin(data, len, read),
+                        wr_pr_debug_end(read));
+       }
+#endif
+
+       return res;
+}
+
+int mpu_memory_write_unaligned(struct nvi_state *st, u16 key, int len,
+                                                               u8 const *d)
+{
+       u32 addr;
+       int start, end;
+       int len1, len2;
+       int result = 0;
+
+       if (len > MPU_MEM_BANK_SIZE)
+               return -EINVAL;
+       addr = inv_dmp_get_address(key);
+       if (addr > MPU6XXX_MAX_MPU_MEM)
+               return -EINVAL;
+
+       start = (addr >> 8);
+       end   = ((addr + len - 1) >> 8);
+       if (start == end) {
+               result = mpu_memory_write(st, st->i2c_addr, addr, len, d);
+       } else {
+               end <<= 8;
+               len1 = end - addr;
+               len2 = len - len1;
+               result = mpu_memory_write(st, st->i2c_addr, addr, len1, d);
+               result |= mpu_memory_write(st, st->i2c_addr, end, len2,
+                                                               d + len1);
+       }
+
+       return result;
+}
+
+/**
+ *  index_of_key()- Inverse lookup of the index of an MPL product key .
+ *  @key: the MPL product indentifier also referred to as 'key'.
+ */
+static short index_of_key(u16 key)
+{
+       int i;
+       for (i = 0; i < NUM_OF_PROD_REVS; i++)
+               if (prod_rev_map[i].mpl_product_key == key)
+                       return (short)i;
+       return -EINVAL;
+}
+
+int inv_get_silicon_rev_mpu6500(struct nvi_state *st)
+{
+       struct inv_chip_info_s *chip_info = &st->chip_info;
+       int result;
+       u8 whoami, sw_rev;
+
+       result = nvi_i2c_rd(st, 0, REG_WHO_AM_I, 1, &whoami);
+       if (result)
+               return result;
+       if (whoami != MPU6500_ID && whoami != MPU9250_ID &&
+                       whoami != MPU9350_ID && whoami != MPU6515_ID)
+               return -EINVAL;
+
+       /*memory read need more time after power up */
+       msleep(POWER_UP_TIME);
+       result = mpu_memory_read(st, st->i2c_addr,
+                       MPU6500_MEM_REV_ADDR, 1, &sw_rev);
+       sw_rev &= INV_MPU_REV_MASK;
+       if (result)
+               return result;
+       if (sw_rev != 0)
+               return -EINVAL;
+       /* these values are place holders and not real values */
+       chip_info->product_id = MPU6500_PRODUCT_REVISION;
+       chip_info->product_revision = MPU6500_PRODUCT_REVISION;
+       chip_info->silicon_revision = MPU6500_PRODUCT_REVISION;
+       chip_info->software_revision = sw_rev;
+       chip_info->gyro_sens_trim = DEFAULT_GYRO_TRIM;
+       chip_info->accel_sens_trim = DEFAULT_ACCEL_TRIM;
+       chip_info->multi = 1;
+
+       return 0;
+}
+
+int inv_get_silicon_rev_mpu6050(struct nvi_state *st)
+{
+       int result;
+       u8 prod_ver = 0x00, prod_rev = 0x00;
+       struct prod_rev_map_t *p_rev;
+       u8 bank =
+           (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
+       u16 mem_addr = ((bank << 8) | MEM_ADDR_PROD_REV);
+       u16 key;
+       u8 regs[5];
+       u16 sw_rev;
+       short index;
+       struct inv_chip_info_s *chip_info = &st->chip_info;
+
+       result = nvi_i2c_rd(st, 0, REG_PRODUCT_ID, 1, &prod_ver);
+       if (result)
+               return result;
+       prod_ver &= 0xf;
+       /*memory read need more time after power up */
+       msleep(POWER_UP_TIME);
+       result = mpu_memory_read(st, st->i2c_addr, mem_addr, 1, &prod_rev);
+       if (result)
+               return result;
+       prod_rev >>= 2;
+       /* clean the prefetch and cfg user bank bits */
+       result = nvi_i2c_wr(st, REG_BANK_SEL, 0);
+       if (result)
+               return result;
+       /* get the software-product version, read from XA_OFFS_L */
+       result = nvi_i2c_rd(st, 0 , REG_XA_OFFS_L_TC,
+                               SOFT_PROD_VER_BYTES, regs);
+       if (result)
+               return result;
+
+       sw_rev = (regs[4] & 0x01) << 2 |        /* 0x0b, bit 0 */
+                (regs[2] & 0x01) << 1 |        /* 0x09, bit 0 */
+                (regs[0] & 0x01);              /* 0x07, bit 0 */
+       /* if 0, use the product key to determine the type of part */
+       if (sw_rev == 0) {
+               key = MPL_PROD_KEY(prod_ver, prod_rev);
+               if (key == 0)
+                       return -EINVAL;
+               index = index_of_key(key);
+               if (index < 0 || index >= NUM_OF_PROD_REVS)
+                       return -EINVAL;
+               /* check MPL is compiled for this device */
+               if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1)
+                       return -EINVAL;
+               p_rev = (struct prod_rev_map_t *)&prod_rev_map[index];
+       /* if valid, use the software product key */
+       } else if (sw_rev < ARRAY_SIZE(sw_rev_map)) {
+               p_rev = (struct prod_rev_map_t *)&sw_rev_map[sw_rev];
+       } else {
+               return -EINVAL;
+       }
+       chip_info->product_id = prod_ver;
+       chip_info->product_revision = prod_rev;
+       chip_info->silicon_revision = p_rev->silicon_rev;
+       chip_info->software_revision = sw_rev;
+       chip_info->gyro_sens_trim = p_rev->gyro_trim;
+       chip_info->accel_sens_trim = p_rev->accel_trim;
+       if (chip_info->accel_sens_trim == 0)
+               chip_info->accel_sens_trim = DEFAULT_ACCEL_TRIM;
+       chip_info->multi = DEFAULT_ACCEL_TRIM / chip_info->accel_sens_trim;
+       if (chip_info->multi != 1)
+               pr_info("multi is %d\n", chip_info->multi);
+       return result;
+}
+
+/**
+ *  read_accel_hw_self_test_prod_shift()- read the accelerometer hardware
+ *                                         self-test bias shift calculated
+ *                                         during final production test and
+ *                                         stored in chip non-volatile memory.
+ *  @st:  main data structure.
+ *  @st_prod:   A pointer to an array of 3 elements to hold the values
+ *              for production hardware self-test bias shifts returned to the
+ *              user.
+ *  @accel_sens: accel sensitivity.
+ */
+static int read_accel_hw_self_test_prod_shift(struct nvi_state *st,
+                                       int *st_prod, int *accel_sens)
+{
+       u8 regs[4];
+       u8 shift_code[3];
+       int result, i;
+
+       for (i = 0; i < 3; i++)
+               st_prod[i] = 0;
+
+       result = nvi_i2c_rd(st, 0, REG_ST_GCT_X, ARRAY_SIZE(regs), regs);
+       if (result)
+               return result;
+       if ((0 == regs[0])  && (0 == regs[1]) &&
+           (0 == regs[2]) && (0 == regs[3]))
+               return -EINVAL;
+       shift_code[X] = ((regs[0] & 0xE0) >> 3) | ((regs[3] & 0x30) >> 4);
+       shift_code[Y] = ((regs[1] & 0xE0) >> 3) | ((regs[3] & 0x0C) >> 2);
+       shift_code[Z] = ((regs[2] & 0xE0) >> 3) |  (regs[3] & 0x03);
+       for (i = 0; i < 3; i++)
+               if (shift_code[i] != 0)
+                       st_prod[i] = accel_sens[i] *
+                                       accel_st_tb[shift_code[i] - 1];
+
+       return 0;
+}
+
+/**
+* inv_check_accel_self_test()- check accel self test. this function returns
+*                              zero as success. A non-zero return value
+*                              indicates failure in self test.
+*  @*st: main data structure.
+*  @*reg_avg: average value of normal test.
+*  @*st_avg:  average value of self test
+*/
+static int inv_check_accel_self_test(struct nvi_state *st,
+                                               int *reg_avg, int *st_avg){
+       int gravity, j, ret_val;
+       int tmp;
+       int st_shift_prod[THREE_AXIS], st_shift_cust[THREE_AXIS];
+       int st_shift_ratio[THREE_AXIS];
+       int accel_sens[THREE_AXIS];
+
+       if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
+           st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
+               return 0;
+       ret_val = 0;
+       tmp = DEF_ST_SCALE * DEF_ST_PRECISION / DEF_ST_ACCEL_FS_MG;
+       for (j = 0; j < 3; j++)
+               accel_sens[j] = tmp;
+
+       if (MPL_PROD_KEY(st->chip_info.product_id,
+                        st->chip_info.product_revision) ==
+           MPU_PRODUCT_KEY_B1_E1_5) {
+               /* half sensitivity Z accelerometer parts */
+               accel_sens[Z] /= 2;
+       } else {
+               /* half sensitivity X, Y, Z accelerometer parts */
+               accel_sens[X] /= st->chip_info.multi;
+               accel_sens[Y] /= st->chip_info.multi;
+               accel_sens[Z] /= st->chip_info.multi;
+       }
+       gravity = accel_sens[Z];
+       ret_val = read_accel_hw_self_test_prod_shift(st, st_shift_prod,
+                                                       accel_sens);
+       if (ret_val)
+               return ret_val;
+
+       for (j = 0; j < 3; j++) {
+               st_shift_cust[j] = abs(reg_avg[j] - st_avg[j]);
+               if (st_shift_prod[j]) {
+                       tmp = st_shift_prod[j] / DEF_ST_PRECISION;
+                       st_shift_ratio[j] = abs(st_shift_cust[j] / tmp
+                               - DEF_ST_PRECISION);
+                       if (st_shift_ratio[j] > DEF_ACCEL_ST_SHIFT_DELTA)
+                               ret_val = 1;
+               } else {
+                       if (st_shift_cust[j] <
+                               DEF_ACCEL_ST_SHIFT_MIN * gravity)
+                               ret_val = 1;
+                       if (st_shift_cust[j] >
+                               DEF_ACCEL_ST_SHIFT_MAX * gravity)
+                               ret_val = 1;
+               }
+       }
+
+       return ret_val;
+}
+
+/**
+* inv_check_6050_gyro_self_test() - check 6050 gyro self test. this function
+*                                   returns zero as success. A non-zero return
+*                                   value indicates failure in self test.
+*  @*st: main data structure.
+*  @*reg_avg: average value of normal test.
+*  @*st_avg:  average value of self test
+*/
+static int inv_check_6050_gyro_self_test(struct nvi_state *st,
+                                               int *reg_avg, int *st_avg){
+       int result;
+       int ret_val;
+       int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
+       u8 regs[3];
+
+       if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
+           st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
+               return 0;
+
+       ret_val = 0;
+       result = nvi_i2c_rd(st, 0, REG_ST_GCT_X, 3, regs);
+       if (result)
+               return result;
+       regs[X] &= 0x1f;
+       regs[Y] &= 0x1f;
+       regs[Z] &= 0x1f;
+       for (i = 0; i < 3; i++) {
+               if (regs[i] != 0)
+                       st_shift_prod[i] = gyro_6050_st_tb[regs[i] - 1];
+               else
+                       st_shift_prod[i] = 0;
+       }
+       st_shift_prod[1] = -st_shift_prod[1];
+
+       for (i = 0; i < 3; i++) {
+               st_shift_cust[i] =  st_avg[i] - reg_avg[i];
+               if (st_shift_prod[i]) {
+                       st_shift_ratio[i] = abs(st_shift_cust[i] /
+                               st_shift_prod[i] - DEF_ST_PRECISION);
+                       if (st_shift_ratio[i] > DEF_GYRO_CT_SHIFT_DELTA)
+                               ret_val = 1;
+               } else {
+                       if (st_shift_cust[i] < DEF_ST_PRECISION *
+                               DEF_GYRO_CT_SHIFT_MIN * DEF_SELFTEST_GYRO_SENS)
+                               ret_val = 1;
+                       if (st_shift_cust[i] > DEF_ST_PRECISION *
+                               DEF_GYRO_CT_SHIFT_MAX * DEF_SELFTEST_GYRO_SENS)
+                               ret_val = 1;
+               }
+       }
+       /* check for absolute value passing criterion. Using DEF_ST_TOR
+        * for certain degree of tolerance */
+       for (i = 0; i < 3; i++)
+               if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH *
+                   DEF_ST_PRECISION * DEF_GYRO_SCALE)
+                       ret_val = 1;
+
+       return ret_val;
+}
+
+/**
+* inv_check_6500_gyro_self_test() - check 6500 gyro self test. this function
+*                                   returns zero as success. A non-zero return
+*                                   value indicates failure in self test.
+*  @*st: main data structure.
+*  @*reg_avg: average value of normal test.
+*  @*st_avg:  average value of self test
+*/
+static int inv_check_6500_gyro_self_test(struct nvi_state *st,
+                                               int *reg_avg, int *st_avg) {
+       u8 regs[3];
+       int ret_val, result;
+       int otp_value_zero = 0;
+       int st_shift_prod[3], st_shift_cust[3], i;
+
+       ret_val = 0;
+       result = nvi_i2c_rd(st, st->hal->reg->self_test_x_gyro.bank,
+                           st->hal->reg->self_test_x_gyro.reg, 3, regs);
+       if (result)
+               return result;
+       pr_debug("%s self_test gyro shift_code - %02x %02x %02x\n",
+                st->hal->part_name, regs[0], regs[1], regs[2]);
+
+       for (i = 0; i < 3; i++) {
+               if (regs[i] != 0) {
+                       st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
+               } else {
+                       st_shift_prod[i] = 0;
+                       otp_value_zero = 1;
+               }
+       }
+       pr_debug("%s self_test gyro st_shift_prod - %+d %+d %+d\n",
+                st->hal->part_name, st_shift_prod[0], st_shift_prod[1],
+                st_shift_prod[2]);
+
+       for (i = 0; i < 3; i++) {
+               st_shift_cust[i] = st_avg[i] - reg_avg[i];
+               if (!otp_value_zero) {
+                       /* Self Test Pass/Fail Criteria A */
+                       if (st_shift_cust[i] < DEF_6500_GYRO_CT_SHIFT_DELTA
+                                               * st_shift_prod[i])
+                                       ret_val = 1;
+               } else {
+                       /* Self Test Pass/Fail Criteria B */
+                       if (st_shift_cust[i] < DEF_GYRO_ST_AL *
+                                               DEF_SELFTEST_GYRO_SENS *
+                                               DEF_ST_PRECISION)
+                               ret_val = 1;
+               }
+       }
+       pr_debug("%s self_test gyro st_shift_cust - %+d %+d %+d\n",
+                st->hal->part_name, st_shift_cust[0], st_shift_cust[1],
+                st_shift_cust[2]);
+
+       if (ret_val == 0) {
+               /* Self Test Pass/Fail Criteria C */
+               for (i = 0; i < 3; i++)
+                       if (abs(reg_avg[i]) > DEF_GYRO_OFFSET_MAX *
+                                               DEF_SELFTEST_GYRO_SENS *
+                                               DEF_ST_PRECISION)
+                               ret_val = 1;
+       }
+
+       return ret_val;
+}
+
+/**
+* inv_check_6500_accel_self_test() - check 6500 accel self test. this function
+*                                   returns zero as success. A non-zero return
+*                                   value indicates failure in self test.
+*  @*st: main data structure.
+*  @*reg_avg: average value of normal test.
+*  @*st_avg:  average value of self test
+*/
+static int inv_check_6500_accel_self_test(struct nvi_state *st,
+                                               int *reg_avg, int *st_avg) {
+       int ret_val, result;
+       int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
+       u8 regs[3];
+       int otp_value_zero = 0;
+
+#define ACCEL_ST_AL_MIN ((DEF_ACCEL_ST_AL_MIN * DEF_ST_SCALE \
+                                / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
+#define ACCEL_ST_AL_MAX ((DEF_ACCEL_ST_AL_MAX * DEF_ST_SCALE \
+                                / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
+
+       ret_val = 0;
+       result = nvi_i2c_rd(st, st->hal->reg->self_test_x_accel.bank,
+                           st->hal->reg->self_test_x_accel.reg, 3, regs);
+       if (result)
+               return result;
+       pr_debug("%s self_test accel shift_code - %02x %02x %02x\n",
+                st->hal->part_name, regs[0], regs[1], regs[2]);
+
+       for (i = 0; i < 3; i++) {
+               if (regs[i] != 0) {
+                       st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
+               } else {
+                       st_shift_prod[i] = 0;
+                       otp_value_zero = 1;
+               }
+       }
+       pr_debug("%s self_test accel st_shift_prod - %+d %+d %+d\n",
+                st->hal->part_name, st_shift_prod[0], st_shift_prod[1],
+                st_shift_prod[2]);
+
+       if (!otp_value_zero) {
+               /* Self Test Pass/Fail Criteria A */
+               for (i = 0; i < 3; i++) {
+                       st_shift_cust[i] = st_avg[i] - reg_avg[i];
+                       st_shift_ratio[i] = abs(st_shift_cust[i] /
+                                       st_shift_prod[i] - DEF_ST_PRECISION);
+                       if (st_shift_ratio[i] > DEF_6500_ACCEL_ST_SHIFT_DELTA)
+                               ret_val = 1;
+               }
+       } else {
+               /* Self Test Pass/Fail Criteria B */
+               for (i = 0; i < 3; i++) {
+                       st_shift_cust[i] = abs(st_avg[i] - reg_avg[i]);
+                       if (st_shift_cust[i] < ACCEL_ST_AL_MIN ||
+                                       st_shift_cust[i] > ACCEL_ST_AL_MAX)
+                               ret_val = 1;
+               }
+       }
+       pr_debug("%s self_test accel st_shift_cust - %+d %+d %+d\n",
+                st->hal->part_name, st_shift_cust[0], st_shift_cust[1],
+                st_shift_cust[2]);
+
+       return ret_val;
+}
+
+/*
+ *  inv_do_test() - do the actual test of self testing
+ */
+static int inv_do_test(struct nvi_state *st, int self_test_flag,
+               int *gyro_result, int *accel_result)
+{
+       int result, i, j, packet_size;
+       u8 data[BYTES_PER_SENSOR * 2], d;
+       bool has_accel;
+       int fifo_count, packet_count, ind, s;
+
+       has_accel = true;
+       if (has_accel)
+               packet_size = BYTES_PER_SENSOR * 2;
+       else
+               packet_size = BYTES_PER_SENSOR;
+
+       result = nvi_wr_int_enable(st, 0);
+       if (result)
+               return result;
+       /* disable the sensor output to FIFO */
+       /* disable fifo reading */
+       result = nvi_user_ctrl_en(st, false, false);
+       if (result)
+               return result;
+       /* clear FIFO */
+       result = nvi_wr_user_ctrl(st, BIT_FIFO_RST);
+       if (result)
+               return result;
+       /* setup parameters */
+       result = nvi_wr_config(st, INV_FILTER_98HZ);
+       if (result < 0)
+               return result;
+
+       if (st->hal->part >= MPU6500) {
+               /* config accel LPF register for MPU6500 */
+               result = nvi_wr_accel_config2(st, DEF_ST_MPU6500_ACCEL_LPF |
+                                               BIT_FIFO_SIZE_1K);
+               if (result < 0)
+                       return result;
+       }
+
+       result = nvi_wr_smplrt_div(st, DEF_SELFTEST_SAMPLE_RATE);
+       if (result)
+               return result;
+       /* wait for the sampling rate change to stabilize */
+       mdelay(INV_MPU_SAMPLE_RATE_CHANGE_STABLE);
+       result = nvi_wr_gyro_config(st, (self_test_flag >> 5),
+                                   DEF_SELFTEST_GYRO_FS);
+       if (result < 0)
+               return result;
+       if (has_accel) {
+               if (st->hal->part >= MPU6500)
+                       d = DEF_SELFTEST_6500_ACCEL_FS;
+               else
+                       d = DEF_SELFTEST_ACCEL_FS;
+               result = nvi_wr_accel_config(st, (self_test_flag >> 5),
+                                            (d >> 3), 0);
+               if (result < 0)
+                       return result;
+       }
+       /* wait for the output to get stable */
+       if (self_test_flag) {
+               if (st->hal->part >= MPU6500)
+                       msleep(DEF_ST_6500_STABLE_TIME);
+               else
+                       msleep(DEF_ST_STABLE_TIME);
+       }
+
+       /* enable FIFO reading */
+       result = nvi_wr_user_ctrl(st, BIT_FIFO_EN);
+       if (result)
+               return result;
+       /* enable sensor output to FIFO */
+       if (has_accel)
+               d = BITS_GYRO_OUT | BIT_ACCEL_OUT;
+       else
+               d = BITS_GYRO_OUT;
+       for (i = 0; i < THREE_AXIS; i++) {
+               gyro_result[i] = 0;
+               accel_result[i] = 0;
+       }
+       s = 0;
+       while (s < st->self_test.samples) {
+               /* enable sensor output to FIFO */
+               result = nvi_wr_fifo_en(st, d);
+               if (result)
+                       return result;
+               mdelay(DEF_GYRO_WAIT_TIME);
+               result = nvi_wr_fifo_en(st, 0);
+               if (result)
+                       return result;
+
+               result = nvi_i2c_rd(st, st->hal->reg->fifo_count_h.bank,
+                                   st->hal->reg->fifo_count_h.reg,
+                                   FIFO_COUNT_BYTE, data);
+               if (result)
+                       return result;
+               fifo_count = be16_to_cpup((__be16 *)(&data[0]));
+               pr_debug("%s self_test fifo_count - %d\n",
+                        st->hal->part_name, fifo_count);
+               packet_count = fifo_count / packet_size;
+               i = 0;
+               while ((i < packet_count) && (s < st->self_test.samples)) {
+                       short vals[3];
+                       result = nvi_i2c_rd(st, st->hal->reg->fifo_r_w.bank,
+                                           st->hal->reg->fifo_r_w.reg,
+                                           packet_size, data);
+                       if (result)
+                               return result;
+                       ind = 0;
+                       if (has_accel) {
+                               for (j = 0; j < THREE_AXIS; j++) {
+                                       vals[j] = (short)be16_to_cpup(
+                                           (__be16 *)(&data[ind + 2 * j]));
+                                       accel_result[j] += vals[j];
+                               }
+                               ind += BYTES_PER_SENSOR;
+                               pr_debug(
+                                   "%s self_test accel data - %d %+d %+d %+d",
+                                        st->hal->part_name,
+                                        s, vals[0], vals[1], vals[2]);
+                       }
+
+                       for (j = 0; j < THREE_AXIS; j++) {
+                               vals[j] = (short)be16_to_cpup(
+                                       (__be16 *)(&data[ind + 2 * j]));
+                               gyro_result[j] += vals[j];
+                       }
+                       pr_debug("%s self_test gyro data - %d %+d %+d %+d",
+                                st->hal->part_name,
+                                s, vals[0], vals[1], vals[2]);
+
+                       s++;
+                       i++;
+               }
+       }
+
+       if (has_accel) {
+               for (j = 0; j < THREE_AXIS; j++) {
+                       accel_result[j] = accel_result[j] / s;
+                       accel_result[j] *= DEF_ST_PRECISION;
+               }
+       }
+       for (j = 0; j < THREE_AXIS; j++) {
+               gyro_result[j] = gyro_result[j] / s;
+               gyro_result[j] *= DEF_ST_PRECISION;
+       }
+
+       return 0;
+}
+
+static int inv_reset_offset_reg(struct nvi_state *st, bool en)
+{
+       int i, result;
+       s16 gyro[3], accel[3];
+
+       if (en) {
+               for (i = 0; i < 3; i++) {
+                       gyro[i] = st->rom_gyro_offset[i];
+                       accel[i] = st->rom_accel_offset[i];
+               }
+       } else {
+               for (i = 0; i < 3; i++) {
+                       gyro[i] = st->rom_gyro_offset[i] +
+                                               st->input_gyro_offset[i];
+                       accel[i] = st->rom_accel_offset[i] +
+                                       (st->input_accel_offset[i] << 1);
+               }
+       }
+       for (i = 0; i < 3; i++) {
+               result = nvi_wr_gyro_offset(st, i, (u16)gyro[i]);
+               if (result)
+                       return result;
+               result = nvi_wr_accel_offset(st, i, (u16)accel[i]);
+               if (result)
+                       return result;
+       }
+
+       return 0;
+}
+
+/**
+ *  inv_recover_setting() recover the old settings after everything is done
+ */
+static void inv_recover_setting(struct iio_dev *indio_dev)
+{
+       struct nvi_state *st = iio_priv(indio_dev);
+
+       inv_reset_offset_reg(st, false);
+       nvi_enable(indio_dev);
+}
+
+/*
+ *  inv_hw_self_test() - main function to do hardware self test
+ */
+int inv_hw_self_test(struct iio_dev *indio_dev)
+{
+       struct nvi_state *st = iio_priv(indio_dev);
+       int result;
+       int gyro_bias_st[THREE_AXIS], gyro_bias_regular[THREE_AXIS];
+       int accel_bias_st[THREE_AXIS], accel_bias_regular[THREE_AXIS];
+       int test_times, i;
+       char compass_result, accel_result, gyro_result;
+
+       result = nvi_pm_wr(st, INV_CLK_PLL, 0, 0);
+       if (result)
+               return result;
+       result = inv_reset_offset_reg(st, true);
+       if (result)
+               return result;
+       compass_result = 0;
+       accel_result = 0;
+       gyro_result = 0;
+       test_times = DEF_ST_TRY_TIMES;
+       while (test_times > 0) {
+               result = inv_do_test(st, 0, gyro_bias_regular,
+                       accel_bias_regular);
+               if (result == -EAGAIN)
+                       test_times--;
+               else
+                       test_times = 0;
+       }
+       if (result)
+               goto test_fail;
+       pr_debug("%s self_test accel bias_regular - %+d %+d %+d\n",
+                st->hal->part_name, accel_bias_regular[0],
+                accel_bias_regular[1], accel_bias_regular[2]);
+       pr_debug("%s self_test gyro bias_regular - %+d %+d %+d\n",
+                st->hal->part_name, gyro_bias_regular[0],
+                gyro_bias_regular[1], gyro_bias_regular[2]);
+
+       for (i = 0; i < 3; i++) {
+               st->gyro_bias[i] = gyro_bias_regular[i];
+               st->accel_bias[i] = accel_bias_regular[i];
+       }
+
+       test_times = DEF_ST_TRY_TIMES;
+       while (test_times > 0) {
+               result = inv_do_test(st, BITS_SELF_TEST_EN, gyro_bias_st,
+                                       accel_bias_st);
+               if (result == -EAGAIN)
+                       test_times--;
+               else
+                       break;
+       }
+       if (result)
+               goto test_fail;
+       pr_debug("%s self_test accel bias_st - %+d %+d %+d\n",
+                st->hal->part_name, accel_bias_st[0], accel_bias_st[1],
+                accel_bias_st[2]);
+       pr_debug("%s self_test gyro bias_st - %+d %+d %+d\n",
+                st->hal->part_name, gyro_bias_st[0], gyro_bias_st[1],
+                gyro_bias_st[2]);
+
+        if (MPU6050 == st->hal->part) {
+               accel_result = !inv_check_accel_self_test(st,
+                       accel_bias_regular, accel_bias_st);
+               gyro_result = !inv_check_6050_gyro_self_test(st,
+                       gyro_bias_regular, gyro_bias_st);
+       } else if (st->hal->part >= MPU6500) {
+               accel_result = !inv_check_6500_accel_self_test(st,
+                       accel_bias_regular, accel_bias_st);
+               gyro_result = !inv_check_6500_gyro_self_test(st,
+                       gyro_bias_regular, gyro_bias_st);
+       }
+
+test_fail:
+       inv_recover_setting(indio_dev);
+
+       return (compass_result << DEF_ST_COMPASS_RESULT_SHIFT) |
+               (accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result;
+}
+
+static int inv_load_firmware(struct nvi_state *st,
+       u8 *data, int size)
+{
+       int bank, write_size;
+       int result;
+       u16 memaddr;
+
+       /* first bank start at MPU_DMP_LOAD_START */
+       write_size = MPU_MEM_BANK_SIZE - MPU_DMP_LOAD_START;
+       memaddr = MPU_DMP_LOAD_START;
+       result = mem_w(memaddr, write_size, data);
+       if (result)
+               return result;
+       size -= write_size;
+       data += write_size;
+
+       /* Write and verify memory */
+       for (bank = 1; size > 0; bank++, size -= write_size,
+                               data += write_size) {
+               if (size > MPU_MEM_BANK_SIZE)
+                       write_size = MPU_MEM_BANK_SIZE;
+               else
+                       write_size = size;
+
+               memaddr = ((bank << 8) | 0x00);
+
+               result = mem_w(memaddr, write_size, data);
+               if (result)
+                       return result;
+       }
+       return 0;
+}
+
+static int inv_verify_firmware(struct nvi_state *st,
+       u8 *data, int size)
+{
+       int bank, write_size;
+       int result;
+       u16 memaddr;
+       u8 firmware[MPU_MEM_BANK_SIZE];
+
+       /* Write and verify memory */
+       write_size = MPU_MEM_BANK_SIZE - MPU_DMP_LOAD_START;
+       size -= write_size;
+       data += write_size;
+       for (bank = 1; size > 0; bank++,
+               size -= write_size,
+               data += write_size) {
+               if (size > MPU_MEM_BANK_SIZE)
+                       write_size = MPU_MEM_BANK_SIZE;
+               else
+                       write_size = size;
+
+               memaddr = ((bank << 8) | 0x00);
+               result = mpu_memory_read(st,
+                       st->i2c_addr, memaddr, write_size, firmware);
+               if (result)
+                       return result;
+               if (0 != memcmp(firmware, data, write_size))
+                       return -EINVAL;
+       }
+       return 0;
+}
+
+int inv_enable_pedometer_interrupt(struct nvi_state *st, bool en)
+{
+       u8 reg[3];
+
+       if (en) {
+               reg[0] = 0xf4;
+               reg[1] = 0x44;
+               reg[2] = 0xf1;
+
+       } else {
+               reg[0] = 0xf1;
+               reg[1] = 0xf1;
+               reg[2] = 0xf1;
+       }
+
+       return mem_w_key(KEY_CFG_PED_INT, ARRAY_SIZE(reg), reg);
+}
+
+int inv_read_pedometer_counter(struct nvi_state *st)
+{
+       int result;
+       u8 d[4];
+       u32 last_step_counter, curr_counter;
+
+       result = mpu_memory_read(st, st->i2c_addr,
+                       inv_dmp_get_address(KEY_D_STPDET_TIMESTAMP), 4, d);
+       if (result)
+               return result;
+       last_step_counter = (u32)be32_to_cpup((__be32 *)(d));
+
+       result = mpu_memory_read(st, st->i2c_addr,
+                       inv_dmp_get_address(KEY_DMP_RUN_CNTR), 4, d);
+       if (result)
+               return result;
+       curr_counter = (u32)be32_to_cpup((__be32 *)(d));
+       if (0 != last_step_counter)
+               /* get_time_ns() - */
+               st->ped.last_step_time = iio_get_time_ns() -
+                       ((u64)(curr_counter - last_step_counter)) *
+                       DMP_INTERVAL_INIT;
+
+       return 0;
+}
+
+int inv_enable_pedometer(struct nvi_state *st, bool en)
+{
+       u8 d[1];
+
+       if (en)
+               d[0] = 0xf1;
+       else
+               d[0] = 0xff;
+
+       return mem_w_key(KEY_CFG_PED_ENABLE, ARRAY_SIZE(d), d);
+}
+
+int inv_get_pedometer_steps(struct nvi_state *st, u32 *steps)
+{
+       u8 d[4];
+       int result;
+
+       result = mpu_memory_read(st, st->i2c_addr,
+                       inv_dmp_get_address(KEY_D_PEDSTD_STEPCTR), 4, d);
+       *steps = (u32)be32_to_cpup((__be32 *)(d));
+
+       return result;
+}
+
+int inv_get_pedometer_time(struct nvi_state *st, u32 *time)
+{
+       u8 d[4];
+       int result;
+
+       result = mpu_memory_read(st, st->i2c_addr,
+                       inv_dmp_get_address(KEY_D_PEDSTD_TIMECTR), 4, d);
+       *time = (u32)be32_to_cpup((__be32 *)(d));
+
+       return result;
+}
+
+int inv_set_display_orient_interrupt_dmp(struct nvi_state *st, bool on)
+{
+       int r;
+       u8  rn[] = {0xf4, 0x41};
+       u8  rf[] = {0xd8, 0xd8};
+
+       if (on)
+               r = mem_w_key(KEY_CFG_DISPLAY_ORIENT_INT, ARRAY_SIZE(rn), rn);
+       else
+               r = mem_w_key(KEY_CFG_DISPLAY_ORIENT_INT, ARRAY_SIZE(rf), rf);
+
+       return r;
+}
+
+int inv_write_2bytes(struct nvi_state *st, int k, int data)
+{
+       u8 d[2];
+
+       if (data < 0 || data > USHRT_MAX)
+               return -EINVAL;
+
+       d[0] = (u8)((data >> 8) & 0xff);
+       d[1] = (u8)(data & 0xff);
+
+       return mem_w_key(k, ARRAY_SIZE(d), d);
+}
+
+static int inv_set_tap_interrupt_dmp(struct nvi_state *st, u8 on)
+{
+       int result;
+       u16 d;
+
+       if (on)
+               d = 192;
+       else
+               d = 128;
+
+       result = inv_write_2bytes(st, KEY_DMP_TAP_GATE, d);
+
+       return result;
+}
+
+/*
+ * inv_set_tap_threshold_dmp():
+ * Sets the tap threshold in the dmp
+ * Simultaneously sets secondary tap threshold to help correct the tap
+ * direction for soft taps.
+ */
+int inv_set_tap_threshold_dmp(struct nvi_state *st, u16 threshold)
+{
+       int result;
+       int sample_divider;
+       int scaled_threshold;
+       u32 dmp_threshold;
+       u8 sample_div;
+       const u32  accel_sens = (0x20000000 / 0x00010000);
+
+       if (threshold > (1 << 15))
+               return -EINVAL;
+       sample_div = INIT_SAMPLE_DIVIDER;
+
+       sample_divider = (1 + sample_div);
+       /* Scale factor corresponds linearly using
+       * 0  : 0
+       * 25 : 0.0250  g/ms
+       * 50 : 0.0500  g/ms
+       * 100: 1.0000  g/ms
+       * 200: 2.0000  g/ms
+       * 400: 4.0000  g/ms
+       * 800: 8.0000  g/ms
+       */
+       /*multiply by 1000 to avoid floating point 1000/1000*/
+       scaled_threshold = threshold;
+       /* Convert to per sample */
+       scaled_threshold *= sample_divider;
+
+       /* Scale to DMP 16 bit value */
+       if (accel_sens != 0)
+               dmp_threshold = (u32)(scaled_threshold * accel_sens);
+       else
+               return -EINVAL;
+       dmp_threshold = dmp_threshold / DMP_PRECISION;
+       result = inv_write_2bytes(st, KEY_DMP_TAP_THR_Z, dmp_threshold);
+       if (result)
+               return result;
+       result = inv_write_2bytes(st, KEY_DMP_TAP_PREV_JERK_Z,
+                                               dmp_threshold * 3 / 4);
+
+       return result;
+}
+
+
+/*
+ * inv_set_min_taps_dmp():
+ * Indicates the minimum number of consecutive taps required
+ * before the DMP will generate an interrupt.
+ */
+int inv_set_min_taps_dmp(struct nvi_state *st, u16 min_taps)
+{
+       u8 result;
+
+       /* check if any spurious bit other the ones expected are set */
+       if ((min_taps > DMP_MAX_MIN_TAPS) || (min_taps < 1))
+               return -EINVAL;
+
+       /* DMP tap count is zero-based. So single-tap is 0.
+          Furthermore, DMP code checks for tap_count > min_taps.
+          So we have to do minus 2 here.
+          For example, if the user expects any single tap will generate an
+          interrupt, (s)he will call inv_set_min_taps_dmp(1).
+          When DMP gets a single tap, tap_count = 0. To get
+          tap_count > min_taps, we have to decrement min_taps by 2 to -1. */
+       result = inv_write_2bytes(st, KEY_DMP_TAP_MIN_TAPS, (u16)(min_taps-2));
+
+       return result;
+}
+
+/*
+ * inv_set_tap_time_dmp():
+ * Determines how long after a tap the DMP requires before
+ * another tap can be registered.
+ */
+int  inv_set_tap_time_dmp(struct nvi_state *st, u16 time)
+{
+       int result;
+       u16 dmp_time;
+       u8 sample_divider;
+
+       sample_divider = INIT_SAMPLE_DIVIDER;
+       sample_divider++;
+
+       /* 60 ms minimum time added */
+       dmp_time = ((time) / sample_divider);
+       result = inv_write_2bytes(st, KEY_DMP_TAPW_MIN, dmp_time);
+
+       return result;
+}
+
+/*
+ * inv_set_multiple_tap_time_dmp():
+ * Determines how close together consecutive taps must occur
+ * to be considered double/triple taps.
+ */
+static int inv_set_multiple_tap_time_dmp(struct nvi_state *st, u32 time)
+{
+       int result;
+       u16 dmp_time;
+       u8 sample_divider;
+
+       sample_divider = INIT_SAMPLE_DIVIDER;
+       sample_divider++;
+
+       /* 60 ms minimum time added */
+       dmp_time = ((time) / sample_divider);
+       result = inv_write_2bytes(st, KEY_DMP_TAP_NEXT_TAP_THRES, dmp_time);
+
+       return result;
+}
+
+int inv_q30_mult(int a, int b)
+{
+       u64 temp;
+       int result;
+
+       temp = (u64)a * b;
+       result = (int)(temp >> DMP_MULTI_SHIFT);
+
+       return result;
+}
+
+static u16 inv_row_2_scale(const s8 *row)
+{
+       u16 b;
+
+       if (row[0] > 0)
+               b = 0;
+       else if (row[0] < 0)
+               b = 4;
+       else if (row[1] > 0)
+               b = 1;
+       else if (row[1] < 0)
+               b = 5;
+       else if (row[2] > 0)
+               b = 2;
+       else if (row[2] < 0)
+               b = 6;
+       else
+               b = 7;
+
+       return b;
+}
+
+/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar
+*      representation.
+* @param[in] mtx Orientation matrix to convert to a scalar.
+* @return Description of orientation matrix. The lowest 2 bits (0 and 1)
+* represent the column the one is on for the
+* first row, with the bit number 2 being the sign. The next 2 bits
+* (3 and 4) represent
+* the column the one is on for the second row with bit number 5 being
+* the sign.
+* The next 2 bits (6 and 7) represent the column the one is on for the
+* third row with
+* bit number 8 being the sign. In binary the identity matrix would therefor
+* be: 010_001_000 or 0x88 in hex.
+*/
+static u16 inv_orientation_matrix_to_scaler(const signed char *mtx)
+{
+
+       u16 scalar;
+       scalar = inv_row_2_scale(mtx);
+       scalar |= inv_row_2_scale(mtx + 3) << 3;
+       scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+       return scalar;
+}
+
+static int inv_gyro_dmp_cal(struct nvi_state *st)
+{
+       int inv_gyro_orient;
+       u8 regs[3];
+       int result;
+
+       u8 tmp_d = DINA4C;
+       u8 tmp_e = DINACD;
+       u8 tmp_f = DINA6C;
+
+       inv_gyro_orient =
+               inv_orientation_matrix_to_scaler(st->pdata.orientation);
+
+       if ((inv_gyro_orient & 3) == 0)
+               regs[0] = tmp_d;
+       else if ((inv_gyro_orient & 3) == 1)
+               regs[0] = tmp_e;
+       else if ((inv_gyro_orient & 3) == 2)
+               regs[0] = tmp_f;
+       if ((inv_gyro_orient & 0x18) == 0)
+               regs[1] = tmp_d;
+       else if ((inv_gyro_orient & 0x18) == 0x8)
+               regs[1] = tmp_e;
+       else if ((inv_gyro_orient & 0x18) == 0x10)
+               regs[1] = tmp_f;
+       if ((inv_gyro_orient & 0xc0) == 0)
+               regs[2] = tmp_d;
+       else if ((inv_gyro_orient & 0xc0) == 0x40)
+               regs[2] = tmp_e;
+       else if ((inv_gyro_orient & 0xc0) == 0x80)
+               regs[2] = tmp_f;
+
+       result = mem_w_key(KEY_FCFG_1, ARRAY_SIZE(regs), regs);
+       if (result)
+               return result;
+
+       if (inv_gyro_orient & 4)
+               regs[0] = DINA36 | 1;
+       else
+               regs[0] = DINA36;
+       if (inv_gyro_orient & 0x20)
+               regs[1] = DINA56 | 1;
+       else
+               regs[1] = DINA56;
+       if (inv_gyro_orient & 0x100)
+               regs[2] = DINA76 | 1;
+       else
+               regs[2] = DINA76;
+       result = mem_w_key(KEY_FCFG_3, ARRAY_SIZE(regs), regs);
+
+       return result;
+}
+
+static int inv_accel_dmp_cal(struct nvi_state *st)
+{
+       int inv_accel_orient;
+       int result;
+       u8 regs[3];
+       const u8 tmp[3] = { DINA0C, DINAC9, DINA2C };
+       inv_accel_orient =
+               inv_orientation_matrix_to_scaler(st->pdata.orientation);
+
+       regs[0] = tmp[inv_accel_orient & 3];
+       regs[1] = tmp[(inv_accel_orient >> 3) & 3];
+       regs[2] = tmp[(inv_accel_orient >> 6) & 3];
+       result = mem_w_key(KEY_FCFG_2, ARRAY_SIZE(regs), regs);
+       if (result)
+               return result;
+
+       regs[0] = DINA26;
+       regs[1] = DINA46;
+       regs[2] = DINA66;
+       if (inv_accel_orient & 4)
+               regs[0] |= 1;
+       if (inv_accel_orient & 0x20)
+               regs[1] |= 1;
+       if (inv_accel_orient & 0x100)
+               regs[2] |= 1;
+       result = mem_w_key(KEY_FCFG_7, ARRAY_SIZE(regs), regs);
+
+       return result;
+}
+
+int inv_set_accel_bias_dmp(struct nvi_state *st)
+{
+       int inv_accel_orient, result, i, accel_bias_body[3], out[3];
+       int tmp[] = {1, 1, 1};
+       int mask[] = {4, 0x20, 0x100};
+       int accel_sf = 0x20000000;/* 536870912 */
+       u8 *regs;
+
+       inv_accel_orient =
+               inv_orientation_matrix_to_scaler(st->pdata.orientation);
+
+       for (i = 0; i < 3; i++)
+               if (inv_accel_orient & mask[i])
+                       tmp[i] = -1;
+
+       for (i = 0; i < 3; i++)
+               accel_bias_body[i] =
+                       st->input_accel_dmp_bias[(inv_accel_orient >>
+                       (i * 3)) & 3] * tmp[i];
+       for (i = 0; i < 3; i++)
+               accel_bias_body[i] = inv_q30_mult(accel_sf,
+                                       accel_bias_body[i]);
+       for (i = 0; i < 3; i++)
+               out[i] = cpu_to_be32p(&accel_bias_body[i]);
+       regs = (u8 *)out;
+       result = mem_w_key(KEY_D_ACCEL_BIAS, sizeof(out), regs);
+
+       return result;
+}
+
+int write_be32_key_to_mem(struct nvi_state *st,
+                                       u32 data, int key)
+{
+       cpu_to_be32s(&data);
+       return mem_w_key(key, sizeof(data), (u8 *)&data);
+}
+
+/*
+ * inv_set_gyro_sf_dmp():
+ * The gyro threshold, in dps, above which taps will be rejected.
+ */
+static int inv_set_gyro_sf_dmp(struct nvi_state *st)
+{
+       int result;
+       u8 sample_divider;
+       u32 gyro_sf;
+       const u32 gyro_sens = 0x03e80000;
+
+       sample_divider = INIT_SAMPLE_DIVIDER;
+       gyro_sf = inv_q30_mult(gyro_sens,
+                       (int)(DMP_TAP_SCALE * (sample_divider + 1)));
+       result = write_be32_key_to_mem(st, gyro_sf, KEY_D_0_104);
+
+       return result;
+}
+
+/*
+ * inv_set_shake_reject_thresh_dmp():
+ * The gyro threshold, in dps, above which taps will be rejected.
+ */
+static int inv_set_shake_reject_thresh_dmp(struct nvi_state *st,
+                                               int thresh)
+{
+       int result;
+       u8 sample_divider;
+       int thresh_scaled;
+       u32 gyro_sf;
+       const u32 gyro_sens = 0x03e80000;
+
+       sample_divider = INIT_SAMPLE_DIVIDER;
+       gyro_sf = inv_q30_mult(gyro_sens, (int)(DMP_TAP_SCALE *
+                       (sample_divider + 1)));
+       /* We're in units of DPS, convert it back to chip units*/
+       /*split the operation to aviod overflow of integer*/
+       thresh_scaled = gyro_sens / (1L << 16);
+       thresh_scaled = thresh_scaled / thresh;
+       thresh_scaled = gyro_sf / thresh_scaled;
+       result = write_be32_key_to_mem(st, thresh_scaled,
+                                               KEY_DMP_TAP_SHAKE_REJECT);
+
+       return result;
+}
+
+/*
+ * inv_set_shake_reject_time_dmp():
+ * How long a gyro axis must remain above its threshold
+ * before taps are rejected.
+ */
+static int inv_set_shake_reject_time_dmp(struct nvi_state *st,
+                                               u32 time)
+{
+       int result;
+       u16 dmp_time;
+       u8 sample_divider;
+
+       sample_divider = INIT_SAMPLE_DIVIDER;
+       sample_divider++;
+
+       /* 60 ms minimum time added */
+       dmp_time = ((time) / sample_divider);
+       result = inv_write_2bytes(st, KEY_DMP_TAP_SHAKE_COUNT_MAX, dmp_time);
+
+       return result;
+}
+
+/*
+ * inv_set_shake_reject_timeout_dmp():
+ * How long the gyros must remain below their threshold,
+ * after taps have been rejected, before taps can be detected again.
+ */
+static int inv_set_shake_reject_timeout_dmp(struct nvi_state *st,
+                                               u32 time)
+{
+       int result;
+       u16 dmp_time;
+       u8 sample_divider;
+
+       sample_divider = INIT_SAMPLE_DIVIDER;
+       sample_divider++;
+
+       /* 60 ms minimum time added */
+       dmp_time = ((time) / sample_divider);
+       result = inv_write_2bytes(st, KEY_DMP_TAP_SHAKE_TIMEOUT_MAX, dmp_time);
+
+       return result;
+}
+
+int inv_set_interrupt_on_gesture_event(struct nvi_state *st, bool on)
+{
+       u8 r;
+       const u8 rn[] = {0xA3};
+       const u8 rf[] = {0xFE};
+
+       if (on)
+               r = mem_w_key(KEY_CFG_FIFO_INT, ARRAY_SIZE(rn), rn);
+       else
+               r = mem_w_key(KEY_CFG_FIFO_INT, ARRAY_SIZE(rf), rf);
+
+       return r;
+}
+
+/*
+ * inv_enable_tap_dmp() -  calling this function will enable/disable tap
+ *                         function.
+ */
+int inv_enable_tap_dmp(struct nvi_state *st, bool on)
+{
+       int result;
+
+       result = inv_set_tap_interrupt_dmp(st, on);
+       if (result)
+               return result;
+       result = inv_set_tap_threshold_dmp(st, st->tap.thresh);
+       if (result)
+               return result;
+
+       result = inv_set_min_taps_dmp(st, st->tap.min_count);
+       if (result)
+               return result;
+
+       result = inv_set_tap_time_dmp(st, st->tap.time);
+       if (result)
+               return result;
+
+       result = inv_set_multiple_tap_time_dmp(st, DMP_MULTI_TAP_TIME);
+       if (result)
+               return result;
+
+       result = inv_set_gyro_sf_dmp(st);
+       if (result)
+               return result;
+
+       result = inv_set_shake_reject_thresh_dmp(st, DMP_SHAKE_REJECT_THRESH);
+       if (result)
+               return result;
+
+       result = inv_set_shake_reject_time_dmp(st, DMP_SHAKE_REJECT_TIME);
+       if (result)
+               return result;
+
+       result = inv_set_shake_reject_timeout_dmp(st,
+                                                 DMP_SHAKE_REJECT_TIMEOUT);
+       return result;
+}
+
+static int inv_dry_run_dmp(struct nvi_state *st)
+{
+       int result;
+
+       result = nvi_pm(st, NVI_PM_ON_FULL);
+       if (result)
+               return result;
+       result = nvi_wr_user_ctrl(st, BIT_DMP_EN);
+       if (result)
+               return result;
+       msleep(400);
+       result = nvi_wr_user_ctrl(st, 0);
+       if (result)
+               return result;
+       result = nvi_pm(st, NVI_PM_ON);
+       if (result)
+               return result;
+
+       return 0;
+}
+
+static void inv_test_reset(struct nvi_state *st)
+{
+       int result, ii;
+       u8 d[0x80];
+
+       if (st->hal->part < MPU6500)
+               return;
+
+       for (ii = 3; ii < 0x80; ii++) {
+               /* don't read fifo r/w register */
+               if (ii != st->hal->reg->fifo_r_w.reg)
+                       nvi_i2c_rd(st, 0, ii, 1, &d[ii]);
+       }
+       result = nvi_wr_pwr_mgmt_1(st, BIT_H_RESET);
+       if (result)
+               return;
+       msleep(POWER_UP_TIME);
+
+       for (ii = 3; ii < 0x80; ii++) {
+               /* don't write certain registers */
+               if ((ii != REG_FIFO_R_W) &&
+                   (ii != REG_MEM_RW) &&
+                   (ii != REG_MEM_START_ADDR) &&
+                   (ii != REG_FIFO_COUNT_H) &&
+                   ii != REG_FIFO_COUNT_L)
+                       result = nvi_i2c_wr(st, ii, d[ii]);
+       }
+}
+
+/*
+ * inv_dmp_firmware_write() -  calling this function will load the firmware.
+ *                        This is the write function of file "dmp_firmware".
+ */
+static ssize_t inv_dmp_firmware_write(struct file *fp, struct kobject *kobj,
+       struct bin_attribute *attr,
+       char *buf, loff_t pos, size_t size)
+{
+       u8 *firmware;
+       int result;
+       struct iio_dev *indio_dev;
+       struct nvi_state *st;
+
+       indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj));
+       st = iio_priv(indio_dev);
+
+       if (st->chip_config.firmware_loaded)
+               return -EINVAL;
+       if (st->chip_config.enable)
+               return -EBUSY;
+
+       if (DMP_IMAGE_SIZE != size) {
+               pr_err("wrong DMP image size - expected %d, actual %d\n",
+                       DMP_IMAGE_SIZE, size);
+               return -EINVAL;
+       }
+
+       firmware = kmalloc(size, GFP_KERNEL);
+       if (!firmware)
+               return -ENOMEM;
+
+       mutex_lock(&indio_dev->mlock);
+
+       memcpy(firmware, buf, size);
+       result = crc32(CRC_FIRMWARE_SEED, firmware, size);
+       if (DMP_IMAGE_CRC_VALUE != result) {
+               pr_err("firmware CRC error - 0x%08x vs 0x%08x\n",
+                       result, DMP_IMAGE_CRC_VALUE);
+               result = -EINVAL;
+               goto firmware_write_fail;
+       }
+
+       result = nvi_pm(st, NVI_PM_ON);
+       if (result)
+               goto firmware_write_fail;
+       inv_test_reset(st);
+
+       result = inv_load_firmware(st, firmware, size);
+       if (result)
+               goto firmware_write_fail;
+
+       result = inv_verify_firmware(st, firmware, size);
+       if (result)
+               goto firmware_write_fail;
+
+       result = nvi_i2c_wr(st, REG_PRGM_STRT_ADDRH, DMP_START_ADDR >> 8);
+       if (result)
+               goto firmware_write_fail;
+       result = nvi_i2c_wr(st, REG_PRGM_STRT_ADDRL, DMP_START_ADDR & 0xff);
+       if (result)
+               goto firmware_write_fail;
+
+       result = inv_gyro_dmp_cal(st);
+       if (result)
+               goto firmware_write_fail;
+       result = inv_accel_dmp_cal(st);
+       if (result)
+               goto firmware_write_fail;
+       result = inv_dry_run_dmp(st);
+       if (result)
+               goto firmware_write_fail;
+
+       st->chip_config.firmware_loaded = 1;
+       st->master_enable |= (1 << EN_FW);
+
+firmware_write_fail:
+       result |= nvi_enable(indio_dev);
+       mutex_unlock(&indio_dev->mlock);
+       kfree(firmware);
+       if (result)
+               return result;
+
+       return size;
+}
+
+static ssize_t inv_dmp_firmware_read(struct file *filp,
+                               struct kobject *kobj,
+                               struct bin_attribute *bin_attr,
+                               char *buf, loff_t off, size_t count)
+{
+       int bank, write_size, size, data, result;
+       u16 memaddr;
+       struct iio_dev *indio_dev;
+       struct nvi_state *st;
+
+       size = count;
+       indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj));
+       st = iio_priv(indio_dev);
+
+       data = 0;
+       mutex_lock(&indio_dev->mlock);
+       result = nvi_pm(st, NVI_PM_ON);
+       if (result) {
+               mutex_unlock(&indio_dev->mlock);
+               return result;
+       }
+       for (bank = 0; size > 0; bank++, size -= write_size,
+                                       data += write_size) {
+               if (size > MPU_MEM_BANK_SIZE)
+                       write_size = MPU_MEM_BANK_SIZE;
+               else
+                       write_size = size;
+
+               memaddr = (bank << 8);
+               result = mpu_memory_read(st,
+                       st->i2c_addr, memaddr, write_size, &buf[data]);
+       }
+       result |= nvi_enable(indio_dev);
+       mutex_unlock(&indio_dev->mlock);
+       if (result)
+               return result;
+
+       return count;
+}
+
+static ssize_t inv_six_q_write(struct file *fp, struct kobject *kobj,
+       struct bin_attribute *attr, char *buf, loff_t pos, size_t size)
+{
+       u8 q[QUATERNION_BYTES];
+       struct iio_dev *indio_dev;
+       struct nvi_state *st;
+       int result;
+
+       indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj));
+       st = iio_priv(indio_dev);
+
+       mutex_lock(&indio_dev->mlock);
+
+       if (!st->chip_config.firmware_loaded) {
+               mutex_unlock(&indio_dev->mlock);
+               return -EINVAL;
+       }
+       if (st->chip_config.enable) {
+               mutex_unlock(&indio_dev->mlock);
+               return -EBUSY;
+       }
+       if (QUATERNION_BYTES != size) {
+               pr_err("wrong quaternion size=%d, should=%d\n", size,
+                                                       QUATERNION_BYTES);
+               mutex_unlock(&indio_dev->mlock);
+               return -EINVAL;
+       }
+
+       memcpy(q, buf, size);
+       result = nvi_pm(st, NVI_PM_ON);
+       if (result)
+               goto firmware_write_fail;
+       result = mem_w_key(KEY_DMP_Q0, QUATERNION_BYTES, q);
+
+firmware_write_fail:
+       result = nvi_enable(indio_dev);
+       mutex_unlock(&indio_dev->mlock);
+       if (result)
+               return result;
+
+       return size;
+}
+
+/*
+ *  inv_create_dmp_sysfs() - create binary sysfs dmp entry.
+ */
+static const struct bin_attribute dmp_firmware = {
+       .attr = {
+               .name = "dmp_firmware",
+               .mode = S_IRUGO | S_IWUSR
+       },
+       .size = DMP_IMAGE_SIZE + 32,
+       .read = inv_dmp_firmware_read,
+       .write = inv_dmp_firmware_write,
+};
+
+static const struct bin_attribute six_q_value = {
+       .attr = {
+               .name = "six_axes_q_value",
+               .mode = S_IWUSR
+       },
+       .size = QUATERNION_BYTES,
+       .read = NULL,
+       .write = inv_six_q_write,
+};
+
+int inv_create_dmp_sysfs(struct iio_dev *ind)
+{
+       int result;
+
+       result = sysfs_create_bin_file(&ind->dev.kobj, &dmp_firmware);
+       if (result)
+               return result;
+       result = sysfs_create_bin_file(&ind->dev.kobj, &six_q_value);
+
+       return result;
+}
+
diff --git a/drivers/iio/imu/nvi_mpu/nvi.c b/drivers/iio/imu/nvi_mpu/nvi.c
new file mode 100644 (file)
index 0000000..5810e39
--- /dev/null
@@ -0,0 +1,5243 @@
+/* Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/interrupt.h>
+#include <linux/regulator/consumer.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger.h>
+#include <linux/mpu_iio.h>
+
+#include "nvi.h"
+
+#define NVI_DRIVER_VERSION             (100)
+#define NVI_NAME                       "mpu6xxx"
+#define NVI_NAME_MPU6050               "MPU6050"
+#define NVI_NAME_MPU6500               "MPU6500"
+#define NVI_NAME_MPU6515               "MPU6515"
+#define NVI_NAME_ICM20628              "ICM20628"
+#define NVI_VENDOR                     "Invensense"
+
+/* _buf_push expects this scan order */
+#define NVI_SCAN_ACCEL_X               (0)
+#define NVI_SCAN_ACCEL_Y               (1)
+#define NVI_SCAN_ACCEL_Z               (2)
+#define NVI_SCAN_ANGLVEL_X             (3)
+#define NVI_SCAN_ANGLVEL_Y             (4)
+#define NVI_SCAN_ANGLVEL_Z             (5)
+#define NVI_SCAN_TEMP                  (6)
+#define NVI_SCAN_TIMESTAMP             (7)
+
+enum NVI_ATTR {
+       NVI_ATTR_ENABLE,
+       NVI_ATTR_PART,
+       NVI_ATTR_VENDOR,
+       NVI_ATTR_FLUSH,
+       NVI_ATTR_FIFO_RSRV_EVNT_CNT,
+       NVI_ATTR_FIFO_MAX_EVNT_CNT,
+       NVI_ATTR_ACCEL_PART,
+       NVI_ATTR_ACCEL_VERSION,
+       NVI_ATTR_ACCEL_MILLIAMP,
+       NVI_ATTR_ACCEL_BATCH_FLAGS,
+       NVI_ATTR_ACCEL_BATCH_PERIOD,
+       NVI_ATTR_ACCEL_BATCH_TIMEOUT,
+       NVI_ATTR_ANGLVEL_PART,
+       NVI_ATTR_ANGLVEL_VERSION,
+       NVI_ATTR_ANGLVEL_MILLIAMP,
+       NVI_ATTR_ANGLVEL_BATCH_FLAGS,
+       NVI_ATTR_ANGLVEL_BATCH_PERIOD,
+       NVI_ATTR_ANGLVEL_BATCH_TIMEOUT,
+       NVI_ATTR_TEMP_PART,
+       NVI_ATTR_TEMP_VERSION,
+       NVI_ATTR_TEMP_MILLIAMP,
+       INV_ATTR_SELF_TEST,
+};
+
+enum NVI_INFO {
+       NVI_INFO_DATA = 0,
+       NVI_INFO_VER,
+       NVI_INFO_ERRS,
+       NVI_INFO_RESET,
+       NVI_INFO_REGS,
+       NVI_INFO_DBG,
+       NVI_INFO_AUX_SPEW,
+       NVI_INFO_ACCEL_UC_SPEW,
+       NVI_INFO_ACCEL_SPEW,
+       NVI_INFO_ANGLVEL_SPEW,
+       NVI_INFO_TEMP_SPEW,
+       NVI_INFO_FIFO_SPEW,
+       NVI_INFO_FIFO_BUF,
+       NVI_INFO_FIFO_IRQ,
+       NVI_INFO_LIMIT_MAX,
+};
+
+/* regulator names in order of powering on */
+static char *nvi_vregs[] = {
+       "vdd",
+       "vlogic",
+};
+
+static unsigned short nvi_i2c_addrs[] = {
+       0x68,
+       0x69,
+};
+
+static int nvi_nb_vreg(struct nvi_state *st,
+                      unsigned long event, unsigned int i);
+
+static int nvi_nb_vreg_vdd(struct notifier_block *nb,
+                          unsigned long event, void *ignored)
+{
+       struct nvi_state *st = container_of(nb, struct nvi_state, nb_vreg[0]);
+
+       return nvi_nb_vreg(st, event, 0);
+}
+
+static int nvi_nb_vreg_vlogic(struct notifier_block *nb,
+                             unsigned long event, void *ignored)
+{
+       struct nvi_state *st = container_of(nb, struct nvi_state, nb_vreg[1]);
+
+       return nvi_nb_vreg(st, event, 1);
+}
+
+static int (* const nvi_nb_vreg_pf[])(struct notifier_block *nb,
+                                     unsigned long event, void *ignored) = {
+       nvi_nb_vreg_vdd,
+       nvi_nb_vreg_vlogic,
+};
+
+static unsigned long nvi_lpf_us_tbl[] = {
+       0, /* WAR: disabled 3906, 256Hz */
+       5319,   /* 188Hz */
+       10204,  /* 98Hz */
+       23810,  /* 42Hz */
+       50000,  /* 20Hz */
+       100000, /* 10Hz */
+       /* 200000, 5Hz */
+};
+
+static struct iio_dev *indio_dev_local;
+
+static void nvi_err(struct nvi_state *st)
+{
+       st->errs++;
+       if (!st->errs)
+               st->errs--;
+}
+
+int nvi_i2c_write(struct nvi_state *st, u16 addr, u16 len, u8 *buf)
+{
+       struct i2c_msg msg;
+
+       msg.addr = addr;
+       msg.flags = 0;
+       msg.len = len;
+       msg.buf = buf;
+       if (i2c_transfer(st->i2c->adapter, &msg, 1) != 1) {
+               nvi_err(st);
+               return -EIO;
+       }
+
+       return 0;
+}
+
+int nvi_i2c_wr(struct nvi_state *st, u8 reg, u8 val)
+{
+       u8 buf[2];
+       int ret = 0;
+
+       if (st->i2c_addr) {
+               buf[0] = reg;
+               buf[1] = val;
+               ret = nvi_i2c_write(st, st->i2c_addr, sizeof(buf), buf);
+               if (ret)
+                       st->i2c_addr = 0;
+       }
+       return ret;
+}
+
+/* Register REG_BANK_SEL */
+static int nvi_wr_reg_bank_sel(struct nvi_state *st, u8 reg_bank_sel)
+{
+       int ret = 0;
+
+       if (st->hal->part >= ICM20628) {
+               reg_bank_sel <<= 4;
+               if ((reg_bank_sel != st->rc.reg_bank_sel) || st->rc_dis) {
+                       ret = nvi_i2c_wr(st, st->hal->reg->reg_bank_sel.reg,
+                                        reg_bank_sel);
+                       if (ret) {
+                               dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                                       __func__, st->rc.reg_bank_sel,
+                                       reg_bank_sel, ret);
+                       } else {
+                               if (st->dbg & NVI_DBG_SPEW_MSG)
+                                       dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                                __func__, st->rc.reg_bank_sel,
+                                                reg_bank_sel);
+                               st->rc.reg_bank_sel = reg_bank_sel;
+                       }
+               }
+       }
+       return ret;
+}
+
+int nvi_i2c_read(struct nvi_state *st, u16 addr, u8 reg, u16 len, u8 *buf)
+{
+       struct i2c_msg msg[2];
+
+       msg[0].addr = addr;
+       msg[0].flags = 0;
+       msg[0].len = 1;
+       msg[0].buf = &reg;
+       msg[1].addr = addr;
+       msg[1].flags = I2C_M_RD;
+       msg[1].len = len;
+       msg[1].buf = buf;
+       if (i2c_transfer(st->i2c->adapter, msg, 2) != 2) {
+               nvi_err(st);
+               return -EIO;
+       }
+
+       return 0;
+}
+
+int nvi_i2c_rd(struct nvi_state *st, u8 bank, u8 reg, u16 len, u8 *buf)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, bank);
+       if (st->i2c_addr && !ret) {
+               ret = nvi_i2c_read(st, st->i2c_addr, reg, len, buf);
+               if (ret)
+                       st->i2c_addr = 0;
+       }
+       return ret;
+}
+
+/* Register ACCEL OFFSET */
+static int nvi_rd_accel_offset(struct nvi_state *st)
+{
+       u8 buf[2];
+       unsigned int i;
+       int ret;
+
+       for (i = 0; i < AXIS_N; i++) {
+               ret = nvi_i2c_rd(st, st->hal->reg->a_offset_h[i].bank,
+                                st->hal->reg->a_offset_h[i].reg, 2, buf);
+               if (!ret)
+                       st->rc.accel_offset[i] = be16_to_cpup((__be16 *)buf);
+       }
+       return ret;
+}
+
+/* Register ACCEL OFFSET */
+int nvi_wr_accel_offset(struct nvi_state *st, unsigned int axis, u16 offset)
+{
+       u8 buf[3];
+       u16 offset_le;
+       int ret;
+
+       if (axis >= AXIS_N)
+               return -EINVAL;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->a_offset_h[axis].bank);
+       if ((!ret) && ((offset != st->rc.accel_offset[axis]) || st->rc_dis)) {
+               if (st->i2c_addr) {
+                       buf[0] = st->hal->reg->a_offset_h[axis].reg;
+                       offset_le = cpu_to_le16(offset);
+                       buf[1] = offset_le >> 8;
+                       buf[2] = offset_le & 0xFF;
+                       ret = nvi_i2c_write(st, st->i2c_addr,
+                                           sizeof(buf), buf);
+                       if (ret) {
+                               dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                                       __func__, st->rc.accel_offset[axis],
+                                       offset, ret);
+                       } else {
+                               if (st->dbg & NVI_DBG_SPEW_MSG)
+                                       dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                                __func__,
+                                                st->rc.accel_offset[axis],
+                                                offset);
+                               st->rc.accel_offset[axis] = offset;
+                       }
+               }
+       }
+       return ret;
+}
+
+/* Register GYRO OFFSET */
+static int nvi_rd_gyro_offset(struct nvi_state *st)
+{
+       u8 buf[AXIS_N * 2];
+       unsigned int i;
+       int ret;
+
+       ret = nvi_i2c_rd(st, st->hal->reg->xg_offset_h.bank,
+                        st->hal->reg->xg_offset_h.reg, 6, buf);
+       if (!ret) {
+               for (i = 0; i < AXIS_N; i++)
+                       st->rc.gyro_offset[i] = be16_to_cpup((__be16 *)
+                                                            (&buf[i << 1]));
+       }
+       return ret;
+}
+
+/* Register GYRO OFFSET */
+int nvi_wr_gyro_offset(struct nvi_state *st, unsigned int axis, u16 offset)
+{
+       u8 buf[3];
+       u16 offset_le;
+       int ret;
+
+       if (axis >= AXIS_N)
+               return -EINVAL;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->xg_offset_h.bank);
+       if ((!ret) && ((offset != st->rc.gyro_offset[axis]) || st->rc_dis)) {
+               if (st->i2c_addr) {
+                       buf[0] = st->hal->reg->xg_offset_h.reg + (axis << 1);
+                       offset_le = cpu_to_le16(offset);
+                       buf[1] = offset_le >> 8;
+                       buf[2] = offset_le & 0xFF;
+                       ret = nvi_i2c_write(st, st->i2c_addr,
+                                           sizeof(buf), buf);
+                       if (ret) {
+                               dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                                       __func__, st->rc.gyro_offset[axis],
+                                       offset, ret);
+                       } else {
+                               if (st->dbg & NVI_DBG_SPEW_MSG)
+                                       dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                                __func__,
+                                                st->rc.gyro_offset[axis],
+                                                offset);
+                               st->rc.gyro_offset[axis] = offset;
+                       }
+               }
+       }
+       return ret;
+}
+
+/* Register SMPLRT_DIV */
+int nvi_wr_smplrt_div(struct nvi_state *st, u8 smplrt_div)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->smplrt_div.bank);
+       if ((!ret) && ((smplrt_div != st->rc.smplrt_div) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->smplrt_div.reg, smplrt_div);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.smplrt_div, smplrt_div, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.smplrt_div,
+                                        smplrt_div);
+                       st->rc.smplrt_div = smplrt_div;
+               }
+       }
+       return ret;
+}
+
+/* Register CONFIG */
+int nvi_wr_config(struct nvi_state *st, u8 val)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->config.bank);
+       if ((!ret) && ((val != st->rc.config) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->config.reg, val);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.config, val, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.config, val);
+                       st->rc.config = val;
+                       ret = 1; /* flag change made */
+               }
+       }
+       return ret;
+}
+
+/* Register GYRO_CONFIG */
+int nvi_wr_gyro_config(struct nvi_state *st, u8 test, u8 fsr)
+{
+       u8 val;
+       int ret;
+
+       val = (test << 5) | (fsr << 3);
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->gyro_config.bank);
+       if ((!ret) && ((val != st->rc.gyro_config) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->gyro_config.reg, val);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.gyro_config, val, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.gyro_config, val);
+                       st->rc.gyro_config = val;
+                       ret = 1; /* flag change made */
+               }
+       }
+       return ret;
+}
+
+/* Register ACCEL_CONFIG2 */
+int nvi_wr_accel_config2(struct nvi_state *st, u8 val)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->accel_config2.bank);
+       if ((!ret) && ((val != st->rc.accel_config2) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->accel_config2.reg, val);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.accel_config2, val, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.accel_config2, val);
+                       st->rc.accel_config2 = val;
+                       ret = 1; /* flag change made */
+               }
+       }
+       return ret;
+}
+
+/* Register ACCEL_CONFIG */
+int nvi_wr_accel_config(struct nvi_state *st, u8 test, u8 fsr, u8 hpf)
+{
+       u8 val;
+       int ret;
+       int ret_t = 0;
+
+       val = (test << 5) | (fsr << 3);
+       if (st->hal->part >= MPU6500)
+               ret_t = nvi_wr_accel_config2(st, BIT_FIFO_SIZE_1K | hpf);
+       else
+               val |= hpf;
+       ret_t |= nvi_wr_reg_bank_sel(st, st->hal->reg->accel_config.bank);
+       if ((!ret_t) && ((val != st->rc.accel_config) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->accel_config.reg, val);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.accel_config, val, ret);
+                       ret_t |= ret;
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.accel_config, val);
+                       st->rc.accel_config = val;
+                       ret_t |= 1; /* flag change made */
+               }
+       }
+       return ret_t;
+}
+
+/* Register LP_ACCEL_ODR */
+static int nvi_wr_lp_accel_odr(struct nvi_state *st, u8 lp_accel_odr)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->lp_accel_odr.bank);
+       if ((!ret) && ((lp_accel_odr != st->rc.lp_accel_odr) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->lp_accel_odr.reg,
+                                lp_accel_odr);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.lp_accel_odr,
+                               lp_accel_odr, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.lp_accel_odr,
+                                        lp_accel_odr);
+                       st->rc.lp_accel_odr = lp_accel_odr;
+               }
+       }
+       return ret;
+}
+
+
+/* Register MOT_THR */
+static int nvi_wr_mot_thr(struct nvi_state *st, u8 mot_thr)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->mot_thr.bank);
+       if ((!ret) && ((mot_thr != st->rc.mot_thr) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->mot_thr.reg, mot_thr);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.mot_thr, mot_thr, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.mot_thr, mot_thr);
+                       st->rc.mot_thr = mot_thr;
+               }
+       }
+       return ret;
+}
+
+/* Register MOT_DUR */
+static int nvi_wr_mot_dur(struct nvi_state *st, u8 mot_dur)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->mot_dur.bank);
+       if ((!ret) && ((mot_dur != st->rc.mot_dur) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->mot_dur.reg, mot_dur);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.mot_dur, mot_dur, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.mot_dur, mot_dur);
+                       st->rc.mot_dur = mot_dur;
+               }
+       }
+       return ret;
+}
+
+/* Register FIFO_EN */
+int nvi_wr_fifo_en(struct nvi_state *st, u8 fifo_en)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->fifo_en.bank);
+       if ((!ret) && ((fifo_en != st->rc.fifo_en) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->fifo_en.reg, fifo_en);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.fifo_en, fifo_en, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.fifo_en, fifo_en);
+                       st->rc.fifo_en = fifo_en;
+               }
+       }
+       return ret;
+}
+
+/* Register I2C_MST_CTRL */
+static int nvi_wr_i2c_mst_ctrl(struct nvi_state *st,
+                              bool port3_fifo_en)
+{
+       u8 val;
+       int ret;
+
+       val = st->aux.clock_i2c;
+       val |= BIT_WAIT_FOR_ES;
+       if (port3_fifo_en)
+               val |= BIT_SLV3_FIFO_EN;
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->i2c_mst_ctrl.bank);
+       if ((!ret) && ((val != st->rc.i2c_mst_ctrl) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->i2c_mst_ctrl.reg, val);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.i2c_mst_ctrl, val, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.i2c_mst_ctrl, val);
+                       st->rc.i2c_mst_ctrl = val;
+               }
+       }
+       return ret;
+}
+
+/* Register I2C_SLV0_ADDR */
+/* Register I2C_SLV1_ADDR */
+/* Register I2C_SLV2_ADDR */
+/* Register I2C_SLV3_ADDR */
+/* Register I2C_SLV4_ADDR */
+static int nvi_wr_i2c_slv_addr(struct nvi_state *st, int port, u8 addr)
+{
+       u8 reg;
+       int ret;
+
+       if (st->hal->part >= ICM20628)
+               reg = (st->hal->reg->i2c_slv0_addr.reg + (port << 2));
+       else
+               reg = (st->hal->reg->i2c_slv0_addr.reg + (port * 3));
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->i2c_slv0_addr.bank);
+       if ((!ret) && ((addr != st->rc.i2c_slv_addr[port]) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, reg, addr);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s[%d]: %x->%x ERR=%d\n",
+                               __func__, port, st->rc.i2c_slv_addr[port],
+                               addr, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s[%d]: %x->%x\n",
+                                        __func__, port,
+                                        st->rc.i2c_slv_addr[port], addr);
+                       st->rc.i2c_slv_addr[port] = addr;
+               }
+       }
+       return ret;
+}
+
+/* Register I2C_SLV0_REG */
+/* Register I2C_SLV1_REG */
+/* Register I2C_SLV2_REG */
+/* Register I2C_SLV3_REG */
+/* Register I2C_SLV4_REG */
+static int nvi_wr_i2c_slv_reg(struct nvi_state *st, int port, u8 val)
+{
+       u8 reg;
+       int ret;
+
+       if (st->hal->part >= ICM20628)
+               reg = (st->hal->reg->i2c_slv0_reg.reg + (port << 2));
+       else
+               reg = (st->hal->reg->i2c_slv0_reg.reg + (port * 3));
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->i2c_slv0_reg.bank);
+       if ((!ret) && ((val != st->rc.i2c_slv_reg[port]) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, reg, val);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s[%d]: %x->%x ERR=%d\n",
+                               __func__, port,
+                               st->rc.i2c_slv_reg[port], val, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s[%d]: %x->%x\n",
+                                        __func__, port,
+                                        st->rc.i2c_slv_reg[port], val);
+                       st->rc.i2c_slv_reg[port] = val;
+               }
+       }
+       return ret;
+}
+
+/* Register I2C_SLV0_CTRL */
+/* Register I2C_SLV1_CTRL */
+/* Register I2C_SLV2_CTRL */
+/* Register I2C_SLV3_CTRL */
+static int nvi_wr_i2c_slv_ctrl(struct nvi_state *st, int port, u8 val)
+{
+       u8 reg;
+       int ret;
+
+       if (st->hal->part >= ICM20628)
+               reg = (st->hal->reg->i2c_slv0_ctrl.reg + (port << 2));
+       else
+               reg = (st->hal->reg->i2c_slv0_ctrl.reg + (port * 3));
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->i2c_slv0_ctrl.bank);
+       if ((!ret) && ((val != st->rc.i2c_slv_ctrl[port]) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, reg, val);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s[%d]: %x->%x ERR=%d\n",
+                               __func__, port,
+                               st->rc.i2c_slv_ctrl[port], val, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s[%d]: %x->%x\n",
+                                        __func__, port,
+                                        st->rc.i2c_slv_ctrl[port], val);
+                       st->rc.i2c_slv_ctrl[port] = val;
+                       ret = 1; /* flag change made */
+               }
+       }
+       return ret;
+}
+
+/* Register I2C_SLV4_CTRL */
+static int nvi_wr_i2c_slv4_ctrl(struct nvi_state *st, bool slv4_en)
+{
+       u8 val;
+       int ret;
+
+       val = st->aux.delay_hw;
+       val |= (st->aux.port[AUX_PORT_SPECIAL].nmp.ctrl & BIT_I2C_SLV_REG_DIS);
+       if (slv4_en)
+               val |= BIT_SLV_EN;
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->i2c_slv4_ctrl.bank);
+       if ((!ret) && ((val != st->rc.i2c_slv4_ctrl) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->i2c_slv4_ctrl.reg, val);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.i2c_slv4_ctrl, val, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.i2c_slv4_ctrl, val);
+                       st->rc.i2c_slv4_ctrl = val;
+                       ret = 1; /* flag change made */
+               }
+       }
+       return ret;
+}
+
+/* Register INT_PIN_CFG */
+static int nvi_wr_int_pin_cfg(struct nvi_state *st, u8 val)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->int_pin_cfg.bank);
+       if ((!ret) && ((val != st->rc.int_pin_cfg) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->int_pin_cfg.reg, val);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.int_pin_cfg, val, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.int_pin_cfg, val);
+                       st->rc.int_pin_cfg = val;
+               }
+       }
+       return ret;
+}
+
+/* Register INT_ENABLE */
+int nvi_wr_int_enable(struct nvi_state *st, u8 int_enable)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->int_enable.bank);
+       if ((!ret) && ((int_enable != st->rc.int_enable) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->int_enable.reg, int_enable);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.int_enable, int_enable, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.int_enable,
+                                        int_enable);
+                       st->rc.int_enable = int_enable;
+               }
+       }
+       if (int_enable && st->irq_dis) {
+               enable_irq(st->i2c->irq);
+               st->irq_dis = false;
+       }
+       return ret;
+}
+
+static int nvi_int_able(struct nvi_state *st, bool enable)
+{
+       u8 int_enable = 0;
+       int ret;
+
+       if (enable) {
+               if (st->master_enable & (1 << DEV_DMP))
+                       int_enable |= BIT_DMP_INT_EN;
+               else if (st->master_enable & (DEV_MPU_MASK | (1 << DEV_AUX)))
+                       int_enable |= BIT_DATA_RDY_EN;
+       }
+       ret = nvi_wr_int_enable(st, int_enable);
+       return ret;
+}
+
+/* Register I2C_SLV0_DO */
+/* Register I2C_SLV1_DO */
+/* Register I2C_SLV2_DO */
+/* Register I2C_SLV3_DO */
+/* Register I2C_SLV4_DO */
+static int nvi_wr_i2c_slv_do(struct nvi_state *st, int port, u8 data_out)
+{
+       u8 *rc;
+       u8 reg;
+       int ret;
+
+       if (port == AUX_PORT_SPECIAL) {
+               rc = &st->rc.i2c_slv4_do;
+               reg = st->hal->reg->i2c_slv4_do.reg;
+       } else {
+               rc = &st->rc.i2c_slv_do[port];
+               reg = st->hal->reg->i2c_slv0_do.reg + port;
+       }
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->i2c_slv0_do.bank);
+       if ((!ret) && ((data_out != *rc) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, reg, data_out);
+               if (!ret)
+                       *rc = data_out;
+       }
+       return ret;
+}
+
+/* Register I2C_MST_DELAY_CTRL */
+static int nvi_wr_i2c_mst_delay_ctrl(struct nvi_state *st,
+                                    u8 i2c_mst_delay_ctrl)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->i2c_mst_delay_ctrl.bank);
+       if ((!ret) && ((i2c_mst_delay_ctrl != st->rc.i2c_mst_delay_ctrl) ||
+                      st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->i2c_mst_delay_ctrl.reg,
+                                i2c_mst_delay_ctrl);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.i2c_mst_delay_ctrl,
+                               i2c_mst_delay_ctrl, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.i2c_mst_delay_ctrl,
+                                        i2c_mst_delay_ctrl);
+                       st->rc.i2c_mst_delay_ctrl = i2c_mst_delay_ctrl;
+               }
+       }
+       return ret;
+}
+
+/* Register USER_CTRL */
+static int nvi_wr_user_ctrl_rc(struct nvi_state *st, u8 user_ctrl)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->user_ctrl.bank);
+       if ((!ret) && ((user_ctrl != st->rc.user_ctrl) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->user_ctrl.reg, user_ctrl);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.user_ctrl, user_ctrl, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.user_ctrl, user_ctrl);
+                       st->rc.user_ctrl = user_ctrl;
+               }
+       }
+       return ret;
+}
+
+int nvi_user_ctrl_en(struct nvi_state *st, bool fifo_enable, bool i2c_enable)
+{
+       u8 val = 0;
+       u16 fifo_sample_size = 0;
+       bool en = false;
+       int i;
+       int ret;
+
+       st->fifo_sample_size = 0;
+       if (fifo_enable) {
+               if (st->enable[DEV_ACCEL]) {
+                       val |= BIT_ACCEL_OUT;
+                       fifo_sample_size += 6;
+               }
+               if (st->enable[DEV_TEMP] && st->chip_config.temp_fifo_en) {
+                       val |= BIT_TEMP_OUT;
+                       fifo_sample_size += 2;
+               }
+               if (st->enable[DEV_ANGLVEL]) {
+                       val |= (st->enable[DEV_ANGLVEL] << 4);
+                       if (val & BIT_GYRO_XOUT)
+                               fifo_sample_size += 2;
+                       if (val & BIT_GYRO_YOUT)
+                               fifo_sample_size += 2;
+                       if (val & BIT_GYRO_ZOUT)
+                               fifo_sample_size += 2;
+               }
+               for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+                       if (st->aux.port[i].fifo_en &&
+                                 (st->aux.port[i].nmp.addr & BIT_I2C_READ) &&
+                                     (st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN)) {
+                               if (i == 3)
+                                       en = true;
+                               else
+                                       val |= (1 << i);
+                               fifo_sample_size += st->aux.port[i].nmp.ctrl &
+                                                   BITS_I2C_SLV_CTRL_LEN;
+                       }
+               }
+               st->fifo_sample_size = fifo_sample_size;
+       }
+       if (st->master_enable & (1 << DEV_DMP)) {
+               ret = nvi_wr_i2c_mst_ctrl(st, false);
+               ret |= nvi_wr_fifo_en(st, 0);
+       } else {
+               ret = nvi_wr_i2c_mst_ctrl(st, en);
+               ret |= nvi_wr_fifo_en(st, val);
+       }
+       if (val || en)
+               en = true;
+       val = 0;
+       if (i2c_enable && (st->master_enable & (1 << DEV_AUX)))
+               val |= BIT_I2C_MST_EN;
+       if (fifo_enable && en) {
+               val |= BIT_FIFO_EN;
+               if (st->master_enable & (1 << DEV_DMP))
+                       val |= BIT_DMP_EN;
+       }
+       ret |= nvi_wr_user_ctrl_rc(st, val);
+       return ret;
+}
+
+/* Register USER_CTRL */
+int nvi_wr_user_ctrl(struct nvi_state *st, u8 user_ctrl)
+{
+       bool fifo_enable = true;
+       bool i2c_enable = true;
+       int i;
+       int ret;
+       int ret_t;
+
+       if (!(user_ctrl & BITS_USER_CTRL_RST))
+               return nvi_wr_user_ctrl_rc(st, user_ctrl);
+
+       if (user_ctrl & BIT_SIG_COND_RST)
+               user_ctrl = BITS_USER_CTRL_RST;
+       if (user_ctrl & BIT_DMP_RST)
+               user_ctrl |= BIT_FIFO_RST;
+       if (user_ctrl & BIT_FIFO_RST) {
+               st->flush = true;
+               fifo_enable = false;
+       }
+       if (user_ctrl & BIT_I2C_MST_RST)
+               i2c_enable = false;
+       /* must make sure FIFO is off or IRQ storm will occur */
+       nvi_user_ctrl_en(st, fifo_enable, i2c_enable);
+       ret_t = nvi_wr_reg_bank_sel(st, st->hal->reg->user_ctrl.bank);
+       if (!ret_t) {
+               ret_t =  nvi_i2c_wr(st, st->hal->reg->user_ctrl.reg,
+                                   user_ctrl);
+               for (i = 0; i < POWER_UP_TIME; i++) {
+                       user_ctrl = -1;
+                       ret = nvi_i2c_rd(st, st->hal->reg->user_ctrl.bank,
+                                        st->hal->reg->user_ctrl.reg, 1,
+                                        &user_ctrl);
+                       if (!(user_ctrl & BITS_USER_CTRL_RST))
+                               break;
+
+                       mdelay(1);
+               }
+               ret_t |= ret;
+               st->rc.user_ctrl = user_ctrl;
+       }
+       return ret_t;
+}
+
+/* Register PWR_MGMT_1 */
+static int nvi_wr_pwr_mgmt_1_war(struct nvi_state *st)
+{
+       u8 val;
+       int i;
+       int ret;
+
+       nvi_wr_reg_bank_sel(st, st->hal->reg->pwr_mgmt_1.bank);
+       for (i = 0; i < (POWER_UP_TIME / REG_UP_TIME); i++) {
+               ret = nvi_i2c_wr(st, st->hal->reg->pwr_mgmt_1.reg, 0);
+               mdelay(REG_UP_TIME);
+               val = -1;
+               ret = nvi_i2c_rd(st, st->hal->reg->pwr_mgmt_1.bank,
+                                st->hal->reg->pwr_mgmt_1.reg, 1, &val);
+               if ((!ret) && (!val))
+                       break;
+       }
+       st->rc.pwr_mgmt_1 = val;
+       return ret;
+}
+
+/* Register PWR_MGMT_1 */
+int nvi_wr_pwr_mgmt_1(struct nvi_state *st, u8 pwr_mgmt_1)
+{
+       unsigned int i;
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->pwr_mgmt_1.bank);
+       if ((!ret) && ((pwr_mgmt_1 != st->rc.pwr_mgmt_1) || st->rc_dis)) {
+               if (pwr_mgmt_1 & BIT_H_RESET)
+                       nvi_wr_user_ctrl(st, BITS_USER_CTRL_RST);
+               ret = nvi_i2c_wr(st, st->hal->reg->pwr_mgmt_1.reg, pwr_mgmt_1);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.pwr_mgmt_1, pwr_mgmt_1, ret);
+               } else {
+                       if (pwr_mgmt_1 & BIT_H_RESET) {
+                               memset(&st->rc, 0, sizeof(struct nvi_rc));
+                               st->poll_delay_us = 0;
+                               for (i = 0; i < (POWER_UP_TIME / REG_UP_TIME);
+                                                                        i++) {
+                                       mdelay(REG_UP_TIME);
+                                       pwr_mgmt_1 = -1;
+                                       ret = nvi_i2c_rd(st,
+                                                st->hal->reg->pwr_mgmt_1.bank,
+                                                 st->hal->reg->pwr_mgmt_1.reg,
+                                                        1, &pwr_mgmt_1);
+                                       if ((!ret) &&
+                                                (!(pwr_mgmt_1 & BIT_H_RESET)))
+                                               break;
+                               }
+                               nvi_rd_accel_offset(st);
+                               nvi_rd_gyro_offset(st);
+                       }
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.pwr_mgmt_1,
+                                        pwr_mgmt_1);
+                       st->rc.pwr_mgmt_1 = pwr_mgmt_1;
+               }
+       }
+       return ret;
+}
+
+/* Register PWR_MGMT_2 */
+static int nvi_wr_pwr_mgmt_2(struct nvi_state *st, u8 pwr_mgmt_2)
+{
+       int ret;
+
+       ret = nvi_wr_reg_bank_sel(st, st->hal->reg->pwr_mgmt_2.bank);
+       if ((!ret) && ((pwr_mgmt_2 != st->rc.pwr_mgmt_2) || st->rc_dis)) {
+               ret = nvi_i2c_wr(st, st->hal->reg->pwr_mgmt_2.reg, pwr_mgmt_2);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s: %x->%x ERR=%d\n",
+                               __func__, st->rc.pwr_mgmt_2, pwr_mgmt_2, ret);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_MSG)
+                               dev_info(&st->i2c->dev, "%s: %x->%x\n",
+                                        __func__, st->rc.pwr_mgmt_2,
+                                        pwr_mgmt_2);
+                       st->rc.pwr_mgmt_2 = pwr_mgmt_2;
+               }
+       }
+       return ret;
+}
+
+static int nvi_vreg_dis(struct nvi_state *st, unsigned int i)
+{
+       int ret = 0;
+
+       if (st->vreg[i].ret && (st->vreg[i].consumer != NULL)) {
+               ret = regulator_disable(st->vreg[i].consumer);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s %s ERR\n",
+                               __func__, st->vreg[i].supply);
+               } else {
+                       st->vreg[i].ret = 0;
+                       dev_dbg(&st->i2c->dev, "%s %s\n",
+                               __func__, st->vreg[i].supply);
+               }
+       }
+       return ret;
+}
+
+static int nvi_vreg_dis_all(struct nvi_state *st)
+{
+       unsigned int i;
+       int ret = 0;
+
+       for (i = ARRAY_SIZE(nvi_vregs); i > 0; i--)
+               ret |= nvi_vreg_dis(st, (i - 1));
+       return ret;
+}
+
+static int nvi_vreg_en(struct nvi_state *st, unsigned int i)
+{
+       int ret = 0;
+
+       if ((!st->vreg[i].ret) && (st->vreg[i].consumer != NULL)) {
+               ret = regulator_enable(st->vreg[i].consumer);
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s %s ERR\n",
+                               __func__, st->vreg[i].supply);
+               } else {
+                       st->vreg[i].ret = 1;
+                       dev_dbg(&st->i2c->dev, "%s %s\n",
+                               __func__, st->vreg[i].supply);
+                       ret = 1; /* flag regulator state change */
+               }
+       }
+       return ret;
+}
+
+static int nvi_vreg_en_all(struct nvi_state *st)
+{
+       unsigned i;
+       int ret = 0;
+
+       for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++)
+               ret |= nvi_vreg_en(st, i);
+       return ret;
+}
+
+static void nvi_vreg_exit(struct nvi_state *st)
+{
+       int i;
+
+       for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++) {
+               if (st->vreg[i].consumer != NULL) {
+                       regulator_unregister_notifier(st->vreg[i].consumer,
+                                                     &st->nb_vreg[i]);
+                       devm_regulator_put(st->vreg[i].consumer);
+                       st->vreg[i].consumer = NULL;
+                       dev_dbg(&st->i2c->dev, "%s %s\n",
+                               __func__, st->vreg[i].supply);
+               }
+       }
+}
+
+static int nvi_vreg_init(struct nvi_state *st)
+{
+       unsigned int i;
+       int ret = 0;
+
+       for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++) {
+               st->vreg[i].supply = nvi_vregs[i];
+               st->vreg[i].ret = 0;
+               st->vreg[i].consumer = devm_regulator_get(&st->i2c->dev,
+                                                         st->vreg[i].supply);
+               if (IS_ERR(st->vreg[i].consumer)) {
+                       ret = PTR_ERR(st->vreg[i].consumer);
+                       dev_err(&st->i2c->dev, "%s ERR %d for %s\n",
+                               __func__, ret, st->vreg[i].supply);
+                       st->vreg_en_ts[i] = iio_get_time_ns();
+                       st->vreg[i].consumer = NULL;
+               } else {
+                       ret = regulator_is_enabled(st->vreg[i].consumer);
+                       if (ret > 0)
+                               st->vreg_en_ts[i] = iio_get_time_ns();
+                       else
+                               st->vreg_en_ts[i] = 0;
+                       st->nb_vreg[i].notifier_call = nvi_nb_vreg_pf[i];
+                       ret = regulator_register_notifier(st->vreg[i].consumer,
+                                                         &st->nb_vreg[i]);
+                       dev_dbg(&st->i2c->dev, "%s %s enable_ts=%lld\n",
+                               __func__, st->vreg[i].supply,
+                               st->vreg_en_ts[i]);
+               }
+       }
+       return ret;
+}
+
+static int nvi_nb_vreg(struct nvi_state *st,
+                      unsigned long event, unsigned int i)
+{
+       if (event & REGULATOR_EVENT_POST_ENABLE)
+               st->vreg_en_ts[i] = iio_get_time_ns();
+       else if (event & (REGULATOR_EVENT_DISABLE |
+                         REGULATOR_EVENT_FORCE_DISABLE))
+               st->vreg_en_ts[i] = 0;
+       if (st->dbg & NVI_DBG_SPEW_MSG)
+               dev_info(&st->i2c->dev, "%s %s event=0x%x ts=%lld\n",
+                        __func__, st->vreg[i].supply, (unsigned int)event,
+                        st->vreg_en_ts[i]);
+       return NOTIFY_OK;
+}
+
+int nvi_pm_wr(struct nvi_state *st, u8 pwr_mgmt_1, u8 pwr_mgmt_2, u8 lp)
+{
+       s64 por_ns;
+       bool rc_dis;
+       unsigned int delay_ms;
+       unsigned int i;
+       int ret;
+       int ret_t = 0;
+
+       ret = nvi_vreg_en_all(st);
+       if (ret) {
+               rc_dis = st->rc_dis;
+               st->rc_dis = true;
+               delay_ms = 0;
+               for (i = 0; i < ARRAY_SIZE(nvi_vregs); i++) {
+                       por_ns = iio_get_time_ns() - st->vreg_en_ts[i];
+                       if ((por_ns < 0) || (!st->vreg_en_ts[i])) {
+                               delay_ms = (POR_MS * 1000000);
+                               break;
+                       }
+
+                       if (por_ns < (POR_MS * 1000000)) {
+                               por_ns = (POR_MS * 1000000) - por_ns;
+                               if (por_ns > delay_ms)
+                                       delay_ms = (unsigned int)por_ns;
+                       }
+               }
+               delay_ms /= 1000000;
+               if (st->dbg & NVI_DBG_SPEW_MSG)
+                       dev_info(&st->i2c->dev, "%s %ums delay\n",
+                                __func__, delay_ms);
+               if (delay_ms)
+                       msleep(delay_ms);
+               ret_t |= nvi_wr_pwr_mgmt_1_war(st);
+               ret_t |= nvi_wr_pwr_mgmt_1(st, BIT_H_RESET);
+               ret_t |= nvi_wr_pwr_mgmt_1_war(st);
+               st->rc_dis = rc_dis;
+       } else {
+               ret_t |= nvi_wr_pwr_mgmt_1_war(st);
+       }
+       switch (st->hal->part) {
+       case MPU6050:
+               pwr_mgmt_2 |= lp << 6;
+               ret = nvi_wr_pwr_mgmt_2(st, pwr_mgmt_2);
+               if (ret)
+                       ret_t |= ret;
+               else
+                       st->rc.lp_accel_odr = lp;
+               ret_t |= nvi_wr_pwr_mgmt_1(st, pwr_mgmt_1);
+               break;
+
+       default: /* INV_MPU65XX */
+               if (pwr_mgmt_1 & BIT_CYCLE) {
+                       ret_t |= nvi_wr_lp_accel_odr(st, lp);
+                       ret_t |= nvi_wr_accel_config2(st, BIT_FIFO_SIZE_1K |
+                                                     BIT_ACCEL_FCHOCIE_B);
+               }
+               ret_t |= nvi_wr_pwr_mgmt_2(st, pwr_mgmt_2);
+               ret_t |= nvi_wr_pwr_mgmt_1(st, pwr_mgmt_1);
+               if (!(pwr_mgmt_1 & BIT_CYCLE))
+                       ret_t |= nvi_wr_accel_config2(st, BIT_FIFO_SIZE_1K);
+               break;
+       }
+
+       return ret_t;
+}
+
+static int nvi_reset(struct nvi_state *st,
+                    bool reset_fifo, bool reset_i2c);
+static int nvi_aux_bypass_enable(struct nvi_state *st, bool enable);
+
+/**
+ * @param st
+ * @param pm_req: call with one of the following:
+ *      NVI_PM_OFF_FORCE = force off state
+ *      NVI_PM_ON = minimum power for device access
+ *      NVI_PM_ON_FULL = power for anglvel
+ *      NVI_PM_AUTO = automatically sets power for configuration
+ *      Typical use is to set needed power for configuration and
+ *      then call with NVI_PM_AUTO when done.
+ *      All other NVI_PM_ levels are handled automatically and
+ *      are for internal use.
+ * @return int: returns 0 for success or error code
+ */
+int nvi_pm(struct nvi_state *st, int pm_req)
+{
+       bool irq;
+       u8 pwr_mgmt_1;
+       u8 pwr_mgmt_2;
+       u8 lp;
+       int i;
+       int pm;
+       int ret = 0;
+
+       lp = st->rc.lp_accel_odr;
+       if (pm_req == NVI_PM_AUTO) {
+               pwr_mgmt_2 = (((~st->enable[DEV_ACCEL]) << 3) &
+                             BIT_PWR_ACCEL_STBY);
+               pwr_mgmt_2 |= ((~st->enable[DEV_ANGLVEL]) & BIT_PWR_GYRO_STBY);
+               if (st->master_enable & DEV_PM_ON_FULL) {
+                       pm = NVI_PM_ON_FULL;
+               } else if (st->master_enable & DEV_PM_ON) {
+                       pm = NVI_PM_ON;
+               } else if ((st->master_enable & DEV_PM_LPA) == DEV_PM_LPA) {
+                       if (st->delay_us[DEV_ACCEL] >=
+                                               st->chip_config.lpa_delay_us) {
+                               for (lp = 0; lp < st->hal->lpa_tbl_n; lp++) {
+                                       if (st->delay_us[DEV_ACCEL] >=
+                                                         st->hal->lpa_tbl[lp])
+                                               break;
+                               }
+                               pm = NVI_PM_ON_CYCLE;
+                       } else {
+                               pm = NVI_PM_ON;
+                       }
+               } else if (st->master_enable & (1 << DEV_ACCEL)) {
+                       pm = NVI_PM_ON;
+               } else if ((st->master_enable & DEV_PM_STDBY) ||
+                                                        st->aux.bypass_lock) {
+                       pm = NVI_PM_STDBY;
+               } else {
+                       pm = NVI_PM_OFF;
+               }
+       } else {
+               pwr_mgmt_2 = st->rc.pwr_mgmt_2;
+               if ((pm_req > NVI_PM_STDBY) && (pm_req < st->pm))
+                       pm = st->pm;
+               else
+                       pm = pm_req;
+       }
+       if (pm == NVI_PM_OFF) {
+               for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+                       if (st->aux.port[i].nmp.shutdown_bypass) {
+                               nvi_aux_bypass_enable(st, true);
+                               pm = NVI_PM_STDBY;
+                               break;
+                       }
+               }
+               if (st->master_enable & EN_FW)
+                       pm = NVI_PM_STDBY;
+       }
+
+       switch (pm) {
+       case NVI_PM_OFF_FORCE:
+       case NVI_PM_OFF:
+               pm = NVI_PM_OFF;
+       case NVI_PM_STDBY:
+               pwr_mgmt_2 = (BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY);
+               pwr_mgmt_1 = BIT_SLEEP;
+               break;
+
+       case NVI_PM_ON_CYCLE:
+               pwr_mgmt_1 = BIT_CYCLE;
+               break;
+
+       case NVI_PM_ON:
+               pwr_mgmt_1 = INV_CLK_INTERNAL;
+               break;
+
+       case NVI_PM_ON_FULL:
+               pwr_mgmt_1 = INV_CLK_PLL;
+               /* anglvel must be turned on before going to PLL clock */
+               pwr_mgmt_2 &= ~BIT_PWR_GYRO_STBY;
+               break;
+
+       default:
+               dev_err(&st->i2c->dev, "%s %d=>%d ERR=EINVAL\n",
+                       __func__, st->pm, pm);
+               return -EINVAL;
+       }
+
+       if ((pm != st->pm) || (lp != st->rc.lp_accel_odr) ||
+                                          (pwr_mgmt_1 != st->rc.pwr_mgmt_1) ||
+                                           (pwr_mgmt_2 != (st->rc.pwr_mgmt_2 &
+                                 (BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY)))) {
+               nvi_int_able(st, false);
+               st->push_ts = 0;
+               if (pm == NVI_PM_OFF) {
+                       switch (st->pm) {
+                       case NVI_PM_STDBY:
+                       case NVI_PM_OFF_FORCE:
+                       case NVI_PM_OFF:
+                       case NVI_PM_ERR:
+                               break;
+
+                       default:
+                               /* disables aux before turning off */
+                               nvi_reset(st, true, false);
+                               break;
+                       }
+               }
+               if ((!(st->rc.pwr_mgmt_1 & (BIT_SLEEP | BIT_CYCLE))) &&
+                            (pm < NVI_PM_ON) && (st->pm > NVI_PM_ON_CYCLE)) {
+                       /* tasks that need access before low power state */
+                       if (pm_req == NVI_PM_AUTO)
+                               /* turn off FIFO and I2C */
+                               nvi_user_ctrl_en(st, false, false);
+               }
+               if (pm == NVI_PM_OFF) {
+                       if (st->pm > NVI_PM_OFF) {
+                               ret |= nvi_wr_pwr_mgmt_1_war(st);
+                               ret |= nvi_wr_pwr_mgmt_1(st, BIT_H_RESET);
+                       }
+                       ret |= nvi_pm_wr(st, pwr_mgmt_1, pwr_mgmt_2, lp);
+                       ret |= nvi_vreg_dis_all(st);
+               } else {
+                       ret |= nvi_pm_wr(st, pwr_mgmt_1, pwr_mgmt_2, lp);
+                       if (pm > NVI_PM_STDBY)
+                               mdelay(REG_UP_TIME);
+               }
+               if (ret < 0) {
+                       dev_err(&st->i2c->dev, "%s %d=>%d ERR=%d\n",
+                               __func__, st->pm, pm, ret);
+                       pm = NVI_PM_ERR;
+               }
+               if (st->dbg & NVI_DBG_SPEW_MSG)
+                       dev_info(&st->i2c->dev, "%s %d=>%d PM2=%x LPA=%x\n",
+                                __func__, st->pm, pm, pwr_mgmt_2, lp);
+               st->pm = pm;
+               if (ret > 0)
+                       ret = 0;
+       }
+       if (pm_req == NVI_PM_AUTO) {
+               if (pm > NVI_PM_STDBY)
+                       irq = true;
+               else
+                       irq = false;
+               if (pm > NVI_PM_ON_CYCLE)
+                       nvi_user_ctrl_en(st, true, true);
+               if ((pm == NVI_PM_ON_FULL) && (!st->push_ts))
+                       st->push_ts = iio_get_time_ns() +
+                                          st->chip_config.gyro_start_delay_ns;
+       } else {
+               /* interrupts are disabled until NVI_PM_AUTO */
+               irq = false;
+       }
+       nvi_int_able(st, irq);
+       return ret;
+}
+
+static void nvi_pm_exit(struct nvi_state *st)
+{
+       nvi_pm(st, NVI_PM_OFF_FORCE);
+       nvi_vreg_exit(st);
+}
+
+static int nvi_pm_init(struct nvi_state *st)
+{
+       int ret = 0;
+
+       nvi_vreg_init(st);
+       ret = nvi_pm_wr(st, 0, 0, 0);
+       st->pm = NVI_PM_ERR;
+       return ret;
+}
+
+static int nvi_aux_delay(struct nvi_state *st, int port, unsigned int delay_ms)
+{
+       struct aux_port *ap;
+       u8 val;
+       u8 i;
+       unsigned int delay_new;
+       int delay_rtn;
+
+       if (port != AUX_PORT_BYPASS)
+               st->aux.port[port].nmp.delay_ms = delay_ms;
+       /* determine valid delays by ports enabled */
+       delay_new = 0;
+       delay_rtn = 0;
+       for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+               ap = &st->aux.port[i];
+               if (delay_rtn < ap->nmp.delay_ms)
+                       delay_rtn = ap->nmp.delay_ms;
+               if (st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN) {
+                       if (delay_new < ap->nmp.delay_ms)
+                               delay_new = ap->nmp.delay_ms;
+               }
+       }
+       ap = &st->aux.port[AUX_PORT_SPECIAL];
+       if (delay_rtn < ap->nmp.delay_ms)
+               delay_rtn = ap->nmp.delay_ms;
+       if (st->rc.i2c_slv4_ctrl & BIT_SLV_EN) {
+               if (delay_new < ap->nmp.delay_ms)
+                       delay_new = ap->nmp.delay_ms;
+       }
+       if (!(st->rc.user_ctrl & BIT_I2C_MST_EN)) {
+               /* delay will execute when re-enabled */
+               if (delay_ms)
+                       return delay_rtn;
+               else
+                       return 0;
+       }
+
+       /* HW global delay */
+       delay_new *= 1000;
+       if (delay_new % st->poll_delay_us) {
+               delay_new /= st->poll_delay_us;
+       } else {
+               delay_new /= st->poll_delay_us;
+               if (delay_new)
+                       delay_new--;
+       }
+       st->aux.delay_hw = delay_new;
+       nvi_wr_i2c_slv4_ctrl(st, (bool)(st->rc.i2c_slv4_ctrl & BIT_SLV_EN));
+       /* HW port delay enable */
+       val = BIT_DELAY_ES_SHADOW;
+       for (i = 0; i < AUX_PORT_MAX; i++) {
+               ap = &st->aux.port[i];
+               if (ap->nmp.delay_ms)
+                       val |= (1 << i);
+       }
+       nvi_wr_i2c_mst_delay_ctrl(st, val);
+       if (delay_ms)
+               return delay_rtn;
+       else
+               return 0;
+}
+
+static int nvi_global_delay(struct nvi_state *st)
+{
+       unsigned long delay_us;
+       unsigned long delay_us_old;
+       unsigned long fs_hz;
+       u8 dlpf;
+       u8 smplrt_div;
+       int i;
+       int ret;
+       int ret_t = 0;
+
+       /* find the fastest polling of all the devices */
+       delay_us = -1;
+       for (i = 0; i < AUX_PORT_MAX; i++) {
+               if (st->aux.port[i].enable && st->aux.port[i].nmp.delay_us) {
+                       if (st->aux.port[i].nmp.delay_us < delay_us)
+                               delay_us = st->aux.port[i].nmp.delay_us;
+               }
+       }
+       for (i = 0; i < DEV_N; i++) {
+               if (st->enable[i] && st->delay_us[i]) {
+                       if (st->delay_us[i] < delay_us)
+                               delay_us = st->delay_us[i];
+               }
+       }
+       if (delay_us == -1)
+               delay_us = NVI_DELAY_DEFAULT; /* default if nothing found */
+       /* set the limits */
+       if (delay_us < st->hal->min_delay_us)
+               delay_us = st->hal->min_delay_us;
+       if (delay_us > NVI_DELAY_US_MAX)
+               delay_us = NVI_DELAY_US_MAX;
+       delay_us_old = st->poll_delay_us;
+       st->poll_delay_us = delay_us;
+       delay_us <<= 1;
+       for (dlpf = 0; dlpf < ARRAY_SIZE(nvi_lpf_us_tbl); dlpf++) {
+               if (delay_us < nvi_lpf_us_tbl[dlpf])
+                       break;
+       }
+       if (dlpf)
+               fs_hz = 1000;
+       else
+               fs_hz = 8000;
+       smplrt_div = st->poll_delay_us / fs_hz - 1;
+       dlpf |= (st->rc.config & 0xF8);
+       fs_hz = 1000000 / st->poll_delay_us;
+       if ((smplrt_div != st->rc.smplrt_div) || (dlpf != st->rc.config)) {
+               if (st->dbg)
+                       dev_info(&st->i2c->dev, "%s %lu\n",
+                                __func__, delay_us);
+               if (st->poll_delay_us < delay_us_old) {
+                       /* go faster */
+                       nvi_aux_delay(st, AUX_PORT_BYPASS, 0);
+                       ret = nvi_wr_config(st, dlpf);
+                       if (ret < 0)
+                               ret_t |= ret;
+                       ret_t |= nvi_wr_smplrt_div(st, smplrt_div);
+               } else {
+                       /* go slower */
+                       ret_t |= nvi_wr_smplrt_div(st, smplrt_div);
+                       ret = nvi_wr_config(st, dlpf);
+                       nvi_aux_delay(st, AUX_PORT_BYPASS, 0);
+                       if (ret < 0)
+                               ret_t |= ret;
+               }
+       } else {
+               nvi_aux_delay(st, AUX_PORT_BYPASS, 0);
+       }
+       return ret_t;
+}
+
+static void nvi_en(struct iio_dev *indio_dev)
+{
+       struct nvi_state *st = iio_priv(indio_dev);
+       unsigned int i;
+
+       for (i = 0; i < DEV_N; i++)
+               st->enable[i] = 0;
+       st->master_enable &= ~DEV_MPU_MASK;
+       /* note enable axis' are reversed for HW */
+       if (iio_scan_mask_query(indio_dev, indio_dev->buffer,
+                               NVI_SCAN_ACCEL_X)) {
+               st->master_enable |= (1 << DEV_ACCEL);
+               st->enable[DEV_ACCEL] |= (1 << AXIS_Z);
+       }
+       if (iio_scan_mask_query(indio_dev, indio_dev->buffer,
+                               NVI_SCAN_ACCEL_Y)) {
+               st->master_enable |= (1 << DEV_ACCEL);
+               st->enable[DEV_ACCEL] |= (1 << AXIS_Y);
+       }
+       if (iio_scan_mask_query(indio_dev, indio_dev->buffer,
+                               NVI_SCAN_ACCEL_Z)) {
+               st->master_enable |= (1 << DEV_ACCEL);
+               st->enable[DEV_ACCEL] |= (1 << AXIS_X);
+       }
+       /* note enable axis' are reversed for HW */
+       if (iio_scan_mask_query(indio_dev, indio_dev->buffer,
+                               NVI_SCAN_ANGLVEL_X)) {
+               st->master_enable |= ((1 << DEV_ANGLVEL) | (1 << DEV_TEMP));
+               st->enable[DEV_ANGLVEL] |= (1 << AXIS_Z);
+       }
+       if (iio_scan_mask_query(indio_dev, indio_dev->buffer,
+                               NVI_SCAN_ANGLVEL_Y)) {
+               st->master_enable |= ((1 << DEV_ANGLVEL) | (1 << DEV_TEMP));
+               st->enable[DEV_ANGLVEL] |= (1 << AXIS_Y);
+       }
+       if (iio_scan_mask_query(indio_dev, indio_dev->buffer,
+                               NVI_SCAN_ANGLVEL_Z)) {
+               st->master_enable |= ((1 << DEV_ANGLVEL) | (1 << DEV_TEMP));
+               st->enable[DEV_ANGLVEL] |= (1 << AXIS_X);
+       }
+       if (iio_scan_mask_query(indio_dev, indio_dev->buffer,
+                               NVI_SCAN_TEMP)) {
+               st->master_enable |= (1 << DEV_TEMP);
+               st->enable[DEV_TEMP] |= DEV_TEMP_EN;
+       }
+}
+
+int nvi_enable(struct iio_dev *indio_dev)
+{
+       struct nvi_state *st = iio_priv(indio_dev);
+       int i;
+       int ret;
+       int ret_t;
+
+       nvi_en(indio_dev);
+       if (st->master_enable & (1 << DEV_ANGLVEL))
+               ret_t = nvi_pm(st, NVI_PM_ON_FULL);
+       else if (st->master_enable & (DEV_MPU_MASK | (1 << DEV_AUX)))
+               ret_t = nvi_pm(st, NVI_PM_ON);
+       else
+               return nvi_pm(st, NVI_PM_AUTO);
+
+       if (st->master_enable & (1 << DEV_ANGLVEL)) {
+               for (i = 0; i < AXIS_N; i++)
+                       ret_t |= nvi_wr_gyro_offset(st, i,
+                                                (u16)(st->rom_gyro_offset[i] +
+                                                   st->input_gyro_offset[i]));
+               ret = nvi_wr_gyro_config(st, 0, st->chip_config.fsr);
+               if (ret < 0)
+                       ret_t |= ret;
+       }
+       if (st->master_enable & (1 << DEV_ACCEL)) {
+               for (i = 0; i < AXIS_N; i++)
+                       ret_t |= nvi_wr_accel_offset(st, i,
+                                               (u16)(st->rom_accel_offset[i] +
+                                           (st->input_accel_offset[i] << 1)));
+               ret = nvi_wr_accel_config(st, 0, st->chip_config.accel_fs, 0);
+               if (ret < 0)
+                       ret_t |= ret;
+       }
+       nvi_global_delay(st);
+       ret_t |= nvi_reset(st, true, false);
+       ret_t |= nvi_pm(st, NVI_PM_AUTO);
+       return ret_t;
+}
+
+static int nvi_batch(struct nvi_state *st)
+{
+       int ret = 0;
+
+       if (st->hal->dmp)
+               ret = 1;
+       return ret;
+}
+
+static void nvi_aux_dbg(struct nvi_state *st, char *tag, int val)
+{
+       struct nvi_mpu_port *n;
+       struct aux_port *p;
+       struct aux_ports *a;
+       u8 data[4];
+       int i;
+
+       if (!(st->dbg & NVI_DBG_SPEW_AUX))
+               return;
+
+       dev_info(&st->i2c->dev, "%s %s %d\n", __func__, tag, val);
+       for (i = 0; i < AUX_PORT_MAX; i++) {
+               nvi_i2c_rd(st, st->hal->reg->i2c_slv0_addr.bank,
+                          st->hal->reg->i2c_slv0_addr.reg + (i * 3), 3, data);
+               nvi_i2c_rd(st, st->hal->reg->i2c_slv0_do.bank,
+                          st->hal->reg->i2c_slv0_do.reg + i, 1, &data[3]);
+               /* HW = hardware */
+               pr_info("HW: P%d AD=%x RG=%x CL=%x DO=%x\n",
+                       i, data[0], data[1], data[2], data[3]);
+               n = &st->aux.port[i].nmp;
+               /* NS = nmp structure */
+               pr_info("NS: P%d AD=%x RG=%x CL=%x DO=%x MS=%u US=%lu SB=%x\n",
+                       i, n->addr, n->reg, n->ctrl, n->data_out, n->delay_ms,
+                       n->delay_us, n->shutdown_bypass);
+               p = &st->aux.port[i];
+               /* PS = port structure */
+               pr_info("PS: P%d OFFSET=%u EN=%x FIFOEN=%x HWDOUT=%x\n", i,
+                       p->ext_data_offset, p->enable, p->fifo_en, p->hw_do);
+       }
+       a = &st->aux;
+       pr_info("AUX: EN=%x MEN=%x MDLY=%x GDLY=%u DATN=%u BPEN=%x BPLK=%d\n",
+               (bool)(st->master_enable & (1 << DEV_AUX)),
+               (bool)(st->rc.user_ctrl & BIT_I2C_MST_EN),
+               (st->rc.i2c_slv4_ctrl & BITS_I2C_MST_DLY),
+               st->poll_delay_us, a->ext_data_n,
+               (st->rc.int_pin_cfg & BIT_BYPASS_EN), a->bypass_lock);
+}
+
+static void nvi_aux_read(struct nvi_state *st)
+{
+       struct aux_port *ap;
+       s64 ts;
+       unsigned int i;
+       unsigned int len;
+       u8 *p;
+       int ret;
+
+       if ((!st->aux.ext_data_n) || (!(st->rc.user_ctrl & BIT_I2C_MST_EN)))
+               return;
+
+       ret = nvi_i2c_rd(st, st->hal->reg->ext_sens_data_00.bank,
+                        st->hal->reg->ext_sens_data_00.reg,
+                        st->aux.ext_data_n, (u8 *)&st->aux.ext_data);
+       if (ret)
+               return;
+
+       if (st->flush)
+               ts = 0;
+       else
+               ts = iio_get_time_ns();
+       for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+               ap = &st->aux.port[i];
+               if ((st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN) && (!ap->fifo_en) &&
+                                              (ap->nmp.addr & BIT_I2C_READ) &&
+                                                  (ap->nmp.handler != NULL)) {
+                       p = &st->aux.ext_data[ap->ext_data_offset];
+                       len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
+                       ap->nmp.handler(p, len, ts, ap->nmp.ext_driver);
+               }
+       }
+}
+
+static void nvi_aux_ext_data_offset(struct nvi_state *st)
+{
+       int i;
+       unsigned short offset;
+
+       offset = 0;
+       for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+               if ((st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN) &&
+                                 (st->aux.port[i].nmp.addr & BIT_I2C_READ)) {
+                       st->aux.port[i].ext_data_offset = offset;
+                       offset += (st->aux.port[i].nmp.ctrl &
+                                  BITS_I2C_SLV_CTRL_LEN);
+               }
+       }
+       if (offset > AUX_EXT_DATA_REG_MAX) {
+               offset = AUX_EXT_DATA_REG_MAX;
+               dev_err(&st->i2c->dev,
+                       "%s ERR MPU slaves exceed data storage\n", __func__);
+       }
+       st->aux.ext_data_n = offset;
+       return;
+}
+
+static int nvi_aux_port_data_out(struct nvi_state *st,
+                                int port, u8 data_out)
+{
+       int ret;
+
+       ret = nvi_wr_i2c_slv_do(st, port, data_out);
+       if (!ret) {
+               st->aux.port[port].nmp.data_out = data_out;
+               st->aux.port[port].hw_do = true;
+       } else {
+               st->aux.port[port].hw_do = false;
+       }
+       return ret;
+}
+
+static int nvi_aux_port_wr(struct nvi_state *st, int port)
+{
+       struct aux_port *ap;
+       int ret;
+
+       ap = &st->aux.port[port];
+       ret = nvi_wr_i2c_slv_addr(st, port, ap->nmp.addr);
+       ret |= nvi_wr_i2c_slv_reg(st, port, ap->nmp.reg);
+       ret |= nvi_wr_i2c_slv_do(st, port, ap->nmp.data_out);
+       return ret;
+}
+
+static int nvi_aux_port_en(struct nvi_state *st,
+                          int port, bool en)
+{
+       struct aux_port *ap;
+       u8 val;
+       int ret = 0;
+
+       st->aux.ext_data_n = 0;
+       ap = &st->aux.port[port];
+       if ((!(st->rc.i2c_slv_addr[port])) && en) {
+               ret = nvi_aux_port_wr(st, port);
+               if (!ret)
+                       ap->hw_do = true;
+       }
+       if ((!ap->hw_do) && en)
+               nvi_aux_port_data_out(st, port, ap->nmp.data_out);
+       if (port == AUX_PORT_SPECIAL) {
+               ret = nvi_wr_i2c_slv4_ctrl(st, en);
+       } else {
+               if (en)
+                       val = (ap->nmp.ctrl | BIT_SLV_EN);
+               else
+                       val = 0;
+               ret = nvi_wr_i2c_slv_ctrl(st, port, val);
+       }
+       if (ret > 0) {
+               nvi_aux_ext_data_offset(st);
+               ret = 0;
+       }
+       return ret;
+}
+
+static int nvi_aux_enable(struct nvi_state *st, bool enable)
+{
+       bool en;
+       unsigned int i;
+       int ret = 0;
+
+       if (st->rc.int_pin_cfg & BIT_BYPASS_EN)
+               enable = false;
+       en = false;
+       if (enable) {
+               /* global enable is honored only if a port is enabled */
+               for (i = 0; i < AUX_PORT_MAX; i++) {
+                       if (st->aux.port[i].enable) {
+                               en = true;
+                               break;
+                       }
+               }
+               if (en == (bool)(st->rc.user_ctrl & BIT_I2C_MST_EN))
+                       /* if already on then just update delays */
+                       nvi_global_delay(st);
+       }
+       if (en)
+               st->master_enable |= (1 << DEV_AUX);
+       else
+               st->master_enable &= (~(1 << DEV_AUX));
+       if ((bool)(st->rc.user_ctrl & BIT_I2C_MST_EN) == en) {
+               if (st->aux.reset_fifo)
+                       nvi_reset(st, true, false);
+               return 0;
+       }
+
+       if (en) {
+               for (i = 0; i < AUX_PORT_MAX; i++) {
+                       if (st->aux.port[i].enable)
+                               ret |= nvi_aux_port_en(st, i, true);
+               }
+       } else {
+               for (i = 0; i < AUX_PORT_MAX; i++) {
+                       if (st->rc.i2c_slv_addr[i])
+                               nvi_aux_port_en(st, i, false);
+               }
+       }
+       ret |= nvi_global_delay(st);
+       if (st->aux.reset_fifo)
+               ret |= nvi_reset(st, true, false);
+       else
+               ret |= nvi_user_ctrl_en(st, true, en);
+       return ret;
+}
+
+static int nvi_aux_port_enable(struct nvi_state *st,
+                              int port, bool enable, bool fifo_enable)
+{
+       struct aux_port *ap;
+       int ret;
+
+       ap = &st->aux.port[port];
+       ap->enable = enable;
+       if ((!enable) || (!(ap->nmp.addr & BIT_I2C_READ)))
+               fifo_enable = false;
+       if (ap->fifo_en != fifo_enable)
+               st->aux.reset_fifo = true;
+       ap->fifo_en = fifo_enable;
+       if (enable && (st->rc.int_pin_cfg & BIT_BYPASS_EN))
+               return 0;
+
+       ret = nvi_aux_port_en(st, port, enable);
+       ret |= nvi_aux_enable(st, true);
+       return ret;
+}
+
+static int nvi_reset(struct nvi_state *st,
+                    bool reset_fifo, bool reset_i2c)
+{
+       u8 irq;
+       u8 val;
+       unsigned long flags;
+       int ret;
+
+       if (st->dbg & NVI_DBG_SPEW_MSG)
+               dev_info(&st->i2c->dev, "%s FIFO=%x I2C=%x\n",
+                        __func__, reset_fifo, reset_i2c);
+       irq = st->rc.int_enable;
+       ret = nvi_int_able(st, false);
+       val = 0;
+       if (reset_i2c) {
+               st->aux.reset_i2c = false;
+               ret |= nvi_aux_enable(st, false);
+               val |= BIT_I2C_MST_RST;
+       }
+       if (reset_fifo) {
+               val |= BIT_FIFO_RST;
+               if (st->master_enable & (1 << DEV_DMP))
+                       val |= BIT_DMP_RST;
+       }
+       ret |= nvi_user_ctrl_en(st, !reset_fifo, !reset_i2c);
+       val |= st->rc.user_ctrl;
+       ret |= nvi_wr_user_ctrl(st, val);
+       if (reset_i2c)
+               ret |= nvi_aux_enable(st, true);
+       else
+               ret |= nvi_user_ctrl_en(st, true, true);
+       if (reset_fifo && (st->rc.user_ctrl & BIT_FIFO_EN)) {
+               spin_lock_irqsave(&st->time_stamp_lock, flags);
+               kfifo_reset(&st->timestamps);
+               spin_unlock_irqrestore(&st->time_stamp_lock, flags);
+               st->fifo_ts = iio_get_time_ns();
+       }
+       if (irq)
+               ret |= nvi_int_able(st, true);
+       return ret;
+}
+
+static int nvi_aux_port_free(struct nvi_state *st, int port)
+{
+       memset(&st->aux.port[port], 0, sizeof(struct aux_port));
+       if (st->rc.i2c_slv_addr[port]) {
+               nvi_aux_port_wr(st, port);
+               nvi_aux_port_en(st, port, false);
+               nvi_aux_enable(st, false);
+               nvi_aux_enable(st, true);
+               if (port != AUX_PORT_SPECIAL)
+                       st->aux.reset_i2c = true;
+       }
+       return 0;
+}
+
+static int nvi_aux_port_alloc(struct nvi_state *st,
+                             struct nvi_mpu_port *nmp, int port)
+{
+       int i;
+
+       if (st->aux.reset_i2c)
+               nvi_reset(st, false, true);
+       if (port < 0) {
+               for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+                       if (st->aux.port[i].nmp.addr == 0)
+                               break;
+               }
+               if (i == AUX_PORT_SPECIAL)
+                       return -ENODEV;
+       } else {
+               if (st->aux.port[port].nmp.addr == 0)
+                       i = port;
+               else
+                       return -ENODEV;
+       }
+
+       memset(&st->aux.port[i], 0, sizeof(struct aux_port));
+       memcpy(&st->aux.port[i].nmp, nmp, sizeof(struct nvi_mpu_port));
+       return i;
+}
+
+static int nvi_aux_bypass_enable(struct nvi_state *st, bool enable)
+{
+       u8 val;
+       int ret;
+
+       if ((bool)(st->rc.int_pin_cfg & BIT_BYPASS_EN) == enable)
+               return 0;
+
+       val = st->rc.int_pin_cfg;
+       if (enable) {
+               ret = nvi_aux_enable(st, false);
+               if (!ret) {
+                       val |= BIT_BYPASS_EN;
+                       ret = nvi_wr_int_pin_cfg(st, val);
+               }
+       } else {
+               val &= ~BIT_BYPASS_EN;
+               ret = nvi_wr_int_pin_cfg(st, val);
+               if (!ret)
+                       nvi_aux_enable(st, true);
+       }
+       return ret;
+}
+
+static int nvi_aux_bypass_request(struct nvi_state *st, bool enable)
+{
+       s64 ns;
+       s64 to;
+       int ret = 0;
+
+       if ((bool)(st->rc.int_pin_cfg & BIT_BYPASS_EN) == enable) {
+               st->aux.bypass_timeout_ns = iio_get_time_ns();
+               st->aux.bypass_lock++;
+               if (!st->aux.bypass_lock)
+                       dev_err(&st->i2c->dev, "%s rollover ERR\n", __func__);
+       } else {
+               if (st->aux.bypass_lock) {
+                       ns = iio_get_time_ns() - st->aux.bypass_timeout_ns;
+                       to = st->chip_config.bypass_timeout_ms * 1000000;
+                       if (ns > to)
+                               st->aux.bypass_lock = 0;
+                       else
+                               ret = -EBUSY;
+               }
+               if (!st->aux.bypass_lock) {
+                       ret = nvi_aux_bypass_enable(st, enable);
+                       if (ret)
+                               dev_err(&st->i2c->dev, "%s ERR=%d\n",
+                                       __func__, ret);
+                       else
+                               st->aux.bypass_lock++;
+               }
+       }
+       return ret;
+}
+
+static int nvi_aux_bypass_release(struct nvi_state *st)
+{
+       int ret = 0;
+
+       if (st->aux.bypass_lock)
+               st->aux.bypass_lock--;
+       if (!st->aux.bypass_lock) {
+               ret = nvi_aux_bypass_enable(st, false);
+               if (ret)
+                       dev_err(&st->i2c->dev, "%s ERR=%d\n", __func__, ret);
+       }
+       return ret;
+}
+
+static int nvi_aux_dev_valid(struct nvi_state *st,
+                            struct nvi_mpu_port *nmp, u8 *data)
+{
+       u8 val;
+       int i;
+       int ret;
+
+       /* turn off bypass */
+       ret = nvi_aux_bypass_request(st, false);
+       if (ret)
+               return -EBUSY;
+
+       /* grab the special port */
+       ret = nvi_aux_port_alloc(st, nmp, AUX_PORT_SPECIAL);
+       if (ret != AUX_PORT_SPECIAL) {
+               nvi_aux_bypass_release(st);
+               return -EBUSY;
+       }
+
+       /* enable it */
+       st->aux.port[AUX_PORT_SPECIAL].nmp.delay_ms = 0;
+       st->aux.port[AUX_PORT_SPECIAL].nmp.delay_us = st->hal->min_delay_us;
+       ret = nvi_aux_port_enable(st, AUX_PORT_SPECIAL, true, false);
+       if (ret) {
+               nvi_aux_port_free(st, AUX_PORT_SPECIAL);
+               nvi_aux_bypass_release(st);
+               return -EBUSY;
+       }
+
+       /* now turn off all the other ports for fastest response */
+       for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+               if (st->rc.i2c_slv_addr[i])
+                       nvi_aux_port_en(st, i, false);
+       }
+       /* start reading the results */
+       for (i = 0; i < AUX_DEV_VALID_READ_LOOP_MAX; i++) {
+               mdelay(AUX_DEV_VALID_READ_DELAY_MS);
+               val = 0;
+               ret = nvi_i2c_rd(st, st->hal->reg->i2c_mst_status.bank,
+                                st->hal->reg->i2c_mst_status.reg, 1, &val);
+               if (ret)
+                       continue;
+
+               if (val & 0x50)
+                       break;
+       }
+       /* these will restore all previously disabled ports */
+       nvi_aux_bypass_release(st);
+       nvi_aux_port_free(st, AUX_PORT_SPECIAL);
+       if (i == AUX_DEV_VALID_READ_LOOP_MAX)
+               return -ENODEV;
+
+       if (val & 0x10) /* NACK */
+               return -EIO;
+
+       if (nmp->addr & BIT_I2C_READ) {
+               ret = nvi_i2c_rd(st, st->hal->reg->i2c_slv4_di.bank,
+                                st->hal->reg->i2c_slv4_di.reg, 1, &val);
+               if (ret)
+                       return -EBUSY;
+
+               *data = (u8)val;
+               dev_info(&st->i2c->dev, "%s MPU read 0x%x from device 0x%x\n",
+                       __func__, val, (nmp->addr & ~BIT_I2C_READ));
+       } else {
+               dev_info(&st->i2c->dev, "%s MPU found device 0x%x\n",
+                       __func__, (nmp->addr & ~BIT_I2C_READ));
+       }
+       return 0;
+}
+
+static int nvi_aux_mpu_call_pre(struct nvi_state *st, int port)
+{
+       if ((port < 0) || (port >= AUX_PORT_SPECIAL))
+               return -EINVAL;
+
+       if (st->shutdown || st->suspend)
+               return -EPERM;
+
+       if (!st->aux.port[port].nmp.addr)
+               return -EINVAL;
+
+       return 0;
+}
+
+static int nvi_aux_mpu_call_post(struct nvi_state *st,
+                                char *tag, int ret)
+{
+       if (ret < 0)
+               ret = -EBUSY;
+       nvi_aux_dbg(st, tag, ret);
+       return ret;
+}
+
+/* See the mpu.h file for details on the nvi_mpu_ calls.
+ */
+int nvi_mpu_dev_valid(struct nvi_mpu_port *nmp, u8 *data)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret = -EPERM;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s\n", __func__);
+       } else {
+               pr_debug("%s ERR -EAGAIN\n", __func__);
+               return -EAGAIN;
+       }
+
+       if (nmp == NULL)
+               return -EINVAL;
+
+       if ((nmp->addr & BIT_I2C_READ) && (data == NULL))
+               return -EINVAL;
+
+       mutex_lock(&indio_dev->mlock);
+       if (!st->shutdown && !st->suspend) {
+               nvi_pm(st, NVI_PM_ON);
+               ret = nvi_aux_dev_valid(st, nmp, data);
+               nvi_pm(st, NVI_PM_AUTO);
+               nvi_aux_dbg(st, "nvi_mpu_dev_valid ret=", ret);
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_dev_valid);
+
+int nvi_mpu_port_alloc(struct nvi_mpu_port *nmp)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret = -EPERM;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s\n", __func__);
+       } else {
+               pr_debug("%s ERR -EAGAIN\n", __func__);
+               return -EAGAIN;
+       }
+
+       if (nmp == NULL)
+               return -EINVAL;
+
+       if (!(nmp->ctrl & BITS_I2C_SLV_CTRL_LEN))
+               return -EINVAL;
+
+       mutex_lock(&indio_dev->mlock);
+       if (!st->shutdown && !st->suspend) {
+               nvi_pm(st, NVI_PM_ON);
+               ret = nvi_aux_port_alloc(st, nmp, -1);
+               nvi_pm(st, NVI_PM_AUTO);
+               ret = nvi_aux_mpu_call_post(st,
+                                        "nvi_mpu_port_alloc ret/port: ", ret);
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_port_alloc);
+
+int nvi_mpu_port_free(int port)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s port %d\n", __func__, port);
+       } else {
+               pr_debug("%s port %d ERR -EAGAIN\n", __func__, port);
+               return -EAGAIN;
+       }
+
+       mutex_lock(&indio_dev->mlock);
+       ret = nvi_aux_mpu_call_pre(st, port);
+       if (!ret) {
+               nvi_pm(st, NVI_PM_ON);
+               ret = nvi_aux_port_free(st, port);
+               nvi_pm(st, NVI_PM_AUTO);
+               ret = nvi_aux_mpu_call_post(st, "nvi_mpu_port_free ret: ", ret);
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_port_free);
+
+int nvi_mpu_enable(int port, bool enable, bool fifo_enable)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s port %d: %x\n", __func__, port, enable);
+       } else {
+               pr_debug("%s port %d: %x ERR -EAGAIN\n",
+                        __func__, port, enable);
+               return -EAGAIN;
+       }
+
+       mutex_lock(&indio_dev->mlock);
+       ret = nvi_aux_mpu_call_pre(st, port);
+       if (!ret) {
+               nvi_pm(st, NVI_PM_ON);
+               ret = nvi_aux_port_enable(st, port, enable, fifo_enable);
+               nvi_pm(st, NVI_PM_AUTO);
+               ret = nvi_aux_mpu_call_post(st, "nvi_mpu_enable ret: ", ret);
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_enable);
+
+int nvi_mpu_delay_ms(int port, u8 delay_ms)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s port %d: %u\n", __func__, port, delay_ms);
+       } else {
+               pr_debug("%s port %d: %u ERR -EAGAIN\n",
+                        __func__, port, delay_ms);
+               return -EAGAIN;
+       }
+
+       mutex_lock(&indio_dev->mlock);
+       ret = nvi_aux_mpu_call_pre(st, port);
+       if (!ret) {
+               if (st->rc.i2c_slv_ctrl[port] & BIT_SLV_EN) {
+                       ret = nvi_aux_delay(st, port, delay_ms);
+                       nvi_global_delay(st);
+               } else {
+                       st->aux.port[port].nmp.delay_ms = delay_ms;
+               }
+               ret = nvi_aux_mpu_call_post(st, "nvi_mpu_delay_ms ret: ", ret);
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_delay_ms);
+
+int nvi_mpu_delay_us(int port, unsigned long delay_us)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s port %d: %lu\n", __func__, port, delay_us);
+       } else {
+               pr_debug("%s port %d: %lu ERR -EAGAIN\n",
+                       __func__, port, delay_us);
+               return -EAGAIN;
+       }
+
+       mutex_lock(&indio_dev->mlock);
+       ret = nvi_aux_mpu_call_pre(st, port);
+       if (!ret) {
+               st->aux.port[port].nmp.delay_us = delay_us;
+               if (st->rc.i2c_slv_ctrl[port] & BIT_SLV_EN)
+                       ret = nvi_global_delay(st);
+               ret = nvi_aux_mpu_call_post(st, "nvi_mpu_delay_us ret: ", ret);
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_delay_us);
+
+int nvi_mpu_data_out(int port, u8 data_out)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret;
+
+       if (indio_dev == NULL)
+               return -EAGAIN;
+
+       st = iio_priv(indio_dev);
+       ret = nvi_aux_mpu_call_pre(st, port);
+       if (!ret) {
+               if (st->rc.i2c_slv_ctrl[port] & BIT_SLV_EN) {
+                       ret = nvi_aux_port_data_out(st, port, data_out);
+               } else {
+                       st->aux.port[port].nmp.data_out = data_out;
+                       st->aux.port[port].hw_do = false;
+               }
+               if (ret < 0)
+                       ret = -EBUSY;
+       }
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_data_out);
+
+int nvi_mpu_batch(int port, unsigned int flags,
+                 unsigned int period_us, unsigned int timeout_ms)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s port %d: f=%x p=%u t=%u\n",
+                               __func__, port, flags, period_us, timeout_ms);
+       } else {
+               pr_debug("%s port %d: f=%x p=%u t=%u ERR -EAGAIN\n",
+                       __func__, port, flags, period_us, timeout_ms);
+               return -EAGAIN;
+       }
+
+       mutex_lock(&indio_dev->mlock);
+       ret = nvi_aux_mpu_call_pre(st, port);
+       if (!ret) {
+               if ((st->aux.port[port].nmp.id != ID_INVALID) &&
+                               (st->aux.port[port].nmp.id < ID_INVALID_END)) {
+                       st->aux.port[port].batch_flags = flags;
+                       st->aux.port[port].batch_period_us = period_us;
+                       st->aux.port[port].batch_timeout_ms = timeout_ms;
+                       ret = nvi_batch(st);
+                       ret = nvi_aux_mpu_call_post(st,
+                                             "nvi_mpu_batch ret/flags=", ret);
+               } else {
+                       ret = -EINVAL;
+               }
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_batch);
+
+int nvi_mpu_flush(int port)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s port %d\n", __func__, port);
+       } else {
+               pr_debug("%s port %d ERR -EAGAIN\n", __func__, port);
+               return -EAGAIN;
+       }
+
+       mutex_lock(&indio_dev->mlock);
+       ret = nvi_aux_mpu_call_pre(st, port);
+       if (!ret) {
+               if ((st->aux.port[port].nmp.id != ID_INVALID) &&
+                               (st->aux.port[port].nmp.id < ID_INVALID_END)) {
+                       ret = nvi_enable(indio_dev);
+                       ret = nvi_aux_mpu_call_post(st, "nvi_mpu_flush ret=",
+                                                   ret);
+               } else {
+                       ret = -EINVAL;
+               }
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_flush);
+
+int nvi_mpu_fifo(int port, unsigned int *reserve, unsigned int *max)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s port %d\n", __func__, port);
+       } else {
+               pr_debug("%s port %d ERR -EAGAIN\n", __func__, port);
+               return -EAGAIN;
+       }
+
+       mutex_lock(&indio_dev->mlock);
+       ret = nvi_aux_mpu_call_pre(st, port);
+       if (!ret) {
+               if ((st->aux.port[port].nmp.id != ID_INVALID) &&
+                       (st->aux.port[port].nmp.id < ID_INVALID_END)) {
+                       if (reserve) {
+                               if (st->hal->dmp)
+                                       *reserve =
+                                                 st->chip_config.fifo_reserve;
+                               else
+                                       *reserve = 0;
+                       }
+                       if (max) {
+                               if (st->hal->dmp)
+                                       *max = st->chip_config.fifo_max;
+                               else
+                                       *max = 0;
+                       }
+                       ret = nvi_aux_mpu_call_post(st, "nvi_mpu_fifo ret=",
+                                                   ret);
+               } else {
+                       ret = -EINVAL;
+               }
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_fifo);
+
+int nvi_mpu_bypass_request(bool enable)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+       int ret = -EPERM;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s enable=%x\n", __func__, enable);
+       } else {
+               pr_debug("%s ERR -EAGAIN\n", __func__);
+               return -EAGAIN;
+       }
+
+       mutex_lock(&indio_dev->mlock);
+       if (!st->shutdown && !st->suspend) {
+               nvi_pm(st, NVI_PM_ON);
+               ret = nvi_aux_bypass_request(st, enable);
+               nvi_pm(st, NVI_PM_AUTO);
+               ret = nvi_aux_mpu_call_post(st, "nvi_mpu_bypass_request ret=",
+                                           ret);
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return ret;
+}
+EXPORT_SYMBOL(nvi_mpu_bypass_request);
+
+int nvi_mpu_bypass_release(void)
+{
+       struct iio_dev *indio_dev = indio_dev_local;
+       struct nvi_state *st;
+
+       if (indio_dev != NULL) {
+               st = iio_priv(indio_dev);
+               if (st->dbg & NVI_DBG_SPEW_AUX)
+                       pr_info("%s\n", __func__);
+       } else {
+               pr_debug("%s\n", __func__);
+               return 0;
+       }
+
+       mutex_lock(&indio_dev->mlock);
+       if (!st->shutdown && !st->suspend) {
+               nvi_pm(st, NVI_PM_ON);
+               nvi_aux_bypass_release(st);
+               nvi_pm(st, NVI_PM_AUTO);
+               nvi_aux_mpu_call_post(st, "nvi_mpu_bypass_release", 0);
+       }
+       mutex_unlock(&indio_dev->mlock);
+       return 0;
+}
+EXPORT_SYMBOL(nvi_mpu_bypass_release);
+
+
+static unsigned int nvi_buf_index(unsigned int size, unsigned int *bytes)
+{
+       unsigned int index;
+
+       if (!(*bytes % size))
+               index = *bytes;
+       else
+               index = *bytes - *bytes % size + size;
+       *bytes = index + size;
+       return index;
+}
+
+static void nvi_buf_push(struct iio_dev *indio_dev, s64 ts)
+{
+       struct nvi_state *st = iio_priv(indio_dev);
+       unsigned char buf[24];
+       unsigned int n;
+       unsigned int i;
+       unsigned int bytes = 0;
+       unsigned int axis;
+
+       for (axis = 0; axis < AXIS_N; axis++) {
+               if (iio_scan_mask_query(indio_dev, indio_dev->buffer,
+                                       axis + NVI_SCAN_ACCEL_X)) {
+                       n = sizeof(st->accel[axis]);
+                       i = nvi_buf_index(n, &bytes);
+                       memcpy(&buf[i], &st->accel[axis], n);
+               }
+       }
+       for (axis = 0; axis < AXIS_N; axis++) {
+               if (iio_scan_mask_query(indio_dev, indio_dev->buffer,
+                                       axis + NVI_SCAN_ANGLVEL_X)) {
+                       n = sizeof(st->anglvel[axis]);
+                       i = nvi_buf_index(n, &bytes);
+                       memcpy(&buf[i], &st->anglvel[axis], n);
+               }
+       }
+       if (iio_scan_mask_query(indio_dev, indio_dev->buffer, NVI_SCAN_TEMP)) {
+               n = sizeof(st->temp);
+               i = nvi_buf_index(n, &bytes);
+               memcpy(&buf[i], &st->temp, n);
+       }
+       st->ts = ts;
+       if (indio_dev->buffer->scan_timestamp) {
+               if (st->flush || (ts < st->push_ts))
+                       ts = 0;
+               n = sizeof(ts);
+               i = nvi_buf_index(n, &bytes);
+               memcpy(&buf[i], &ts, n);
+       }
+       if (iio_buffer_enabled(indio_dev)) {
+               if (st->dbg & NVI_DBG_SPEW_BUF) {
+                       for (i = 0; i < bytes; i++)
+                               dev_info(&st->i2c->dev, "buf[%d]=%x\n",
+                                        i, buf[i]);
+               }
+               iio_push_to_buffers(indio_dev, buf);
+       }
+}
+
+static u16 nvi_report_accl(struct nvi_state *st, u8 *data, s64 ts)
+{
+       s16 accel[AXIS_N];
+       s16 accel_uc[AXIS_N];
+       u16 buf_i;
+       unsigned int i;
+
+       accel_uc[AXIS_X] = be16_to_cpup((__be16 *)&data[0]);
+       accel_uc[AXIS_Y] = be16_to_cpup((__be16 *)&data[2]);
+       accel_uc[AXIS_Z] = be16_to_cpup((__be16 *)&data[4]);
+       accel[AXIS_X] = accel_uc[AXIS_X] * st->chip_info.multi;
+       accel[AXIS_Y] = accel_uc[AXIS_Y] * st->chip_info.multi;
+       accel[AXIS_Z] = accel_uc[AXIS_Z] * st->chip_info.multi;
+       buf_i = 0;
+       if (!(st->rc.pwr_mgmt_2 & BIT_STBY_XA)) {
+               buf_i += 2;
+       } else {
+               accel_uc[AXIS_X] = 0;
+               accel[AXIS_X] = 0;
+       }
+       if (!(st->rc.pwr_mgmt_2 & BIT_STBY_YA)) {
+               buf_i += 2;
+       } else {
+               accel_uc[AXIS_Y] = 0;
+               accel[AXIS_Y] = 0;
+       }
+       if (!(st->rc.pwr_mgmt_2 & BIT_STBY_ZA)) {
+               buf_i += 2;
+       } else {
+               accel_uc[AXIS_Z] = 0;
+               accel[AXIS_Z] = 0;
+       }
+       for (i = 0; i < AXIS_N; i++) {
+               st->accel_uc[i] = accel_uc[i];
+               st->accel[i] = accel[i];
+       }
+       if (st->dbg & NVI_DBG_SPEW_ACCEL_UC)
+               dev_info(&st->i2c->dev, "accel_uc %hd %hd %hd %lld\n",
+                        st->accel_uc[AXIS_X], st->accel_uc[AXIS_Y],
+                        st->accel_uc[AXIS_Z], ts);
+       if (st->dbg & NVI_DBG_SPEW_ACCEL)
+               dev_info(&st->i2c->dev, "accel %hd %hd %hd %lld\n",
+                        st->accel[AXIS_X], st->accel[AXIS_Y],
+                        st->accel[AXIS_Z], ts);
+       return buf_i;
+}
+
+static void nvi_report_temp(struct nvi_state *st, u8 *data, s64 ts)
+{
+       st->temp = be16_to_cpup((__be16 *)data);
+       if (st->dbg & NVI_DBG_SPEW_TEMP)
+               dev_info(&st->i2c->dev, "temp %d %lld\n", st->temp, ts);
+}
+
+static u16 nvi_report_gyro(struct nvi_state *st, u8 *data, u8 mask, s64 ts)
+{
+       s16 anglvel[AXIS_N];
+       u16 buf_i;
+       unsigned int i;
+
+       for (i = 0; i < AXIS_N; i++)
+               anglvel[i] = 0;
+       buf_i = 0;
+       if (mask & 4) {
+               if (!(st->rc.pwr_mgmt_2 & BIT_STBY_XG))
+                       anglvel[AXIS_X] = be16_to_cpup((__be16 *)&data[buf_i]);
+               buf_i += 2;
+       }
+       if (mask & 2) {
+               if (!(st->rc.pwr_mgmt_2 & BIT_STBY_YG))
+                       anglvel[AXIS_Y] = be16_to_cpup((__be16 *)&data[buf_i]);
+               buf_i += 2;
+       }
+       if (mask & 1) {
+               if (!(st->rc.pwr_mgmt_2 & BIT_STBY_ZG))
+                       anglvel[AXIS_Z] = be16_to_cpup((__be16 *)&data[buf_i]);
+               buf_i += 2;
+       }
+       for (i = 0; i < AXIS_N; i++)
+               st->anglvel[i] = anglvel[i];
+       if (st->dbg & NVI_DBG_SPEW_ANGLVEL)
+               dev_info(&st->i2c->dev, "anglvel %hd %hd %hd %lld\n",
+                        anglvel[AXIS_X], anglvel[AXIS_Y],
+                        anglvel[AXIS_Z], ts);
+       return buf_i;
+}
+
+static int nvi_accl_read(struct nvi_state *st, s64 ts)
+{
+       u8 data[6];
+       int ret;
+
+       ret = nvi_i2c_rd(st, st->hal->reg->accel_xout_h.bank,
+                        st->hal->reg->accel_xout_h.reg, 6, data);
+       if (!ret)
+               ret = nvi_report_accl(st, data, ts);
+       return ret;
+}
+
+static int nvi_temp_read(struct nvi_state *st, s64 ts)
+{
+       u8 data[2];
+       int ret;
+
+       ret = nvi_i2c_rd(st, st->hal->reg->temp_out_h.bank,
+                        st->hal->reg->temp_out_h.reg, 2, data);
+       if (!ret)
+               nvi_report_temp(st, data, ts);
+       return ret;
+}
+
+static u16 nvi_fifo_read_accl(struct nvi_state *st,
+                             u16 buf_index, s64 ts)
+{
+       if (st->rc.fifo_en & BIT_ACCEL_OUT) {
+               nvi_report_accl(st, &st->buf[buf_index], ts);
+               buf_index += 6;
+       }
+       return buf_index;
+}
+
+static u16 nvi_fifo_read_gyro(struct nvi_state *st,
+                             u16 buf_index, s64 ts)
+{
+       u8 mask;
+
+       if (st->rc.fifo_en & BIT_TEMP_OUT) {
+               nvi_report_temp(st, &st->buf[buf_index], ts);
+               buf_index += 2;
+       }
+       mask = st->rc.fifo_en;
+       mask &= (BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
+       mask >>= 4;
+       if (mask)
+               buf_index += nvi_report_gyro(st, &st->buf[buf_index],
+                                            mask, ts);
+       return buf_index;
+}
+
+static irqreturn_t nvi_irq_thread(int irq, void *dev_id)
+{
+       struct nvi_state *st = (struct nvi_state *)dev_id;
+       struct iio_dev *indio_dev = iio_priv_to_dev(st);
+       struct aux_port *ap;
+       u8 mask;
+       u16 fifo_count = 0;
+       u16 fifo_sample_size;
+       u16 fifo_rd_n;
+       u16 fifo_align;
+       u16 buf_index;
+       s64 ts;
+       s64 ts_irq;
+       s64 delay;
+       bool push;
+       unsigned int ts_len;
+       unsigned int samples;
+       unsigned int len;
+       int i;
+       int ret;
+
+       mutex_lock(&st->srlock);
+       mutex_lock(&indio_dev->mlock);
+       /* if only accelermeter data */
+       if (st->rc.pwr_mgmt_1 & BIT_CYCLE) {
+               ts = iio_get_time_ns();
+               ret = nvi_accl_read(st, ts);
+               if (ret < 0)
+                       goto nvi_irq_thread_exit;
+
+               nvi_buf_push(indio_dev, ts);
+               st->flush = false;
+               goto nvi_irq_thread_exit;
+       }
+
+       /* handle FIFO disabled data */
+       push = false;
+       ts = iio_get_time_ns();
+       if (((~st->rc.pwr_mgmt_2) & BIT_PWR_ACCEL_STBY) &&
+                              (!(st->rc.fifo_en & BIT_ACCEL_OUT))) {
+               ret = nvi_accl_read(st, ts);
+               if (ret > 0)
+                       push = true;
+       }
+       if (!(st->rc.fifo_en & BIT_TEMP_OUT))
+               ret = nvi_temp_read(st, ts);
+       mask = (BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
+       mask &= ~st->rc.fifo_en;
+       mask >>= 4;
+       if (st->enable[DEV_ANGLVEL] && mask) {
+               buf_index = 0;
+               ret = 0;
+               if (mask & 4) {
+                       ret = nvi_i2c_rd(st, st->hal->reg->gyro_xout_h.bank,
+                                        st->hal->reg->gyro_xout_h.reg,
+                                        2, &st->buf[buf_index]);
+                       buf_index = 2;
+               }
+               if (mask & 2) {
+                       ret = nvi_i2c_rd(st, st->hal->reg->gyro_xout_h.bank,
+                                        st->hal->reg->gyro_xout_h.reg + 2,
+                                        2, &st->buf[buf_index]);
+                       buf_index += 2;
+               }
+               if (mask & 1)
+                       ret = nvi_i2c_rd(st, st->hal->reg->gyro_xout_h.bank,
+                                        st->hal->reg->gyro_xout_h.reg + 4,
+                                        2, &st->buf[buf_index]);
+               if (!ret) {
+                       buf_index = nvi_report_gyro(st, st->buf, mask, ts);
+                       if (buf_index)
+                               push = true;
+               }
+       }
+       nvi_aux_read(st);
+       if (!(st->rc.user_ctrl & BIT_FIFO_EN)) {
+               if (push)
+                       /* push FIFO disabled data before exit */
+                       nvi_buf_push(indio_dev, ts);
+               st->flush = false;
+               goto nvi_irq_thread_exit;
+       }
+
+       /* handle FIFO enabled data */
+       fifo_sample_size = st->fifo_sample_size;
+       if (!fifo_sample_size)
+               goto nvi_irq_thread_exit;
+
+       /* must get IRQ timestamp len first for timestamp best-fit algorithm */
+       ts_len = kfifo_len(&st->timestamps);
+       ret = nvi_i2c_rd(st, st->hal->reg->fifo_count_h.bank,
+                        st->hal->reg->fifo_count_h.reg, 2, st->buf);
+       if (ret)
+               goto nvi_irq_thread_exit;
+
+       fifo_count = be16_to_cpup((__be16 *)(&st->buf));
+       /* FIFO threshold */
+       if (st->chip_config.fifo_thr > fifo_sample_size) {
+               if (fifo_count > st->chip_config.fifo_thr) {
+                       if (st->dbg & NVI_DBG_SPEW_FIFO)
+                               dev_info(&st->i2c->dev,
+                                        "FIFO threshold exceeded\n");
+                       goto nvi_irq_thread_exit_reset;
+               }
+       }
+
+       fifo_align = fifo_count % fifo_sample_size;
+       if (fifo_count < fifo_sample_size + fifo_align)
+               /* consider resetting FIFO if doen't divide cleanly */
+               goto nvi_irq_thread_exit;
+
+       ts = st->fifo_ts;
+       delay = st->poll_delay_us * 1000;
+       samples = (fifo_count / fifo_sample_size);
+       if (st->dbg & NVI_DBG_SPEW_FIFO)
+               dev_info(&st->i2c->dev,
+                        "fifo_count=%u sample_size=%u offset=%u samples=%u\n",
+                        fifo_count, fifo_sample_size, fifo_align, samples);
+       fifo_rd_n = 0;
+       buf_index = 0;
+       while (samples) {
+               if (buf_index >= fifo_rd_n) {
+                       fifo_rd_n = sizeof(st->buf);
+                       fifo_rd_n -= fifo_align;
+                       fifo_rd_n /= fifo_sample_size;
+                       if (samples < fifo_rd_n)
+                               fifo_rd_n = samples;
+                       fifo_rd_n *= fifo_sample_size;
+                       fifo_rd_n += fifo_align;
+                       ret = nvi_i2c_rd(st, st->hal->reg->fifo_r_w.bank,
+                                        st->hal->reg->fifo_r_w.reg,
+                                        fifo_rd_n, st->buf);
+                       if (ret)
+                               goto nvi_irq_thread_exit;
+
+                       buf_index = fifo_align;
+               }
+
+               if (ts_len) {
+                       len = ts_len;
+                       for (i = 0; i < len; i++) {
+                               ret = kfifo_out_peek(&st->timestamps,
+                                                    &ts_irq, 1);
+                               if (ret != 1)
+                                       goto nvi_irq_thread_exit_reset;
+
+                               if (ts < (ts_irq - delay))
+                                       break;
+
+                               ret = kfifo_out(&st->timestamps,
+                                               &ts_irq, 1);
+                               if (ret != 1)
+                                       goto nvi_irq_thread_exit_reset;
+
+                               ts_len--;
+                               if (ts < (ts_irq + delay)) {
+                                       ts = ts_irq;
+                                       break;
+                               }
+                       }
+                       if ((st->dbg & NVI_DBG_SPEW_FIFO) && (ts != ts_irq))
+                               dev_info(&st->i2c->dev,
+                                        "%s TS=%lld != IRQ=%lld s=%u i=%u\n",
+                                       __func__, ts, ts_irq, samples, ts_len);
+               } else {
+                       if (st->dbg & NVI_DBG_SPEW_FIFO)
+                               dev_info(&st->i2c->dev,
+                                        "%s NO IRQ_TS TS=%lld s=%u\n",
+                                        __func__, ts, samples);
+               }
+               buf_index = nvi_fifo_read_accl(st, buf_index, ts);
+               buf_index = nvi_fifo_read_gyro(st, buf_index, ts);
+               nvi_buf_push(indio_dev, ts);
+               if (st->flush)
+                       ts_irq = 0;
+               else
+                       ts_irq = ts;
+               for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+                       ap = &st->aux.port[i];
+                       if (ap->fifo_en &&
+                                     (st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN)) {
+                               len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
+                               if (ap->nmp.handler != NULL)
+                                       ap->nmp.handler(&st->buf[buf_index],
+                                                       len, ts_irq,
+                                                       ap->nmp.ext_driver);
+                               buf_index += len;
+                       }
+               }
+               st->flush = false;
+               ts += delay;
+               samples--;
+       }
+       if (ts_len) {
+               if (st->dbg & NVI_DBG_SPEW_FIFO)
+                       dev_info(&st->i2c->dev, "%s SYNC TO IRQ_TS %lld\n",
+                                __func__, ts);
+               for (i = 0; i < ts_len; i++) {
+                       ret = kfifo_out(&st->timestamps, &ts, 1);
+                       if (ret != 1)
+                               goto nvi_irq_thread_exit_reset;
+               }
+       }
+
+       st->fifo_ts = ts;
+nvi_irq_thread_exit:
+       if (st->irq_dis) {
+               enable_irq(st->i2c->irq);
+               st->irq_dis = false;
+       }
+       mutex_unlock(&indio_dev->mlock);
+       mutex_unlock(&st->srlock);
+       return IRQ_HANDLED;
+
+nvi_irq_thread_exit_reset:
+       if (st->dbg & NVI_DBG_SPEW_FIFO)
+               dev_info(&st->i2c->dev,
+                        "%s_exit_reset fifo_count=%u fifo_sample_size=%u\n",
+                        __func__, fifo_count, fifo_sample_size);
+       nvi_reset(st, true, false);
+       mutex_unlock(&indio_dev->mlock);
+       mutex_unlock(&st->srlock);
+       return IRQ_HANDLED;
+}
+
+static irqreturn_t nvi_irq_handler(int irq, void *dev_id)
+{
+       struct nvi_state *st = (struct nvi_state *)dev_id;
+       s64 ts;
+
+       if (!(st->master_enable & (1 << DEV_DMP))) {
+               ts = iio_get_time_ns();
+               kfifo_in_spinlocked(&st->timestamps, &ts, 1,
+                                   &st->time_stamp_lock);
+               if (kfifo_is_full(&st->timestamps)) {
+                       disable_irq_nosync(st->i2c->irq);
+                       st->irq_dis = true;
+               }
+       }
+       if (st->dbg & NVI_DBG_SPEW_IRQ)
+               dev_info(&st->i2c->dev, "%s\n", __func__);
+       return IRQ_WAKE_THREAD;
+}
+
+static ssize_t nvi_attr_store(struct device *dev,
+                             struct device_attribute *attr,
+                             const char *buf, size_t count)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+       const char *msg;
+       unsigned int new;
+       unsigned int old = 0;
+       int ret;
+
+       ret = kstrtouint(buf, 10, &new);
+       if (ret)
+               return -EINVAL;
+
+       mutex_lock(&indio_dev->mlock);
+       if (st->shutdown || st->suspend) {
+               mutex_unlock(&indio_dev->mlock);
+               return -EPERM;
+       }
+
+       switch (this_attr->address) {
+       case NVI_ATTR_ENABLE:
+               msg = "ATTR_ENABLE";
+               old = st->master_enable;
+               if (new & (1 << DEV_MASTER))
+                       st->master_enable |= new;
+               else if (new)
+                       st->master_enable &= ~new;
+               ret = nvi_enable(indio_dev);
+               break;
+
+       case NVI_ATTR_ACCEL_BATCH_FLAGS:
+               msg = "ATTR_ACCEL_BATCH_FLAGS";
+               old = st->batch_flags[DEV_ACCEL];
+               st->batch_flags[DEV_ACCEL] = new;
+               break;
+
+       case NVI_ATTR_ACCEL_BATCH_PERIOD:
+               msg = "ATTR_ACCEL_BATCH_PERIOD";
+               old = st->batch_period_us[DEV_ACCEL];
+               if (new < st->hal->min_delay_us)
+                       new = st->hal->min_delay_us;
+               st->batch_period_us[DEV_ACCEL] = new;
+               break;
+
+       case NVI_ATTR_ACCEL_BATCH_TIMEOUT:
+               msg = "ATTR_ACCEL_BATCH_TIMEOUT";
+               old = st->batch_timeout_ms[DEV_ACCEL];
+               st->batch_timeout_ms[DEV_ACCEL] = new;
+               ret = nvi_batch(st);
+               break;
+
+       case NVI_ATTR_ANGLVEL_BATCH_FLAGS:
+               msg = "ATTR_ANGLVEL_BATCH_FLAGS";
+               old = st->batch_flags[DEV_ANGLVEL];
+               st->batch_flags[DEV_ANGLVEL] = new;
+               break;
+
+       case NVI_ATTR_ANGLVEL_BATCH_PERIOD:
+               msg = "ATTR_ANGLVEL_BATCH_PERIOD";
+               old = st->batch_period_us[DEV_ANGLVEL];
+               if (new < st->hal->min_delay_us)
+                       new = st->hal->min_delay_us;
+               st->batch_period_us[DEV_ANGLVEL] = new;
+               break;
+
+       case NVI_ATTR_ANGLVEL_BATCH_TIMEOUT:
+               msg = "ATTR_ANGLVEL_BATCH_TIMEOUT";
+               old = st->batch_timeout_ms[DEV_ANGLVEL];
+               st->batch_timeout_ms[DEV_ANGLVEL] = new;
+               ret = nvi_batch(st);
+               break;
+
+       case NVI_ATTR_FLUSH:
+               ret = nvi_enable(indio_dev);
+               break;
+
+       case NVI_ATTR_FIFO_RSRV_EVNT_CNT:
+               msg = "ATTR_FIFO_RSRV_EVNT_CNT";
+               old = st->chip_config.fifo_reserve;
+               st->chip_config.fifo_reserve = new;
+               break;
+
+       case NVI_ATTR_FIFO_MAX_EVNT_CNT:
+               msg = "ATTR_FIFO_MAX_EVNT_CNT";
+               old = st->chip_config.fifo_max;
+               st->chip_config.fifo_max = new;
+               break;
+
+       case INV_ATTR_SELF_TEST:
+               msg = "ATTR_SELF_TEST";
+               old = !!(st->master_enable & (1 << EN_SELF_TEST));
+               if (new)
+                       st->master_enable |= (1 << EN_SELF_TEST);
+               else
+                       st->master_enable &= ~(1 << EN_SELF_TEST);
+               break;
+
+       default:
+               msg = "ATTR_UNKNOWN";
+               ret = -EINVAL;
+       }
+
+       mutex_unlock(&indio_dev->mlock);
+       if (st->dbg & NVI_DBG_SPEW_MSG) {
+               if (ret)
+                       dev_err(&st->i2c->dev, "%s %s %d->%d ERR=%d\n",
+                               __func__, msg, old, new, ret);
+               else
+                       dev_info(&st->i2c->dev, "%s %s %d->%d\n",
+                                __func__, msg, old, new);
+       }
+       if (ret)
+               return ret;
+
+       return count;
+}
+
+static ssize_t nvi_attr_show(struct device *dev,
+                            struct device_attribute *attr, char *buf)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+       int ret;
+
+       switch (this_attr->address) {
+       case NVI_ATTR_ENABLE:
+               return sprintf(buf, "%x\n", st->master_enable);
+
+       case NVI_ATTR_PART:
+               return sprintf(buf, "%s\n", st->hal->part_name);
+
+       case NVI_ATTR_VENDOR:
+               return sprintf(buf, "%s\n", NVI_VENDOR);
+
+       case NVI_ATTR_FLUSH:
+               /* returns batch capability */
+               return sprintf(buf, "%x\n", st->hal->dmp);
+
+       case NVI_ATTR_FIFO_RSRV_EVNT_CNT:
+               return sprintf(buf, "%u\n", st->chip_config.fifo_reserve);
+
+       case NVI_ATTR_FIFO_MAX_EVNT_CNT:
+               return sprintf(buf, "%u\n", st->chip_config.fifo_max);
+
+       case NVI_ATTR_ACCEL_PART:
+               return sprintf(buf, "%s accelerometer\n", st->hal->part_name);
+
+       case NVI_ATTR_ACCEL_VERSION:
+               return sprintf(buf, "%d\n", st->hal->accel->version);
+
+       case NVI_ATTR_ACCEL_MILLIAMP:
+               return sprintf(buf, "%s\n", st->hal->accel->power_ma);
+
+       case NVI_ATTR_ANGLVEL_PART:
+               return sprintf(buf, "%s gyro\n", st->hal->part_name);
+
+       case NVI_ATTR_ANGLVEL_VERSION:
+               return sprintf(buf, "%d\n", st->hal->anglvel->version);
+
+       case NVI_ATTR_ANGLVEL_MILLIAMP:
+               return sprintf(buf, "%s\n", st->hal->anglvel->power_ma);
+
+       case NVI_ATTR_TEMP_PART:
+               return sprintf(buf, "%s temperature\n", st->hal->part_name);
+
+       case NVI_ATTR_TEMP_VERSION:
+               return sprintf(buf, "%d\n", st->hal->temp->version);
+
+       case NVI_ATTR_TEMP_MILLIAMP:
+               return sprintf(buf, "%s\n", st->hal->temp->power_ma);
+
+       case INV_ATTR_SELF_TEST:
+               mutex_lock(&indio_dev->mlock);
+               ret = inv_hw_self_test(indio_dev);
+               mutex_unlock(&indio_dev->mlock);
+               return sprintf(buf, "%d\n", ret);
+
+       default:
+               return -EINVAL;
+       }
+
+       return -EINVAL;
+}
+
+static ssize_t nvi_data_store(struct device *dev,
+                             struct device_attribute *attr,
+                             const char *buf, size_t count)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       unsigned int info;
+       int ret;
+
+       ret = kstrtouint(buf, 10, &info);
+       if ((ret) || (info >= NVI_INFO_LIMIT_MAX))
+               return -EINVAL;
+
+       st->info = info;
+       switch (info) {
+       case NVI_INFO_DATA:
+               st->dbg = 0;
+               break;
+
+       case NVI_INFO_DBG:
+               st->dbg ^= NVI_DBG_SPEW_MSG;
+               break;
+
+       case NVI_INFO_AUX_SPEW:
+               st->dbg ^= NVI_DBG_SPEW_AUX;
+               nvi_aux_dbg(st, "SNAPSHOT", 0);
+               break;
+
+       case NVI_INFO_ANGLVEL_SPEW:
+               st->dbg ^= NVI_DBG_SPEW_ANGLVEL;
+               break;
+
+       case NVI_INFO_TEMP_SPEW:
+               st->dbg ^= NVI_DBG_SPEW_TEMP;
+               break;
+
+       case NVI_INFO_ACCEL_SPEW:
+               st->dbg ^= NVI_DBG_SPEW_ACCEL;
+               break;
+
+       case NVI_INFO_ACCEL_UC_SPEW:
+               st->dbg ^= NVI_DBG_SPEW_ACCEL_UC;
+               break;
+
+       case NVI_INFO_FIFO_SPEW:
+               st->dbg ^= NVI_DBG_SPEW_FIFO;
+               break;
+
+       case NVI_INFO_FIFO_BUF:
+               st->dbg ^= NVI_DBG_SPEW_BUF;
+               break;
+
+       case NVI_INFO_FIFO_IRQ:
+               st->dbg ^= NVI_DBG_SPEW_IRQ;
+               break;
+
+       default:
+               break;
+       }
+
+       return count;
+}
+
+static ssize_t nvi_data_show(struct device *dev,
+                            struct device_attribute *attr,
+                            char *buf)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       enum NVI_INFO info;
+       ssize_t t;
+       u8 data;
+       unsigned int i;
+       unsigned int j;
+       int ret;
+
+       info = st->info;
+       st->info = NVI_INFO_DATA;
+       switch (info) {
+       case NVI_INFO_DATA:
+               t = sprintf(buf, "ACCEL_UC_XYZ: %hd %hd %hd\n",
+                           st->accel_uc[AXIS_X],
+                           st->accel_uc[AXIS_Y],
+                           st->accel_uc[AXIS_Z]);
+               t += sprintf(buf + t, "ACCEL_XYZ: %hd %hd %hd\n",
+                            st->accel[AXIS_X],
+                            st->accel[AXIS_Y],
+                            st->accel[AXIS_Z]);
+               t += sprintf(buf + t, "ANGLVEL_XYZ: %hd %hd %hd\n",
+                            st->anglvel[AXIS_X],
+                            st->anglvel[AXIS_Y],
+                            st->anglvel[AXIS_Z]);
+               t += sprintf(buf + t, "TEMP: %hd\n", st->temp);
+               t += sprintf(buf + t, "TIMESTAMP: %lld\n", st->ts);
+               return t;
+
+       case NVI_INFO_VER:
+               return sprintf(buf, "version=%u\n", NVI_DRIVER_VERSION);
+
+       case NVI_INFO_ERRS:
+               i = st->errs;
+               st->errs = 0;
+               return sprintf(buf, "error count=%u\n", i);
+
+       case NVI_INFO_RESET:
+               mutex_lock(&indio_dev->mlock);
+               ret = nvi_pm(st, NVI_PM_ON);
+               ret |= nvi_wr_pwr_mgmt_1(st, BIT_H_RESET);
+               ret |= nvi_enable(indio_dev);
+               mutex_unlock(&indio_dev->mlock);
+               if (ret)
+                       return sprintf(buf, "reset ERR\n");
+               else
+                       return sprintf(buf, "reset done\n");
+
+       case NVI_INFO_REGS:
+               t = sprintf(buf, "registers:\n");
+               for (j = 0; j < st->hal->reg_bank_n; j++) {
+                       t += sprintf(buf + t, "register bank %u:\n", j);
+                       for (i = 0; i < st->hal->regs_n; i++) {
+                               if ((j == st->hal->reg->fifo_r_w.bank) &&
+                                            (i == st->hal->reg->fifo_r_w.reg))
+                                       data = 0;
+                               else
+                                       nvi_i2c_rd(st, j, i, 1, &data);
+                               t += sprintf(buf + t, "%hhx=%hhx\n", i, data);
+                       }
+               }
+               return t;
+
+       case NVI_INFO_DBG:
+               return sprintf(buf, "DBG spew=%x\n",
+                              !!(st->dbg & NVI_DBG_SPEW_MSG));
+
+       case NVI_INFO_AUX_SPEW:
+               return sprintf(buf, "AUX spew=%x\n",
+                              !!(st->dbg & NVI_DBG_SPEW_AUX));
+
+       case NVI_INFO_ANGLVEL_SPEW:
+               return sprintf(buf, "anglvel_xyz_ts spew=%x\n",
+                              !!(st->dbg & NVI_DBG_SPEW_ANGLVEL));
+
+       case NVI_INFO_TEMP_SPEW:
+               return sprintf(buf, "temp_ts spew=%x\n",
+                              !!(st->dbg & NVI_DBG_SPEW_TEMP));
+
+       case NVI_INFO_ACCEL_SPEW:
+               return sprintf(buf, "accel_xyz_ts spew=%x\n",
+                              !!(st->dbg & NVI_DBG_SPEW_ACCEL));
+
+       case NVI_INFO_ACCEL_UC_SPEW:
+               return sprintf(buf, "accel_xyz_uncalibrated_ts spew=%x\n",
+                              !!(st->dbg & NVI_DBG_SPEW_ACCEL_UC));
+
+       case NVI_INFO_FIFO_SPEW:
+               return sprintf(buf, "FIFO spew=%x\n",
+                              !!(st->dbg & NVI_DBG_SPEW_FIFO));
+
+       case NVI_INFO_FIFO_BUF:
+               return sprintf(buf, "BUF spew=%x\n",
+                              !!(st->dbg & NVI_DBG_SPEW_BUF));
+
+       case NVI_INFO_FIFO_IRQ:
+               return sprintf(buf, "IRQ spew=%x\n",
+                              !!(st->dbg & NVI_DBG_SPEW_IRQ));
+
+       default:
+               break;
+       }
+
+       return -EINVAL;
+}
+
+static ssize_t nvi_orientation_show(struct device *dev,
+                                   struct device_attribute *attr, char *buf)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       signed char *m;
+
+       m = st->pdata.orientation;
+       return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
+                      m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
+}
+
+#ifdef NVI_I2C_DEBUG_INTERFACE
+static ssize_t nvi_dbg_i2c_addr_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       u16 dbg_i2c_addr;
+
+       if (kstrtou16(buf, 16, &dbg_i2c_addr))
+               return -EINVAL;
+
+       st->dbg_i2c_addr = dbg_i2c_addr;
+       return count;
+}
+
+static ssize_t nvi_dbg_i2c_addr_show(struct device *dev,
+                                    struct device_attribute *attr, char *buf)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       u16 dbg_i2c_addr;
+
+       if (st->dbg_i2c_addr)
+               dbg_i2c_addr = st->dbg_i2c_addr;
+       else
+               dbg_i2c_addr = st->i2c->addr;
+       return sprintf(buf, "%#2x\n", dbg_i2c_addr);
+}
+
+static ssize_t nvi_dbg_reg_store(struct device *dev,
+                                struct device_attribute *attr,
+                                const char *buf, size_t count)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       u8 dbg_reg;
+
+       if (kstrtou8(buf, 16, &dbg_reg))
+               return -EINVAL;
+
+       st->dbg_reg = dbg_reg;
+       return count;
+}
+
+static ssize_t nvi_dbg_reg_show(struct device *dev,
+                               struct device_attribute *attr, char *buf)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+
+       return sprintf(buf, "%#2x\n", st->dbg_reg);
+}
+
+static ssize_t nvi_dbg_dat_store(struct device *dev,
+                                struct device_attribute *attr,
+                                const char *buf, size_t count)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       u16 dbg_i2c_addr;
+       u8 data[2];
+       int ret;
+
+       ret = kstrtou8(buf, 16, &data[1]);
+       if (ret)
+               return -EINVAL;
+
+       if (st->dbg_i2c_addr)
+               dbg_i2c_addr = st->dbg_i2c_addr;
+       else
+               dbg_i2c_addr = st->i2c->addr;
+       data[0] = st->dbg_reg;
+       ret = nvi_i2c_write(st, dbg_i2c_addr, sizeof(data), data);
+       pr_info("%s dev=%x reg=%x data=%x ret=%d\n",
+               __func__, dbg_i2c_addr, data[0], data[1], ret);
+       return count;
+}
+
+static ssize_t nvi_dbg_dat_show(struct device *dev,
+                               struct device_attribute *attr, char *buf)
+{
+       struct iio_dev *indio_dev = dev_get_drvdata(dev);
+       struct nvi_state *st = iio_priv(indio_dev);
+       u16 dbg_i2c_addr;
+       u8 dbg_dat = 0;
+       int ret;
+
+       if (st->dbg_i2c_addr)
+               dbg_i2c_addr = st->dbg_i2c_addr;
+       else
+               dbg_i2c_addr = st->i2c->addr;
+       ret = nvi_i2c_read(st, dbg_i2c_addr, st->dbg_reg, 1, &dbg_dat);
+       return sprintf(buf + ret, "%s dev=%x reg=%x data=%x ret=%d\n",
+                      __func__, dbg_i2c_addr, st->dbg_reg, dbg_dat, ret);
+}
+
+static DEVICE_ATTR(dbg_reg, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_dbg_reg_show, nvi_dbg_reg_store);
+static DEVICE_ATTR(dbg_dat, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_dbg_dat_show, nvi_dbg_dat_store);
+static DEVICE_ATTR(dbg_i2c_addr, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_dbg_i2c_addr_show, nvi_dbg_i2c_addr_store);
+#endif /* NVI_I2C_DEBUG_INTERFACE */
+
+#if 0
+static DEVICE_ATTR(lpa_delay, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_lpa_delay_enable_show, nvi_lpa_delay_enable_store);
+#endif
+
+static IIO_DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store, NVI_ATTR_ENABLE);
+static IIO_DEVICE_ATTR(part, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_PART);
+
+static IIO_DEVICE_ATTR(accel_part, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_ACCEL_PART);
+static IIO_DEVICE_ATTR(accel_vendor, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_VENDOR);
+static IIO_DEVICE_ATTR(accel_version, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_ACCEL_VERSION);
+static IIO_DEVICE_ATTR(accel_milliamp, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_ACCEL_MILLIAMP);
+static IIO_DEVICE_ATTR(accel_batch_flags, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_ACCEL_BATCH_FLAGS);
+static IIO_DEVICE_ATTR(accel_batch_period, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_ACCEL_BATCH_PERIOD);
+static IIO_DEVICE_ATTR(accel_batch_timeout, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_ACCEL_BATCH_TIMEOUT);
+static IIO_DEVICE_ATTR(accel_flush, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store, NVI_ATTR_FLUSH);
+static IIO_DEVICE_ATTR(accel_fifo_reserved_event_count,
+                      S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_FIFO_RSRV_EVNT_CNT);
+static IIO_DEVICE_ATTR(accel_fifo_max_event_count, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_FIFO_MAX_EVNT_CNT);
+static DEVICE_ATTR(accel_orientation, S_IRUGO,
+                  nvi_orientation_show, NULL);
+
+static IIO_DEVICE_ATTR(anglvel_part, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_ANGLVEL_PART);
+static IIO_DEVICE_ATTR(anglvel_vendor, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_VENDOR);
+static IIO_DEVICE_ATTR(anglvel_version, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_ANGLVEL_VERSION);
+static IIO_DEVICE_ATTR(anglvel_milliamp, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_ANGLVEL_MILLIAMP);
+static IIO_DEVICE_ATTR(anglvel_batch_flags, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_ANGLVEL_BATCH_FLAGS);
+static IIO_DEVICE_ATTR(anglvel_batch_period, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_ANGLVEL_BATCH_PERIOD);
+static IIO_DEVICE_ATTR(anglvel_batch_timeout, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_ANGLVEL_BATCH_TIMEOUT);
+static IIO_DEVICE_ATTR(anglvel_flush, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store, NVI_ATTR_FLUSH);
+static IIO_DEVICE_ATTR(anglvel_fifo_reserved_event_count,
+                      S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_FIFO_RSRV_EVNT_CNT);
+static IIO_DEVICE_ATTR(anglvel_fifo_max_event_count,
+                      S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store,
+                      NVI_ATTR_FIFO_MAX_EVNT_CNT);
+static DEVICE_ATTR(anglvel_orientation, S_IRUGO,
+                  nvi_orientation_show, NULL);
+
+static IIO_DEVICE_ATTR(temp_part, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_TEMP_PART);
+static IIO_DEVICE_ATTR(temp_vendor, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_VENDOR);
+static IIO_DEVICE_ATTR(temp_version, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_TEMP_VERSION);
+static IIO_DEVICE_ATTR(temp_milliamp, S_IRUGO,
+                      nvi_attr_show, NULL, NVI_ATTR_TEMP_MILLIAMP);
+
+static IIO_DEVICE_ATTR(self_test, S_IRUGO | S_IWUSR | S_IWGRP,
+                      nvi_attr_show, nvi_attr_store, INV_ATTR_SELF_TEST);
+
+static DEVICE_ATTR(data, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_data_show, nvi_data_store);
+
+static struct attribute *nvi_attrs[] = {
+       &iio_dev_attr_enable.dev_attr.attr,
+       &iio_dev_attr_part.dev_attr.attr,
+
+       &iio_dev_attr_accel_part.dev_attr.attr,
+       &iio_dev_attr_accel_vendor.dev_attr.attr,
+       &iio_dev_attr_accel_version.dev_attr.attr,
+       &iio_dev_attr_accel_milliamp.dev_attr.attr,
+       &iio_dev_attr_accel_batch_flags.dev_attr.attr,
+       &iio_dev_attr_accel_batch_period.dev_attr.attr,
+       &iio_dev_attr_accel_batch_timeout.dev_attr.attr,
+       &iio_dev_attr_accel_flush.dev_attr.attr,
+       &iio_dev_attr_accel_fifo_reserved_event_count.dev_attr.attr,
+       &iio_dev_attr_accel_fifo_max_event_count.dev_attr.attr,
+       &dev_attr_accel_orientation.attr,
+
+       &iio_dev_attr_anglvel_part.dev_attr.attr,
+       &iio_dev_attr_anglvel_vendor.dev_attr.attr,
+       &iio_dev_attr_anglvel_version.dev_attr.attr,
+       &iio_dev_attr_anglvel_milliamp.dev_attr.attr,
+       &iio_dev_attr_anglvel_batch_flags.dev_attr.attr,
+       &iio_dev_attr_anglvel_batch_period.dev_attr.attr,
+       &iio_dev_attr_anglvel_batch_timeout.dev_attr.attr,
+       &iio_dev_attr_anglvel_flush.dev_attr.attr,
+       &iio_dev_attr_anglvel_fifo_reserved_event_count.dev_attr.attr,
+       &iio_dev_attr_anglvel_fifo_max_event_count.dev_attr.attr,
+       &dev_attr_anglvel_orientation.attr,
+
+       &iio_dev_attr_temp_part.dev_attr.attr,
+       &iio_dev_attr_temp_vendor.dev_attr.attr,
+       &iio_dev_attr_temp_version.dev_attr.attr,
+       &iio_dev_attr_temp_milliamp.dev_attr.attr,
+
+       &iio_dev_attr_self_test.dev_attr.attr,
+
+       &dev_attr_data.attr,
+
+#ifdef NVI_I2C_DEBUG_INTERFACE
+       &dev_attr_dbg_reg.attr,
+       &dev_attr_dbg_dat.attr,
+       &dev_attr_dbg_i2c_addr.attr,
+#endif /* NVI_I2C_DEBUG_INTERFACE */
+       NULL
+};
+
+static struct attribute_group nvi_attr_group = {
+       .name = NVI_NAME,
+       .attrs = nvi_attrs
+};
+
+static int nvi_read_raw(struct iio_dev *indio_dev,
+                       struct iio_chan_spec const *chan,
+                       int *val, int *val2, long mask)
+{
+       struct nvi_state *st = iio_priv(indio_dev);
+       int i;
+       int ret = -EINVAL;
+
+       switch (mask) {
+       case IIO_CHAN_INFO_RAW:
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       *val = st->accel[chan->channel2 - IIO_MOD_X];
+                       return IIO_VAL_INT;
+
+               case IIO_ANGL_VEL:
+                       *val = st->anglvel[chan->channel2 - IIO_MOD_X];
+                       return IIO_VAL_INT;
+
+               case IIO_TEMP:
+                       if (st->pm >= NVI_PM_ON)
+                               nvi_temp_read(st, 0);
+                       *val = st->temp;
+                       return IIO_VAL_INT;
+
+               default:
+                       return -EINVAL;
+               }
+
+       case IIO_CHAN_INFO_SAMP_FREQ:
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       i = DEV_ACCEL;
+                       break;
+
+               case IIO_ANGL_VEL:
+                       i = DEV_ANGLVEL;
+                       break;
+
+               case IIO_TEMP:
+                       i = DEV_TEMP;
+                       break;
+
+               default:
+                       return -EINVAL;
+               }
+               if (st->master_enable & (1 << i))
+                       *val = st->delay_us[i];
+               else
+                       *val = st->hal->min_delay_us;
+               return IIO_VAL_INT;
+
+       case IIO_CHAN_INFO_SCALE:
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       *val = st->hal->accel->scale.ival;
+                       *val2 = st->hal->accel->scale.micro;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               case IIO_ANGL_VEL:
+                       *val = st->hal->anglvel->scale.ival;
+                       *val2 = st->hal->anglvel->scale.micro;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               case IIO_TEMP:
+                       *val = st->hal->temp->scale.ival;
+                       *val2 = st->hal->temp->scale.micro;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               default:
+                       return -EINVAL;
+               }
+
+       case IIO_CHAN_INFO_OFFSET:
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       /* offset is handled in HW */
+                       *val = 0;
+                       *val2 = 0;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               case IIO_ANGL_VEL:
+                       /* offset is handled in HW */
+                       *val = 0;
+                       *val2 = 0;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               case IIO_TEMP:
+                       *val = st->hal->temp->offset.ival;
+                       *val2 = st->hal->temp->offset.micro;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               default:
+                       return -EINVAL;
+               }
+
+       case IIO_CHAN_INFO_PEAK:
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       if ((st->master_enable & (1 << EN_SELF_TEST))) {
+                               *val = st->hal->accel_self_test_scale;
+                               return IIO_VAL_INT;
+                       } else {
+                               i = st->chip_config.accel_fs;
+                               *val = st->hal->accel->rr[i].max_range.ival;
+                               *val2 = st->hal->accel->rr[i].max_range.micro;
+                               return IIO_VAL_INT_PLUS_MICRO;
+                       }
+
+               case IIO_ANGL_VEL:
+                       if ((st->master_enable & (1 << EN_SELF_TEST))) {
+                               *val = st->hal->anglvel_self_test_scale;
+                               return IIO_VAL_INT;
+                       } else {
+                               i = st->chip_config.fsr;
+                               *val = st->hal->anglvel->rr[i].max_range.ival;
+                               *val2 = st->hal->anglvel->rr[i].max_range.micro;
+                               return IIO_VAL_INT_PLUS_MICRO;
+                       }
+
+               case IIO_TEMP:
+                       *val = st->hal->temp->rr->max_range.ival;
+                       *val2 = st->hal->temp->rr->max_range.micro;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               default:
+                       return -EINVAL;
+               }
+
+       case IIO_CHAN_INFO_PEAK_SCALE:
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       i = st->chip_config.accel_fs;
+                       *val = st->hal->accel->rr[i].resolution.ival;
+                       *val2 = st->hal->accel->rr[i].resolution.micro;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               case IIO_ANGL_VEL:
+                       i = st->chip_config.fsr;
+                       *val = st->hal->anglvel->rr[i].resolution.ival;
+                       *val2 = st->hal->anglvel->rr[i].resolution.micro;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               case IIO_TEMP:
+                       *val = st->hal->temp->rr->resolution.ival;
+                       *val2 = st->hal->temp->rr->resolution.micro;
+                       return IIO_VAL_INT_PLUS_MICRO;
+
+               default:
+                       return -EINVAL;
+               }
+
+       case IIO_CHAN_INFO_CALIBBIAS:
+               i = chan->channel2 - IIO_MOD_X;
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       *val = st->accel_bias[i] * st->chip_info.multi;
+                       return IIO_VAL_INT;
+
+               case IIO_ANGL_VEL:
+                       *val = st->gyro_bias[i];
+                       return IIO_VAL_INT;
+
+               default:
+                       return -EINVAL;
+               }
+
+
+       default:
+               return -EINVAL;
+       }
+
+       return ret;
+}
+
+static int nvi_write_raw(struct iio_dev *indio_dev,
+                        struct iio_chan_spec const *chan,
+                        int val, int val2, long mask)
+{
+       struct nvi_state *st = iio_priv(indio_dev);
+       const char *msg;
+       unsigned int dev;
+       unsigned int axis;
+       int old = 0;
+       int old2 = 0;
+       int ret = 0;
+
+       mutex_lock(&indio_dev->mlock);
+       if (st->shutdown || st->suspend) {
+               mutex_unlock(&indio_dev->mlock);
+               return -EPERM;
+       }
+
+       switch (mask) {
+       case IIO_CHAN_INFO_SAMP_FREQ:
+               msg = "IIO_CHAN_INFO_SAMP_FREQ";
+               if (val < st->hal->min_delay_us)
+                       val = st->hal->min_delay_us;
+               if (val > NVI_DELAY_US_MAX)
+                       val = NVI_DELAY_US_MAX;
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       dev = DEV_ACCEL;
+                       break;
+
+               case IIO_ANGL_VEL:
+                       dev = DEV_ANGLVEL;
+                       break;
+
+               case IIO_TEMP:
+                       dev = DEV_TEMP;
+                       break;
+
+               default:
+                       ret = -EINVAL;
+               }
+               if (!ret) {
+                       old = st->delay_us[dev];
+                       st->delay_us[dev] = val;
+                       if (st->enable[dev]) {
+                               ret = nvi_enable(indio_dev);
+                               if (ret)
+                                       st->delay_us[dev] = old;
+                       }
+               }
+               break;
+
+       case IIO_CHAN_INFO_PEAK:
+               msg = "IIO_CHAN_INFO_PEAK";
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       old = st->chip_config.accel_fs;
+                       st->chip_config.accel_fs = val;
+                       if (st->master_enable & (1 << DEV_ACCEL)) {
+                               ret = nvi_enable(indio_dev);
+                               if (ret)
+                                       st->chip_config.accel_fs = old;
+                       }
+                       break;
+
+               case IIO_ANGL_VEL:
+                       old = st->chip_config.fsr;
+                       st->chip_config.fsr = val;
+                       if (st->master_enable & (1 << DEV_ANGLVEL)) {
+                               ret = nvi_enable(indio_dev);
+                               if (ret)
+                                       st->chip_config.fsr = old;
+                       }
+                       break;
+
+               default:
+                       ret = -EINVAL;
+                       break;
+               }
+
+               break;
+
+       case IIO_CHAN_INFO_OFFSET:
+               msg = "IIO_CHAN_INFO_OFFSET";
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       axis = chan->channel2 - IIO_MOD_X;
+                       old = st->input_accel_offset[axis];
+                       st->input_accel_offset[axis] = val;
+                       if (st->master_enable & (1 << DEV_ACCEL)) {
+                               ret = nvi_enable(indio_dev);
+                               if (ret)
+                                       st->input_accel_offset[axis] = old;
+                       }
+                       break;
+
+               case IIO_ANGL_VEL:
+                       axis = chan->channel2 - IIO_MOD_X;
+                       old = st->input_gyro_offset[axis];
+                       st->input_gyro_offset[axis] = val;
+                       if (st->master_enable & (1 << DEV_ANGLVEL)) {
+                               ret = nvi_enable(indio_dev);
+                               if (ret)
+                                       st->input_gyro_offset[axis] = old;
+                       }
+                       break;
+
+               default:
+                       ret = -EINVAL;
+                       break;
+               }
+
+               break;
+
+       case IIO_CHAN_INFO_RAW:
+               msg = "IIO_CHAN_INFO_RAW";
+               switch (chan->type) {
+               case IIO_ACCEL:
+                       axis = chan->channel2 - IIO_MOD_X;
+                       old = st->accel[axis];
+                       st->accel[axis] = val;
+                       st->dbg |= (NVI_DBG_ACCEL_AXIS_X << axis);
+                       break;
+
+               case IIO_ANGL_VEL:
+                       axis = chan->channel2 - IIO_MOD_X;
+                       old = st->anglvel[axis];
+                       st->anglvel[chan->channel2 - IIO_MOD_X] = val;
+                       st->dbg |= (NVI_DBG_ANGLVEL_AXIS_X << axis);
+                       break;
+
+               case IIO_TEMP:
+                       old = st->temp;
+                       st->temp = val;
+                       st->dbg |= NVI_DBG_TEMP_VAL;
+                       break;
+
+               default:
+                       return -EINVAL;
+               }
+
+       default:
+               msg = "IIO_CHAN_INFO_UNKNOWN";
+               ret = -EINVAL;
+               break;
+       }
+
+       mutex_unlock(&indio_dev->mlock);
+       if (st->dbg & NVI_DBG_SPEW_MSG) {
+               if (ret) {
+                       dev_err(&st->i2c->dev, "%s %s chan=%d %d:%d->%d:%d\n",
+                               __func__, msg, chan->scan_index,
+                                old, old2, val, val2);
+                       dev_err(&st->i2c->dev, "%s ERR=%d mask=%ld type=%d\n",
+                               __func__, ret, mask, chan->type);
+               } else {
+                       dev_info(&st->i2c->dev, "%s %s chan=%d %d:%d->%d:%d\n",
+                                __func__, msg, chan->scan_index,
+                                old, old2, val, val2);
+               }
+       }
+       return ret;
+}
+
+static const struct iio_info nvi_iio_info = {
+       .driver_module = THIS_MODULE,
+       .attrs = &nvi_attr_group,
+       .read_raw = &nvi_read_raw,
+       .write_raw = &nvi_write_raw,
+};
+
+static const struct iio_chan_spec nvi_channels[] = {
+       {
+               .type                   = IIO_ACCEL,
+               .channel2               = IIO_MOD_X,
+               .scan_index             = NVI_SCAN_ACCEL_X,
+               .scan_type              = IIO_ST('s', 16, 16, 0),
+               .info_mask              = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                         BIT(IIO_CHAN_INFO_PEAK) |
+                                         BIT(IIO_CHAN_INFO_PEAK_SCALE) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_separate     = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                           BIT(IIO_CHAN_INFO_PEAK) |
+                                           BIT(IIO_CHAN_INFO_PEAK_SCALE),
+               .modified               = 1,
+       },
+       {
+               .type                   = IIO_ACCEL,
+               .channel2               = IIO_MOD_Y,
+               .scan_index             = NVI_SCAN_ACCEL_Y,
+               .scan_type              = IIO_ST('s', 16, 16, 0),
+               .info_mask              = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                         BIT(IIO_CHAN_INFO_PEAK) |
+                                         BIT(IIO_CHAN_INFO_PEAK_SCALE) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_separate     = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                           BIT(IIO_CHAN_INFO_PEAK) |
+                                           BIT(IIO_CHAN_INFO_PEAK_SCALE),
+               .modified               = 1,
+       },
+       {
+               .type                   = IIO_ACCEL,
+               .channel2               = IIO_MOD_Z,
+               .scan_index             = NVI_SCAN_ACCEL_Z,
+               .scan_type              = IIO_ST('s', 16, 16, 0),
+               .info_mask              = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                         BIT(IIO_CHAN_INFO_PEAK) |
+                                         BIT(IIO_CHAN_INFO_PEAK_SCALE) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_separate     = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                           BIT(IIO_CHAN_INFO_PEAK) |
+                                           BIT(IIO_CHAN_INFO_PEAK_SCALE),
+               .modified               = 1,
+       },
+       {
+               .type                   = IIO_ANGL_VEL,
+               .channel2               = IIO_MOD_X,
+               .scan_index             = NVI_SCAN_ANGLVEL_X,
+               .scan_type              = IIO_ST('s', 16, 16, 0),
+               .info_mask              = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                         BIT(IIO_CHAN_INFO_PEAK) |
+                                         BIT(IIO_CHAN_INFO_PEAK_SCALE) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_separate     = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                           BIT(IIO_CHAN_INFO_PEAK) |
+                                           BIT(IIO_CHAN_INFO_PEAK_SCALE),
+               .modified               = 1,
+       },
+       {
+               .type                   = IIO_ANGL_VEL,
+               .channel2               = IIO_MOD_Y,
+               .scan_index             = NVI_SCAN_ANGLVEL_Y,
+               .scan_type              = IIO_ST('s', 16, 16, 0),
+               .info_mask              = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                         BIT(IIO_CHAN_INFO_PEAK) |
+                                         BIT(IIO_CHAN_INFO_PEAK_SCALE) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_separate     = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                           BIT(IIO_CHAN_INFO_PEAK) |
+                                           BIT(IIO_CHAN_INFO_PEAK_SCALE),
+               .modified               = 1,
+       },
+       {
+               .type                   = IIO_ANGL_VEL,
+               .channel2               = IIO_MOD_Z,
+               .scan_index             = NVI_SCAN_ANGLVEL_Z,
+               .scan_type              = IIO_ST('s', 16, 16, 0),
+               .info_mask              = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+                                         BIT(IIO_CHAN_INFO_PEAK) |
+                                         BIT(IIO_CHAN_INFO_PEAK_SCALE) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+                                         BIT(IIO_CHAN_INFO_OFFSET),
+               .info_mask_separate     = BIT(IIO_CHAN_INFO_RAW) |
+                                         BIT(IIO_CHAN_INFO_CALIBBIAS) |
+                                         BIT(IIO_CHAN_INFO_SCALE) |
+