input: misc: fix sysfs permissions
Erik Lilliebjerg [Thu, 6 Jun 2013 14:25:35 +0000 (07:25 -0700)]
- Fix sysfs permissions.
- Fix raw accelerometer access.
- Fix autodetection behind MPU.
- Add self-test.

Bug 1224709
Bug 1243584
Bug 1291044
Bug 1295651
Bug 1290313
Bug 1298831

Change-Id: I55847fd158abdb9f12dc830218619c6ed7913396
Signed-off-by: Erik Lilliebjerg <elilliebjerg@nvidia.com>
Reviewed-on: http://git-master/r/243010
Reviewed-by: Seema Khowala <seemaj@nvidia.com>
Tested-by: Seema Khowala <seemaj@nvidia.com>

drivers/input/misc/compass/ak8975_input.c
drivers/input/misc/mpu/inv_gyro.c
drivers/input/misc/mpu/inv_gyro.h
drivers/input/misc/mpu/inv_gyro_misc.c
drivers/input/misc/mpu/inv_mpu3050.c
drivers/input/misc/pressure/bmp180.c
include/linux/mpu.h

index cfa6115..b882f66 100644 (file)
@@ -29,7 +29,7 @@
 #define AKM_HW_DELAY_ROM_ACCESS_US     (200)
 #define AKM_POLL_DELAY_MS_DFLT         (200)
 #define AKM_MPU_RETRY_COUNT            (20)
-#define AKM_MPU_RETRY_DELAY_MS         (20)
+#define AKM_MPU_RETRY_DELAY_MS         (100)
 #define AKM_ERR_CNT_MAX                        (20)
 
 #define AKM_INPUT_RESOLUTION           (1)
@@ -66,6 +66,7 @@
 #define AKM_REG_CNTL2                  (0x0B)
 #define AKM_CNTL2_SRST                 (0x01)
 #define AKM_REG_ASTC                   (0x0C)
+#define AKM_REG_ASTC_SELF              (0x40)
 #define AKM_REG_TS1                    (0x0D)
 #define AKM_REG_TS2                    (0x0E)
 #define AKM_REG_I2CDIS                 (0x0F)
 #define AKM8963_SCALE16                        (4915)
 #define AKM8972_SCALE                  (19661)
 #define AKM8975_SCALE                  (9830)
-#define AKM8975_RANGE_X_HI             (100)
 #define AKM8975_RANGE_X_LO             (-100)
-#define AKM8975_RANGE_Y_HI             (100)
+#define AKM8975_RANGE_X_HI             (100)
 #define AKM8975_RANGE_Y_LO             (-100)
-#define AKM8975_RANGE_Z_HI             (-300)
+#define AKM8975_RANGE_Y_HI             (100)
 #define AKM8975_RANGE_Z_LO             (-1000)
-#define AKM8972_RANGE_X_HI             (50)
+#define AKM8975_RANGE_Z_HI             (-300)
 #define AKM8972_RANGE_X_LO             (-50)
-#define AKM8972_RANGE_Y_HI             (50)
+#define AKM8972_RANGE_X_HI             (50)
 #define AKM8972_RANGE_Y_LO             (-50)
-#define AKM8972_RANGE_Z_HI             (-100)
+#define AKM8972_RANGE_Y_HI             (50)
 #define AKM8972_RANGE_Z_LO             (-500)
-#define AKM8963_RANGE_X_HI             (200)
-#define AKM8963_RANGE_X_LO             (-200)
-#define AKM8963_RANGE_Y_HI             (200)
-#define AKM8963_RANGE_Y_LO             (-200)
-#define AKM8963_RANGE_Z_HI             (-800)
-#define AKM8963_RANGE_Z_LO             (-3200)
+#define AKM8972_RANGE_Z_HI             (-100)
+#define AKM8963_RANGE14_X_LO           (-50)
+#define AKM8963_RANGE14_X_HI           (50)
+#define AKM8963_RANGE14_Y_LO           (-50)
+#define AKM8963_RANGE14_Y_HI           (50)
+#define AKM8963_RANGE14_Z_LO           (-800)
+#define AKM8963_RANGE14_Z_HI           (-200)
+#define AKM8963_RANGE16_X_LO           (-200)
+#define AKM8963_RANGE16_X_HI           (200)
+#define AKM8963_RANGE16_Y_LO           (-200)
+#define AKM8963_RANGE16_Y_HI           (200)
+#define AKM8963_RANGE16_Z_LO           (-3200)
+#define AKM8963_RANGE16_Z_HI           (-800)
+
 #define AXIS_X                         (0)
 #define AXIS_Y                         (1)
 #define AXIS_Z                         (2)
 #define WR                             (0)
 #define RD                             (1)
 
+enum AKM_DATA_INFO {
+       AKM_DATA_INFO_MAGNETIC_FIELD = 0,
+       AKM_DATA_INFO_MAGNETIC_FIELD_RAW,
+       AKM_DATA_INFO_CALIBRATION,
+       AKM_DATA_INFO_SELF_TEST,
+       AKM_DATA_INFO_SELF_TEST_RAW,
+       AKM_DATA_INFO_RESET,
+       AKM_DATA_INFO_REGISTERS,
+       AKM_DATA_INFO_LIMIT_MAX,
+};
 
 /* regulator names in order of powering on */
 static char *akm_vregs[] = {
@@ -129,20 +147,20 @@ struct akm_inf {
        struct regulator_bulk_data vreg[ARRAY_SIZE(akm_vregs)];
        struct mpu_platform_data pdata;
        struct akm_asa asa;             /* data for calibration */
-       unsigned int compass_id;        /* compass id */
-       unsigned long poll_delay_us;    /* requested sampling delay (us) */
+       unsigned int poll_delay_us;     /* requested sampling delay (us) */
+       unsigned int range_i;           /* max_range index */
        unsigned int resolution;        /* report when new data outside this */
+       unsigned int data_info;         /* data info to return */
+       unsigned int dev_id;            /* device id */
        bool use_mpu;                   /* if device behind MPU */
        bool initd;                     /* set if initialized */
        bool enable;                    /* enable status */
+       bool fifo_enable;               /* MPU FIFO enable */
        bool port_en[2];                /* enable status of MPU write port */
        int port_id[2];                 /* MPU port ID */
        u8 data_out;                    /* write value to trigger a sample */
-       u8 range_index;                 /* max_range index */
-       short xyz[3];                   /* sample data */
-       bool cycle;                     /* cycle MPU en/dis for high speed */
-       unsigned long cycle_delay_us;   /* cycle off time (us) */
-       s64 cycle_ts;                   /* cycle MPU disable timestamp */
+       s16 xyz_raw[3];                 /* raw sample data */
+       s16 xyz[3];                     /* sample data after calibration */
 };
 
 
@@ -330,12 +348,12 @@ static int akm_pm_init(struct akm_inf *inf)
 
        inf->initd = false;
        inf->enable = false;
+       inf->fifo_enable = false; /* DON'T ENABLE: MPU FIFO HW BROKEN */
        inf->port_en[WR] = false;
        inf->port_en[RD] = false;
        inf->port_id[WR] = -1;
        inf->port_id[RD] = -1;
        inf->poll_delay_us = (AKM_POLL_DELAY_MS_DFLT * 1000);
-       inf->cycle_delay_us = (AKM_INPUT_DELAY_MS_MIN * 1000);
        akm_vreg_init(inf);
        err = akm_pm(inf, true);
        return err;
@@ -388,7 +406,8 @@ static int akm_port_enable(struct akm_inf *inf, int port, bool enable)
        int err = 0;
 
        if (enable != inf->port_en[port]) {
-               err = nvi_mpu_enable(inf->port_id[port], enable, false);
+               err = nvi_mpu_enable(inf->port_id[port],
+                                    enable, inf->fifo_enable);
                if (!err)
                        inf->port_en[port] = enable;
        }
@@ -404,7 +423,8 @@ static int akm_ports_enable(struct akm_inf *inf, bool enable)
        return err;
 }
 
-static int akm_mode_wr(struct akm_inf *inf, bool reset, u8 range, u8 mode)
+static int akm_mode_wr(struct akm_inf *inf, bool reset,
+                      unsigned int range_i, u8 mode)
 {
        u8 mode_old;
        u8 mode_new;
@@ -413,7 +433,7 @@ static int akm_mode_wr(struct akm_inf *inf, bool reset, u8 range, u8 mode)
        int err = 0;
 
        mode_new = mode;
-       if (range)
+       if (range_i)
                mode |= AKM_CNTL1_OUTPUT_BIT16;
        if ((mode == inf->data_out) && (!reset))
                return err;
@@ -427,7 +447,7 @@ static int akm_mode_wr(struct akm_inf *inf, bool reset, u8 range, u8 mode)
                return err;
 
        if (reset) {
-               if (inf->compass_id == COMPASS_ID_AK8963) {
+               if (inf->dev_id == COMPASS_ID_AK8963) {
                        err = akm_nvi_mpu_bypass_request(inf);
                        if (!err) {
                                err = akm_wr(inf, AKM_REG_CNTL2,
@@ -446,7 +466,7 @@ static int akm_mode_wr(struct akm_inf *inf, bool reset, u8 range, u8 mode)
                        }
                }
        }
-       inf->range_index = range;
+       inf->range_i = range_i;
        inf->data_out = mode;
        if (inf->use_mpu) {
                if ((mode_old > AKM_CNTL1_MODE_SINGLE) ||
@@ -468,94 +488,31 @@ static int akm_mode_wr(struct akm_inf *inf, bool reset, u8 range, u8 mode)
        return err;
 }
 
-static int akm_delay(struct akm_inf *inf, unsigned long delay_us)
+static int akm_delay(struct akm_inf *inf, unsigned int delay_us)
 {
        u8 mode;
        int err = 0;
 
        if (inf->use_mpu)
-               err |= nvi_mpu_delay_us(inf->port_id[RD], delay_us);
+               err |= nvi_mpu_delay_us(inf->port_id[RD],
+                                       (unsigned int)delay_us);
        if (!err) {
-               if (inf->compass_id == COMPASS_ID_AK8963) {
+               if (inf->dev_id == COMPASS_ID_AK8963) {
                        if (delay_us == (AKM_INPUT_DELAY_MS_MIN * 1000))
                                mode = AKM_CNTL1_MODE_CONT2;
                        else
                                mode = AKM_CNTL1_MODE_SINGLE;
-                       err = akm_mode_wr(inf, false, inf->range_index, mode);
-               } else {
-                       if ((delay_us == (AKM_INPUT_DELAY_MS_MIN * 1000)) &&
-                                                          inf->cycle_delay_us)
-                               inf->cycle = true;
-                       else
-                               inf->cycle = false;
+                       err = akm_mode_wr(inf, false, inf->range_i, mode);
                }
        }
        return err;
 }
 
-static int akm_init_hw(struct akm_inf *inf)
-{
-       u8 val[8];
-       int err;
-       int err_t;
-
-       err_t = akm_nvi_mpu_bypass_request(inf);
-       if (!err_t) {
-               err_t = akm_wr(inf, AKM_REG_CNTL1, AKM_CNTL1_MODE_ROM_ACCESS);
-               udelay(AKM_HW_DELAY_ROM_ACCESS_US);
-               err_t |= akm_i2c_rd(inf, AKM_REG_ASAX, 3, inf->asa.asa);
-               /* we can autodetect AK8963 with BITM */
-               inf->compass_id = 0;
-               err = akm_wr(inf, AKM_REG_CNTL1, (AKM_CNTL1_MODE_SINGLE |
-                                                 AKM_CNTL1_OUTPUT_BIT16));
-               if (!err) {
-                       mdelay(AKM_HW_DELAY_TSM_MS);
-                       err = akm_i2c_rd(inf, AKM_REG_ST2, 1, val);
-                       if ((!err) && (val[0] & AKM_CNTL1_OUTPUT_BIT16))
-                               inf->compass_id = COMPASS_ID_AK8963;
-               }
-               akm_nvi_mpu_bypass_release(inf);
-               if (!inf->compass_id)
-                       inf->compass_id = inf->pdata.sec_slave_id;
-               if (inf->compass_id == COMPASS_ID_AK8963) {
-                       inf->asa.range_lo[AXIS_X] = AKM8963_RANGE_X_LO;
-                       inf->asa.range_hi[AXIS_X] = AKM8963_RANGE_X_HI;
-                       inf->asa.range_lo[AXIS_Y] = AKM8963_RANGE_Y_LO;
-                       inf->asa.range_hi[AXIS_Y] = AKM8963_RANGE_Y_HI;
-                       inf->asa.range_lo[AXIS_Z] = AKM8963_RANGE_Z_LO;
-                       inf->asa.range_hi[AXIS_Z] = AKM8963_RANGE_Z_HI;
-                       inf->range_index = 1;
-               } else if (inf->compass_id == COMPASS_ID_AK8972) {
-                       inf->asa.range_lo[AXIS_X] = AKM8972_RANGE_X_LO;
-                       inf->asa.range_hi[AXIS_X] = AKM8972_RANGE_X_HI;
-                       inf->asa.range_lo[AXIS_Y] = AKM8972_RANGE_Y_LO;
-                       inf->asa.range_hi[AXIS_Y] = AKM8972_RANGE_Y_HI;
-                       inf->asa.range_lo[AXIS_Z] = AKM8972_RANGE_Z_LO;
-                       inf->asa.range_hi[AXIS_Z] = AKM8972_RANGE_Z_HI;
-                       inf->range_index = 0;
-               } else { /* default COMPASS_ID_AK8975 */
-                       inf->compass_id = COMPASS_ID_AK8975;
-                       inf->asa.range_lo[AXIS_X] = AKM8975_RANGE_X_LO;
-                       inf->asa.range_hi[AXIS_X] = AKM8975_RANGE_X_HI;
-                       inf->asa.range_lo[AXIS_Y] = AKM8975_RANGE_Y_LO;
-                       inf->asa.range_hi[AXIS_Y] = AKM8975_RANGE_Y_HI;
-                       inf->asa.range_lo[AXIS_Z] = AKM8975_RANGE_Z_LO;
-                       inf->asa.range_hi[AXIS_Z] = AKM8975_RANGE_Z_HI;
-                       inf->range_index = 0;
-               }
-       }
-       if (!err_t)
-               inf->initd = true;
-       else
-               dev_err(&inf->i2c->dev, "%s ERR %d", __func__, err_t);
-       return err_t;
-}
-
 static void akm_calc(struct akm_inf *inf, u8 *data)
 {
-       short x;
-       short y;
-       short z;
+       s16 x;
+       s16 y;
+       s16 z;
 
        /* data[1] = AKM_REG_HXL
         * data[2] = AKM_REG_HXH
@@ -564,25 +521,40 @@ static void akm_calc(struct akm_inf *inf, u8 *data)
         * data[5] = AKM_REG_HZL
         * data[6] = AKM_REG_HZH
         */
-       x = ((data[2] << 8) | data[1]);
-       y = ((data[4] << 8) | data[3]);
-       z = ((data[6] << 8) | data[5]);
-       x = ((x * (inf->asa.asa[AXIS_X] + 128)) >> 8);
-       y = ((y * (inf->asa.asa[AXIS_Y] + 128)) >> 8);
-       z = ((z * (inf->asa.asa[AXIS_Z] + 128)) >> 8);
        mutex_lock(&inf->mutex_data);
+       x = (s16)le16_to_cpup((__le16 *)(&data[1]));
+       y = (s16)le16_to_cpup((__le16 *)(&data[3]));
+       z = (s16)le16_to_cpup((__le16 *)(&data[5]));
+       inf->xyz_raw[AXIS_X] = x;
+       inf->xyz_raw[AXIS_Y] = y;
+       inf->xyz_raw[AXIS_Z] = z;
+       x = (((int)x * (inf->asa.asa[AXIS_X] + 128)) >> 8);
+       y = (((int)y * (inf->asa.asa[AXIS_Y] + 128)) >> 8);
+       z = (((int)z * (inf->asa.asa[AXIS_Z] + 128)) >> 8);
        inf->xyz[AXIS_X] = x;
        inf->xyz[AXIS_Y] = y;
        inf->xyz[AXIS_Z] = z;
        mutex_unlock(&inf->mutex_data);
 }
 
+static s64 akm_timestamp_ns(void)
+{
+       struct timespec ts;
+       s64 ns;
+
+       ktime_get_ts(&ts);
+       ns = timespec_to_ns(&ts);
+       return ns;
+}
+
 static void akm_report(struct akm_inf *inf, u8 *data, s64 ts)
 {
        akm_calc(inf, data);
+       mutex_lock(&inf->mutex_data);
        input_report_rel(inf->idev, REL_X, inf->xyz[AXIS_X]);
        input_report_rel(inf->idev, REL_Y, inf->xyz[AXIS_Y]);
        input_report_rel(inf->idev, REL_Z, inf->xyz[AXIS_Z]);
+       mutex_unlock(&inf->mutex_data);
        input_report_rel(inf->idev, REL_MISC, (unsigned int)(ts >> 32));
        input_report_rel(inf->idev, REL_WHEEL,
                         (unsigned int)(ts & 0xffffffff));
@@ -591,30 +563,19 @@ static void akm_report(struct akm_inf *inf, u8 *data, s64 ts)
 
 static int akm_read_sts(struct akm_inf *inf, u8 *data)
 {
-       int err = -1; /* assume something wrong */
+       int err = 0; /* assume still processing */
        /* data[0] = AKM_REG_ST1
         * data[7] = AKM_REG_ST2
         * data[8] = AKM_REG_CNTL1
         */
-       if ((data[0] & AKM_ST1_DRDY) && (!(data[7] &
+       if ((data[0] == AKM_ST1_DRDY) && (!(data[7] &
                                           (AKM_ST2_HOFL | AKM_ST2_DERR))))
                err = 1; /* data ready to be reported */
-       else if ((data[8] & AKM_CNTL1_MODE_MASK) == (inf->data_out &
-                                                    AKM_CNTL1_MODE_MASK))
-               err = 0; /* still processing */
+       else if (!(data[8] & AKM_CNTL1_MODE_MASK))
+               err = -1; /* something wrong */
        return err;
 }
 
-static s64 akm_timestamp_ns(void)
-{
-       struct timespec ts;
-       s64 ns;
-
-       ktime_get_ts(&ts);
-       ns = timespec_to_ns(&ts);
-       return ns;
-}
-
 static int akm_read(struct akm_inf *inf)
 {
        long long timestamp1;
@@ -638,7 +599,7 @@ static int akm_read(struct akm_inf *inf)
                        akm_i2c_wr(inf, AKM_REG_CNTL1, inf->data_out);
        } else if (err < 0) {
                        dev_err(&inf->i2c->dev, "%s ERR\n", __func__);
-                       akm_mode_wr(inf, true, inf->range_index,
+                       akm_mode_wr(inf, true, inf->range_i,
                                    inf->data_out & AKM_CNTL1_MODE_MASK);
        }
        return err;
@@ -651,95 +612,144 @@ static void akm_mpu_handler(u8 *data, unsigned int len, s64 ts, void *p_val)
 
        inf = (struct akm_inf *)p_val;
        if (inf->enable) {
-               if (inf->cycle && (!inf->port_en[WR])) {
-                       ts -= inf->cycle_ts;
-                       if (ts > (inf->cycle_delay_us * 1000))
-                               akm_port_enable(inf, WR, true);
-                       return;
-               }
                err = akm_read_sts(inf, data);
-               if (err > 0) {
+               if (err > 0)
                        akm_report(inf, data, ts);
-                       if (inf->cycle) {
-                               akm_port_enable(inf, WR, false);
-                               inf->cycle_ts = ts;
-                       }
-               }
        }
 }
 
-static int akm_id(struct akm_inf *inf)
+static void akm_work(struct work_struct *ws)
 {
-       struct nvi_mpu_port nmp;
-       u8 config_boot;
-       u8 val = 0;
-       int err;
-
-       config_boot = inf->pdata.config & NVI_CONFIG_BOOT_MASK;
-       if (config_boot == NVI_CONFIG_BOOT_AUTO) {
-               nmp.addr = inf->i2c->addr | 0x80;
-               nmp.reg = AKM_REG_WIA;
-               nmp.ctrl = 1;
-               err = nvi_mpu_dev_valid(&nmp, &val);
-               /* see mpu.h for possible return values */
-               if ((err == -EAGAIN) || (err == -EBUSY))
-                       return -EAGAIN;
+       struct akm_inf *inf;
 
-               if (((!err) && (val == AKM_WIA_ID)) || (err == -EIO))
-                       config_boot = NVI_CONFIG_BOOT_MPU;
-       }
-       if (config_boot == NVI_CONFIG_BOOT_MPU) {
-               inf->use_mpu = true;
-               nmp.addr = inf->i2c->addr | 0x80;
-               nmp.reg = AKM_REG_ST1;
-               nmp.ctrl = 9;
-               nmp.data_out = 0;
-               nmp.delay_ms = 0;
-               nmp.delay_us = inf->poll_delay_us;
-               nmp.shutdown_bypass = false;
-               nmp.handler = &akm_mpu_handler;
-               nmp.ext_driver = (void *)inf;
-               err = nvi_mpu_port_alloc(&nmp);
-               if (err < 0)
-                       return err;
+       inf = container_of(ws, struct akm_inf, dw.work);
+       akm_read(inf);
+       queue_delayed_work(inf->wq, &inf->dw,
+                          usecs_to_jiffies(inf->poll_delay_us));
+}
 
-               inf->port_id[RD] = err;
-               nmp.addr = inf->i2c->addr;
-               nmp.reg = AKM_REG_CNTL1;
-               nmp.ctrl = 1;
-               nmp.data_out = inf->data_out;
-               nmp.delay_ms = AKM_HW_DELAY_TSM_MS;
-               nmp.delay_us = 0;
-               nmp.shutdown_bypass = false;
-               nmp.handler = NULL;
-               nmp.ext_driver = NULL;
-               err = nvi_mpu_port_alloc(&nmp);
-               if (err < 0) {
-                       akm_ports_free(inf);
+static void akm_range_limits(struct akm_inf *inf)
+{
+       if (inf->dev_id == COMPASS_ID_AK8963) {
+               if (inf->range_i) {
+                       inf->asa.range_lo[AXIS_X] = AKM8963_RANGE16_X_LO;
+                       inf->asa.range_hi[AXIS_X] = AKM8963_RANGE16_X_HI;
+                       inf->asa.range_lo[AXIS_Y] = AKM8963_RANGE16_Y_LO;
+                       inf->asa.range_hi[AXIS_Y] = AKM8963_RANGE16_Y_HI;
+                       inf->asa.range_lo[AXIS_Z] = AKM8963_RANGE16_Z_LO;
+                       inf->asa.range_hi[AXIS_Z] = AKM8963_RANGE16_Z_HI;
                } else {
-                       inf->port_id[WR] = err;
-                       err = 0;
+                       inf->asa.range_lo[AXIS_X] = AKM8963_RANGE14_X_LO;
+                       inf->asa.range_hi[AXIS_X] = AKM8963_RANGE14_X_HI;
+                       inf->asa.range_lo[AXIS_Y] = AKM8963_RANGE14_Y_LO;
+                       inf->asa.range_hi[AXIS_Y] = AKM8963_RANGE14_Y_HI;
+                       inf->asa.range_lo[AXIS_Z] = AKM8963_RANGE14_Z_LO;
+                       inf->asa.range_hi[AXIS_Z] = AKM8963_RANGE14_Z_HI;
                }
-               return err;
+       } else if (inf->dev_id == COMPASS_ID_AK8972) {
+               inf->asa.range_lo[AXIS_X] = AKM8972_RANGE_X_LO;
+               inf->asa.range_hi[AXIS_X] = AKM8972_RANGE_X_HI;
+               inf->asa.range_lo[AXIS_Y] = AKM8972_RANGE_Y_LO;
+               inf->asa.range_hi[AXIS_Y] = AKM8972_RANGE_Y_HI;
+               inf->asa.range_lo[AXIS_Z] = AKM8972_RANGE_Z_LO;
+               inf->asa.range_hi[AXIS_Z] = AKM8972_RANGE_Z_HI;
+       } else { /* default COMPASS_ID_AK8975 */
+               inf->dev_id = COMPASS_ID_AK8975;
+               inf->asa.range_lo[AXIS_X] = AKM8975_RANGE_X_LO;
+               inf->asa.range_hi[AXIS_X] = AKM8975_RANGE_X_HI;
+               inf->asa.range_lo[AXIS_Y] = AKM8975_RANGE_Y_LO;
+               inf->asa.range_hi[AXIS_Y] = AKM8975_RANGE_Y_HI;
+               inf->asa.range_lo[AXIS_Z] = AKM8975_RANGE_Z_LO;
+               inf->asa.range_hi[AXIS_Z] = AKM8975_RANGE_Z_HI;
        }
+}
 
-       /* NVI_CONFIG_BOOT_EXTERNAL */
-       inf->use_mpu = false;
-       err = akm_i2c_rd(inf, AKM_REG_WIA, 1, &val);
-       if ((!err) && (val == AKM_WIA_ID))
-               return 0;
+static int akm_self_test(struct akm_inf *inf)
+{
+       u8 data[9];
+       u8 mode;
+       int i;
+       int err;
+       int err_t;
 
-       return -ENODEV;
+       err_t = akm_i2c_wr(inf, AKM_REG_CNTL1, AKM_CNTL1_MODE_POWERDOWN);
+       udelay(AKM_HW_DELAY_US);
+       err_t |= akm_i2c_wr(inf, AKM_REG_ASTC, AKM_REG_ASTC_SELF);
+       udelay(AKM_HW_DELAY_US);
+       mode = AKM_CNTL1_MODE_SELFTEST;
+       if (inf->range_i)
+               mode |= AKM_CNTL1_OUTPUT_BIT16;
+       err_t |= akm_i2c_wr(inf, AKM_REG_CNTL1, mode);
+       for (i = 0; i < AKM_HW_DELAY_TSM_MS; i++) {
+               mdelay(AKM_HW_DELAY_TSM_MS);
+               err = akm_i2c_rd(inf, AKM_REG_ST1, 9, data);
+               if (!err) {
+                       err = akm_read_sts(inf, data);
+                       if (err > 0) {
+                               akm_calc(inf, data);
+                               err = 0;
+                               break;
+                       }
+                       err = -EBUSY;
+               }
+       }
+       err_t |= err;
+       akm_i2c_wr(inf, AKM_REG_ASTC, 0);
+       if (!err_t) {
+               akm_range_limits(inf);
+               if ((inf->xyz[AXIS_X] < inf->asa.range_lo[AXIS_X]) ||
+                               (inf->xyz[AXIS_X] > inf->asa.range_hi[AXIS_X]))
+                       err_t |= 1 << AXIS_X;
+               if ((inf->xyz[AXIS_Y] < inf->asa.range_lo[AXIS_Y]) ||
+                               (inf->xyz[AXIS_Y] > inf->asa.range_hi[AXIS_Y]))
+                       err_t |= 1 << AXIS_Y;
+               if ((inf->xyz[AXIS_Z] < inf->asa.range_lo[AXIS_Z]) ||
+                               (inf->xyz[AXIS_Z] > inf->asa.range_hi[AXIS_Z]))
+                       err_t |= 1 << AXIS_Z;
+               if (err_t) {
+                       dev_err(&inf->i2c->dev, "%s ERR: out_of_range %x\n",
+                               __func__, err_t);
+
+               }
+       }
+       return err_t;
 }
 
-static void akm_work(struct work_struct *ws)
+static int akm_init_hw(struct akm_inf *inf)
 {
-       struct akm_inf *inf;
+       u8 val[8];
+       int err;
+       int err_t;
 
-       inf = container_of(ws, struct akm_inf, dw.work);
-       akm_read(inf);
-       queue_delayed_work(inf->wq, &inf->dw,
-                          usecs_to_jiffies(inf->poll_delay_us));
+       err_t = akm_nvi_mpu_bypass_request(inf);
+       if (!err_t) {
+               err_t = akm_wr(inf, AKM_REG_CNTL1, AKM_CNTL1_MODE_ROM_ACCESS);
+               udelay(AKM_HW_DELAY_ROM_ACCESS_US);
+               err_t |= akm_i2c_rd(inf, AKM_REG_ASAX, 3, inf->asa.asa);
+               /* we can autodetect AK8963 with BITM */
+               inf->dev_id = 0;
+               err = akm_wr(inf, AKM_REG_CNTL1, (AKM_CNTL1_MODE_SINGLE |
+                                                 AKM_CNTL1_OUTPUT_BIT16));
+               if (!err) {
+                       mdelay(AKM_HW_DELAY_TSM_MS);
+                       err = akm_i2c_rd(inf, AKM_REG_ST2, 1, val);
+                       if ((!err) && (val[0] & AKM_CNTL1_OUTPUT_BIT16)) {
+                               inf->dev_id = COMPASS_ID_AK8963;
+                               inf->range_i = 1;
+                       }
+               }
+               if (!inf->dev_id)
+                       inf->dev_id = inf->pdata.sec_slave_id;
+               err = akm_self_test(inf);
+               if (err < 0)
+                       err_t |= err;
+               akm_nvi_mpu_bypass_release(inf);
+       }
+       if (!err_t)
+               inf->initd = true;
+       else
+               dev_err(&inf->i2c->dev, "%s ERR %d\n", __func__, err_t);
+       return err_t;
 }
 
 static int akm_enable(struct akm_inf *inf, bool enable)
@@ -752,18 +762,20 @@ static int akm_enable(struct akm_inf *inf, bool enable)
        if (!err) {
                if (enable) {
                        inf->data_out = AKM_CNTL1_MODE_SINGLE;
-                       err = akm_delay(inf, inf->poll_delay_us);
-                       err |= akm_mode_wr(inf, true, inf->range_index,
+                       err |= akm_delay(inf, inf->poll_delay_us);
+                       err |= akm_mode_wr(inf, true, inf->range_i,
                                          inf->data_out & AKM_CNTL1_MODE_MASK);
                        if (!err)
                                inf->enable = true;
                } else {
                        inf->enable = false;
-                       err = akm_mode_wr(inf, false, inf->range_index,
+                       err = akm_mode_wr(inf, false, inf->range_i,
                                          AKM_CNTL1_MODE_POWERDOWN);
                        if (!err)
                                akm_pm(inf, false);
                }
+       } else {
+               akm_pm(inf, false);
        }
        return err;
 }
@@ -786,10 +798,10 @@ static ssize_t akm_enable_store(struct device *dev,
                en = true;
        else
                en = false;
-       dev_dbg(&inf->i2c->dev, "%s: %x", __func__, en);
+       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, en);
        err = akm_enable(inf, en);
        if (err) {
-               dev_err(&inf->i2c->dev, "%s: %x ERR=%d", __func__, en, err);
+               dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", __func__, en, err);
                return err;
        }
 
@@ -800,11 +812,13 @@ static ssize_t akm_enable_show(struct device *dev,
                               struct device_attribute *attr, char *buf)
 {
        struct akm_inf *inf;
-       unsigned int enable = 0;
+       unsigned int enable;
 
        inf = dev_get_drvdata(dev);
        if (inf->enable)
                enable = 1;
+       else
+               enable = 0;
        return sprintf(buf, "%u\n", enable);
 }
 
@@ -813,15 +827,15 @@ static ssize_t akm_delay_store(struct device *dev,
                               const char *buf, size_t count)
 {
        struct akm_inf *inf;
-       unsigned long delay_us;
+       unsigned int delay_us;
        int err;
 
        inf = dev_get_drvdata(dev);
-       err = kstrtoul(buf, 10, &delay_us);
+       err = kstrtouint(buf, 10, &delay_us);
        if (err)
                return -EINVAL;
 
-       dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, delay_us);
+       dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, delay_us);
        if (delay_us < (AKM_INPUT_DELAY_MS_MIN * 1000))
                delay_us = (AKM_INPUT_DELAY_MS_MIN * 1000);
        if ((inf->enable) && (delay_us != inf->poll_delay_us))
@@ -829,7 +843,7 @@ static ssize_t akm_delay_store(struct device *dev,
        if (!err) {
                inf->poll_delay_us = delay_us;
        } else {
-               dev_err(&inf->i2c->dev, "%s: %lu ERR=%d\n",
+               dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
                        __func__, delay_us, err);
                return err;
        }
@@ -844,7 +858,7 @@ static ssize_t akm_delay_show(struct device *dev,
 
        inf = dev_get_drvdata(dev);
        if (inf->enable)
-               return sprintf(buf, "%lu\n", inf->poll_delay_us);
+               return sprintf(buf, "%u\n", inf->poll_delay_us);
 
        return sprintf(buf, "%u\n", (AKM_INPUT_DELAY_MS_MIN * 1000));
 }
@@ -860,7 +874,7 @@ static ssize_t akm_resolution_store(struct device *dev,
        if (kstrtouint(buf, 10, &resolution))
                return -EINVAL;
 
-       dev_dbg(&inf->i2c->dev, "%s %u", __func__, resolution);
+       dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, resolution);
        inf->resolution = resolution;
        return count;
 }
@@ -884,26 +898,26 @@ static ssize_t akm_max_range_store(struct device *dev,
                                   const char *buf, size_t count)
 {
        struct akm_inf *inf;
-       u8 range_index;
+       unsigned int range_i;
        int err;
 
        inf = dev_get_drvdata(dev);
-       err = kstrtou8(buf, 10, &range_index);
+       err = kstrtouint(buf, 10, &range_i);
        if (err)
                return -EINVAL;
 
-       dev_dbg(&inf->i2c->dev, "%s %u", __func__, range_index);
-       if (inf->compass_id == COMPASS_ID_AK8963) {
-               if (range_index > 1)
+       dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, range_i);
+       if (inf->dev_id == COMPASS_ID_AK8963) {
+               if (range_i > 1)
                        return -EINVAL;
 
                if (inf->enable) {
-                       err = akm_mode_wr(inf, false, range_index,
+                       err = akm_mode_wr(inf, false, range_i,
                                          inf->data_out & AKM_CNTL1_MODE_MASK);
                        if (err)
                                return err;
                } else {
-                       inf->range_index = range_index;
+                       inf->range_i = range_i;
                }
        } else {
                return -EINVAL;
@@ -920,14 +934,14 @@ static ssize_t akm_max_range_show(struct device *dev,
 
        inf = dev_get_drvdata(dev);
        if (inf->enable) {
-               range = inf->range_index;
+               range = inf->range_i;
        } else {
-               if (inf->compass_id == COMPASS_ID_AK8963) {
-                       if (inf->range_index)
+               if (inf->dev_id == COMPASS_ID_AK8963) {
+                       if (inf->range_i)
                                range = AKM8963_SCALE16;
                        else
                                range = AKM8963_SCALE14;
-               } else if (inf->compass_id == COMPASS_ID_AK8972) {
+               } else if (inf->dev_id == COMPASS_ID_AK8972) {
                        range = AKM8972_SCALE;
                } else {
                        range = AKM8975_SCALE;
@@ -936,71 +950,221 @@ static ssize_t akm_max_range_show(struct device *dev,
        return sprintf(buf, "%u\n", range);
 }
 
-static ssize_t akm_cycle_delay_us_store(struct device *dev,
-                                       struct device_attribute *attr,
-                                       const char *buf, size_t count)
+static ssize_t akm_data_store(struct device *dev,
+                             struct device_attribute *attr,
+                             const char *buf, size_t count)
 {
        struct akm_inf *inf;
-       unsigned long cycle_delay_us;
+       unsigned int data_info;
        int err;
 
        inf = dev_get_drvdata(dev);
-       err = kstrtoul(buf, 10, &cycle_delay_us);
+       err = kstrtouint(buf, 10, &data_info);
        if (err)
                return -EINVAL;
 
-       dev_dbg(&inf->i2c->dev, "%s %lu", __func__, cycle_delay_us);
-       inf->cycle_delay_us = cycle_delay_us;
+       if (data_info >= AKM_DATA_INFO_LIMIT_MAX)
+               return -EINVAL;
+
+       dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, data_info);
+       inf->data_info = data_info;
        return count;
 }
 
-static ssize_t akm_cycle_delay_us_show(struct device *dev,
-                                      struct device_attribute *attr,
-                                      char *buf)
+static ssize_t akm_data_show(struct device *dev,
+                            struct device_attribute *attr,
+                            char *buf)
 {
-       struct akm_inf *inf = dev_get_drvdata(dev);
+       struct akm_inf *inf;
+       u8 data[16];
+       s16 x = 0;
+       s16 y = 0;
+       s16 z = 0;
+       enum AKM_DATA_INFO data_info;
+       bool enable;
+       char const *info_str;
+       int err = 0;
 
-       return sprintf(buf, "%lu\n", inf->cycle_delay_us);
+       inf = dev_get_drvdata(dev);
+       data_info = inf->data_info;
+       inf->data_info = AKM_DATA_INFO_MAGNETIC_FIELD;
+       enable = inf->enable;
+       switch (data_info) {
+       case AKM_DATA_INFO_MAGNETIC_FIELD:
+               if (inf->enable) {
+                       mutex_lock(&inf->mutex_data);
+                       x = inf->xyz[AXIS_X];
+                       y = inf->xyz[AXIS_Y];
+                       z = inf->xyz[AXIS_Z];
+                       mutex_unlock(&inf->mutex_data);
+                       info_str = "magnetic_field";
+               } else {
+                       info_str = "ERR: DISABLED";
+               }
+               break;
+
+       case AKM_DATA_INFO_MAGNETIC_FIELD_RAW:
+               if (inf->enable) {
+                       mutex_lock(&inf->mutex_data);
+                       x = inf->xyz_raw[AXIS_X];
+                       y = inf->xyz_raw[AXIS_Y];
+                       z = inf->xyz_raw[AXIS_Z];
+                       mutex_unlock(&inf->mutex_data);
+                       info_str = "raw_data";
+               } else {
+                       info_str = "ERR: DISABLED";
+               }
+               break;
+
+       case AKM_DATA_INFO_CALIBRATION:
+               err = 0;
+               if (!inf->initd)
+                       err = akm_enable(inf, enable);
+               if (err) {
+                       info_str = "calibration ERR";
+               } else {
+                       x = (s16)inf->asa.asa[AXIS_X];
+                       y = (s16)inf->asa.asa[AXIS_Y];
+                       z = (s16)inf->asa.asa[AXIS_Z];
+                       info_str = "calibration";
+               }
+               break;
+
+       case AKM_DATA_INFO_SELF_TEST:
+               akm_enable(inf, false);
+               akm_pm(inf, true);
+               if (!inf->initd) {
+                       err = akm_init_hw(inf);
+               } else {
+                       err = akm_nvi_mpu_bypass_request(inf);
+                       if (!err) {
+                               err = akm_self_test(inf);
+                               akm_nvi_mpu_bypass_release(inf);
+                       }
+               }
+               if (err < 0) {
+                       info_str = "self_test ERR";
+               } else {
+                       mutex_lock(&inf->mutex_data);
+                       x = inf->xyz[AXIS_X];
+                       y = inf->xyz[AXIS_Y];
+                       z = inf->xyz[AXIS_Z];
+                       mutex_unlock(&inf->mutex_data);
+                       if (err > 0)
+                               info_str = "self_test FAIL";
+                       else
+                               info_str = "self_test PASS";
+               }
+               akm_enable(inf, enable);
+               break;
+
+       case AKM_DATA_INFO_SELF_TEST_RAW:
+               akm_enable(inf, false);
+               akm_pm(inf, true);
+               if (!inf->initd) {
+                       err = akm_init_hw(inf);
+               } else {
+                       err = akm_nvi_mpu_bypass_request(inf);
+                       if (!err) {
+                               err = akm_self_test(inf);
+                               akm_nvi_mpu_bypass_release(inf);
+                       }
+               }
+               if (err < 0) {
+                       info_str = "self_test ERR";
+               } else {
+                       mutex_lock(&inf->mutex_data);
+                       x = inf->xyz_raw[AXIS_X];
+                       y = inf->xyz_raw[AXIS_Y];
+                       z = inf->xyz_raw[AXIS_Z];
+                       mutex_unlock(&inf->mutex_data);
+                       info_str = "self_test raw";
+               }
+               akm_enable(inf, enable);
+               break;
+
+       case AKM_DATA_INFO_RESET:
+               akm_pm(inf, true);
+               err = akm_mode_wr(inf, true, inf->range_i,
+                                 inf->data_out & AKM_CNTL1_MODE_MASK);
+               akm_enable(inf, enable);
+               if (err)
+                       return sprintf(buf, "reset ERR\n");
+               else
+                       return sprintf(buf, "reset done\n");
+
+       case AKM_DATA_INFO_REGISTERS:
+               err = akm_nvi_mpu_bypass_request(inf);
+               if (!err) {
+                       err = akm_i2c_rd(inf, AKM_REG_WIA, AKM_REG_ASAX, data);
+                       akm_nvi_mpu_bypass_release(inf);
+               }
+               if (err)
+                       return sprintf(buf, "register read ERR\n");
+
+               return sprintf(buf, "%x %x %x %x %x %x %x %x %x %x\n",
+                              data[0], data[1], data[2],
+                              le16_to_cpup((__le16 *)(&data[3])),
+                              le16_to_cpup((__le16 *)(&data[5])),
+                              le16_to_cpup((__le16 *)(&data[7])),
+                              data[9], data[10], data[11], data[12]);
+
+       default:
+               return -EINVAL;
+       }
+
+       return sprintf(buf, "%s: %hd, %hd, %hd\n", info_str, x, y, z);
 }
 
-static ssize_t akm_divisor_show(struct device *dev,
-                               struct device_attribute *attr, char *buf)
+static ssize_t akm_mpu_fifo_enable_store(struct device *dev,
+                                        struct device_attribute *attr,
+                                        const char *buf, size_t count)
 {
        struct akm_inf *inf;
+       unsigned int fifo_enable;
+       int err;
 
        inf = dev_get_drvdata(dev);
-       return sprintf(buf, "%d\n", AKM_INPUT_DIVISOR);
+       err = kstrtouint(buf, 10, &fifo_enable);
+       if (err)
+               return -EINVAL;
+
+       if (!inf->use_mpu)
+               return -EINVAL;
+
+       if (fifo_enable)
+               inf->fifo_enable = true;
+       else
+               inf->fifo_enable = false;
+       dev_dbg(&inf->i2c->dev, "%s %x\n", __func__, inf->fifo_enable);
+       return count;
 }
 
-static ssize_t akm_microamp_show(struct device *dev,
-                                struct device_attribute *attr, char *buf)
+static ssize_t akm_mpu_fifo_enable_show(struct device *dev,
+                                       struct device_attribute *attr,
+                                       char *buf)
+{
+       struct akm_inf *inf = dev_get_drvdata(dev);
+
+       return sprintf(buf, "%x\n", inf->fifo_enable & inf->use_mpu);
+}
+
+static ssize_t akm_divisor_show(struct device *dev,
+                               struct device_attribute *attr, char *buf)
 {
        struct akm_inf *inf;
 
        inf = dev_get_drvdata(dev);
-       return sprintf(buf, "%d\n", AKM_INPUT_POWER_UA);
+       return sprintf(buf, "%u\n", AKM_INPUT_DIVISOR);
 }
 
-static ssize_t akm_magnetic_field_show(struct device *dev,
-                                      struct device_attribute *attr,
-                                      char *buf)
+static ssize_t akm_microamp_show(struct device *dev,
+                                struct device_attribute *attr, char *buf)
 {
        struct akm_inf *inf;
-       short x;
-       short y;
-       short z;
 
        inf = dev_get_drvdata(dev);
-       if (inf->enable) {
-               mutex_lock(&inf->mutex_data);
-               x = inf->xyz[AXIS_X];
-               y = inf->xyz[AXIS_Y];
-               z = inf->xyz[AXIS_Z];
-               mutex_unlock(&inf->mutex_data);
-               return sprintf(buf, "%hd, %hd, %hd\n", x, y, z);
-       }
-
-       return -EPERM;
+       return sprintf(buf, "%u\n", AKM_INPUT_POWER_UA);
 }
 
 static ssize_t akm_orientation_show(struct device *dev,
@@ -1016,22 +1180,22 @@ static ssize_t akm_orientation_show(struct device *dev,
                       m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
 }
 
-static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP,
                   akm_enable_show, akm_enable_store);
-static DEVICE_ATTR(delay, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(delay, S_IRUGO | S_IWUSR | S_IWGRP,
                   akm_delay_show, akm_delay_store);
-static DEVICE_ATTR(resolution, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(resolution, S_IRUGO | S_IWUSR | S_IWGRP,
                   akm_resolution_show, akm_resolution_store);
-static DEVICE_ATTR(max_range, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(max_range, S_IRUGO | S_IWUSR | S_IWGRP,
                   akm_max_range_show, akm_max_range_store);
-static DEVICE_ATTR(cycle_delay_us, S_IRUGO | S_IWUSR | S_IWOTH,
-                  akm_cycle_delay_us_show, akm_cycle_delay_us_store);
+static DEVICE_ATTR(data, S_IRUGO | S_IWUSR | S_IWGRP,
+                  akm_data_show, akm_data_store);
+static DEVICE_ATTR(mpu_fifo_en, S_IRUGO | S_IWUSR | S_IWGRP,
+                  akm_mpu_fifo_enable_show, akm_mpu_fifo_enable_store);
 static DEVICE_ATTR(divisor, S_IRUGO,
                   akm_divisor_show, NULL);
 static DEVICE_ATTR(microamp, S_IRUGO,
                   akm_microamp_show, NULL);
-static DEVICE_ATTR(magnetic_field, S_IRUGO,
-                  akm_magnetic_field_show, NULL);
 static DEVICE_ATTR(orientation, S_IRUGO,
                   akm_orientation_show, NULL);
 
@@ -1042,9 +1206,9 @@ static struct attribute *akm_attrs[] = {
        &dev_attr_max_range.attr,
        &dev_attr_divisor.attr,
        &dev_attr_microamp.attr,
-       &dev_attr_magnetic_field.attr,
+       &dev_attr_data.attr,
        &dev_attr_orientation.attr,
-       &dev_attr_cycle_delay_us.attr,
+       &dev_attr_mpu_fifo_en.attr,
        NULL
 };
 
@@ -1098,6 +1262,78 @@ static int akm_input_create(struct akm_inf *inf)
        return err;
 }
 
+static int akm_id(struct akm_inf *inf)
+{
+       struct nvi_mpu_port nmp;
+       u8 config_boot;
+       u8 val = 0;
+       int err;
+
+       config_boot = inf->pdata.config & NVI_CONFIG_BOOT_MASK;
+       if (config_boot == NVI_CONFIG_BOOT_AUTO) {
+               nmp.addr = inf->i2c->addr | 0x80;
+               nmp.reg = AKM_REG_WIA;
+               nmp.ctrl = 1;
+               err = nvi_mpu_dev_valid(&nmp, &val);
+               /* see mpu.h for possible return values */
+               dev_dbg(&inf->i2c->dev, "%s AUTO ID=%x err=%d\n",
+                       __func__, val, err);
+               if ((err == -EAGAIN) || (err == -EBUSY))
+                       return -EAGAIN;
+
+               if (((!err) && (val == AKM_WIA_ID)) || (err == -EIO))
+                       config_boot = NVI_CONFIG_BOOT_MPU;
+       }
+       if (config_boot == NVI_CONFIG_BOOT_MPU) {
+               inf->use_mpu = true;
+               nmp.addr = inf->i2c->addr | 0x80;
+               nmp.reg = AKM_REG_ST1;
+               nmp.ctrl = 10; /* MPU FIFO can't handle odd size */
+               nmp.data_out = 0;
+               nmp.delay_ms = 0;
+               nmp.delay_us = inf->poll_delay_us;
+               nmp.shutdown_bypass = false;
+               nmp.handler = &akm_mpu_handler;
+               nmp.ext_driver = (void *)inf;
+               err = nvi_mpu_port_alloc(&nmp);
+               dev_dbg(&inf->i2c->dev, "%s MPU port/err=%d\n",
+                       __func__, err);
+               if (err < 0)
+                       return err;
+
+               inf->port_id[RD] = err;
+               nmp.addr = inf->i2c->addr;
+               nmp.reg = AKM_REG_CNTL1;
+               nmp.ctrl = 1;
+               nmp.data_out = inf->data_out;
+               nmp.delay_ms = AKM_HW_DELAY_TSM_MS;
+               nmp.delay_us = 0;
+               nmp.shutdown_bypass = false;
+               nmp.handler = NULL;
+               nmp.ext_driver = NULL;
+               err = nvi_mpu_port_alloc(&nmp);
+               dev_dbg(&inf->i2c->dev, "%s MPU port/err=%d\n",
+                       __func__, err);
+               if (err < 0) {
+                       akm_ports_free(inf);
+               } else {
+                       inf->port_id[WR] = err;
+                       err = 0;
+               }
+               return err;
+       }
+
+       /* NVI_CONFIG_BOOT_HOST */
+       inf->use_mpu = false;
+       err = akm_i2c_rd(inf, AKM_REG_WIA, 1, &val);
+       dev_dbg(&inf->i2c->dev, "%s Host read ID=%x err=%d\n",
+               __func__, val, err);
+       if ((!err) && (val == AKM_WIA_ID))
+               return 0;
+
+       return -ENODEV;
+}
+
 static int akm_remove(struct i2c_client *client)
 {
        struct akm_inf *inf;
@@ -1127,7 +1363,7 @@ static struct mpu_platform_data *akm_parse_dt(struct i2c_client *client)
 {
        struct mpu_platform_data *pdata;
        struct device_node *np = client->dev.of_node;
-       char *pchar;
+       char const *pchar;
        u8 config;
        int len;
 
@@ -1142,8 +1378,8 @@ static struct mpu_platform_data *akm_parse_dt(struct i2c_client *client)
                dev_err(&client->dev, "Cannot read orientation property\n");
                return ERR_PTR(-EINVAL);
        }
-       memcpy(pdata->orientation, pchar, len);
 
+       memcpy(pdata->orientation, pchar, len);
        if (of_property_read_string(np, "config", &pchar)) {
                dev_err(&client->dev, "Cannot read config property\n");
                return ERR_PTR(-EINVAL);
@@ -1155,7 +1391,6 @@ static struct mpu_platform_data *akm_parse_dt(struct i2c_client *client)
                        break;
                }
        }
-
        if (config == ARRAY_SIZE(akm_configs)) {
                dev_err(&client->dev, "Invalid config value\n");
                return ERR_PTR(-EINVAL);
@@ -1180,17 +1415,17 @@ static int akm_probe(struct i2c_client *client,
 
        inf->i2c = client;
        i2c_set_clientdata(client, inf);
-
-       if (client->dev.of_node)
+       if (client->dev.of_node) {
                pd = akm_parse_dt(client);
-       else
+               if (IS_ERR(pd))
+                       return -EINVAL;
+       } else {
                pd = (struct mpu_platform_data *)dev_get_platdata(&client->dev);
-
-       if (!pd || IS_ERR(pd))
-               return -EINVAL;
+               if (!pd)
+                       return -EINVAL;
+       }
 
        inf->pdata = *pd;
-
        akm_pm_init(inf);
        err = akm_id(inf);
        akm_pm(inf, false);
@@ -1225,6 +1460,23 @@ akm_probe_again:
        return err;
 }
 
+static int akm_suspend(struct device *dev)
+{
+       struct akm_inf *inf;
+       int err;
+
+       inf = dev_get_drvdata(dev);
+       err = akm_enable(inf, false);
+       if (err)
+               dev_err(dev, "%s ERR\n", __func__);
+       dev_info(dev, "%s done\n", __func__);
+       return 0;
+}
+
+static const struct dev_pm_ops akm_pm_ops = {
+       .suspend        = akm_suspend,
+};
+
 static const struct i2c_device_id akm_i2c_device_id[] = {
        {AKM_NAME, 0},
        {"ak8963", 0},
@@ -1236,6 +1488,7 @@ static const struct i2c_device_id akm_i2c_device_id[] = {
 MODULE_DEVICE_TABLE(i2c, akm_i2c_device_id);
 
 static const struct of_device_id akm_of_match[] = {
+       { .compatible = "ak,ak89xx", },
        { .compatible = "ak,ak8963", },
        { .compatible = "ak,ak8972", },
        { .compatible = "ak,ak8975", },
@@ -1249,9 +1502,10 @@ static struct i2c_driver akm_driver = {
        .probe          = akm_probe,
        .remove         = akm_remove,
        .driver = {
-               .name   = AKM_NAME,
-               .owner  = THIS_MODULE,
+               .name           = AKM_NAME,
+               .owner          = THIS_MODULE,
                .of_match_table = of_match_ptr(akm_of_match),
+               .pm             = &akm_pm_ops,
        },
        .id_table       = akm_i2c_device_id,
        .shutdown       = akm_shutdown,
index cfa76ab..65e375f 100644 (file)
@@ -23,6 +23,7 @@
  *      @details This driver currently works for the ITG3500, MPU6050, MPU9150
  *               MPU3050
  */
+#define DEBUG_FIFO_DATA_SPEW   (0)
 
 #include <linux/module.h>
 #include <linux/init.h>
@@ -72,18 +73,25 @@ static struct inv_reg_map_s chip_reg = {
        .pwr_mgmt_2             = 0x6C,
        .mem_start_addr         = 0x6E,
        .mem_r_w                = 0x6F,
-       .prgm_strt_addrh        = 0x70
+       .prgm_strt_addrh        = 0x70,
+
+       .accl_fifo_en           = BIT_ACCEL_OUT,
+       .fifo_reset             = BIT_FIFO_RST,
+       .i2c_mst_reset          = BIT_I2C_MST_RST,
+       .cycle                  = BIT_CYCLE
 };
 
 static const struct inv_hw_s hw_info[INV_NUM_PARTS] = {
        {119, "ITG3500"},
        { 63, "MPU3050"},
-       {118, "MPU6050"},
-       {118, "MPU9150"}
+       {117, "MPU6050"},
+       {118, "MPU9150"},
+       {119, "MPU6500"},
+       {118, "MPU9250"},
 };
 
 static unsigned long nvi_lpf_us_tbl[] = {
-       3906,   /* 256Hz */
+       0, /* WAR: disabled 3906, 256Hz */
        5319,   /* 188Hz */
        10204,  /* 98Hz */
        23810,  /* 42Hz */
@@ -92,16 +100,31 @@ static unsigned long nvi_lpf_us_tbl[] = {
        /* 200000, 5Hz */
 };
 
-static unsigned long nvi_lpa_delay_us_tbl[] = {
-       800000,
-       200000,
-       50000,
-       /* 25000, */
+static unsigned long nvi_lpa_delay_us_tbl_6050[] = {
+       800000, /* 800ms */
+       200000, /* 200ms */
+       50000,  /* 50ms */
+       /* 25000, 25ms */
+};
+
+static unsigned long nvi_lpa_delay_us_tbl_6500[] = {
+       4096000,/* 4096ms */
+       2048000,/* 2048ms */
+       1024000,/* 1024ms */
+       512000, /* 512ms */
+       256000, /* 256ms */
+       128000, /* 128ms */
+       64000,  /* 64ms */
+       32000,  /* 32ms */
+       16000,  /* 16ms */
+       8000,   /* 8ms */
+       4000,   /* 4ms */
+       /* 2000, 2ms */
 };
 
 static struct inv_gyro_state_s *inf_local;
 
-s64 get_time_ns(void)
+s64 nvi_ts_ns(void)
 {
        struct timespec ts;
        ktime_get_ts(&ts);
@@ -190,8 +213,7 @@ int inv_i2c_single_write_base(struct inv_gyro_state_s *st,
 
 
 /* Register SMPLRT_DIV (0x19) */
-static int nvi_smplrt_div_wr(struct inv_gyro_state_s *inf,
-                            unsigned char smplrt_div)
+static int nvi_smplrt_div_wr(struct inv_gyro_state_s *inf, u8 smplrt_div)
 {
        int err = 0;
 
@@ -205,24 +227,33 @@ static int nvi_smplrt_div_wr(struct inv_gyro_state_s *inf,
 }
 
 /* Register CONFIG (0x1A) */
-static int nvi_config_wr(struct inv_gyro_state_s *inf, unsigned char lpf)
+static int nvi_config_wr(struct inv_gyro_state_s *inf, u8 val)
 {
        int err = 0;
 
-       if (lpf != inf->hw.config) {
-               err = inv_i2c_single_write(inf, inf->reg->lpf, lpf);
-               if (!err)
-                       inf->hw.config = lpf;
+       if (val != inf->hw.config) {
+               err = inv_i2c_single_write(inf, inf->reg->lpf, val);
+               if (!err) {
+                       inf->hw.config = val;
+                       err = 1; /* flag change made */
+               }
        }
        return err;
 }
 
 /* Register GYRO_CONFIG (0x1B) */
-static int nvi_gyro_config_wr(struct inv_gyro_state_s *inf, unsigned char fsr)
+static int nvi_gyro_config_wr(struct inv_gyro_state_s *inf, u8 fsr)
 {
-       unsigned char val;
+       u8 val;
        int err = 0;
 
+       if (inf->chip_type == INV_MPU3050) {
+               val = inf->hw.config;
+               val &= 0xE7;
+               val |= fsr << 3;
+               return nvi_config_wr(inf, val);
+       }
+
        if (fsr != inf->hw.gyro_config) {
                val = (fsr << 3);
                err = inv_i2c_single_write(inf, inf->reg->gyro_config, val);
@@ -234,29 +265,61 @@ static int nvi_gyro_config_wr(struct inv_gyro_state_s *inf, unsigned char fsr)
        return err;
 }
 
-/* Register ACCEL_CONFIG (0x1C) */
-static int nvi_accel_config_wr(struct inv_gyro_state_s *inf,
-                              unsigned char fsr, unsigned char hpf)
+/* Register ACCEL_CONFIG2 (0x1D) */
+static int nvi_accel_config2_wr(struct inv_gyro_state_s *inf, u8 val)
 {
-       unsigned char val;
        int err = 0;
 
-       val = (fsr << 3) | hpf;
+       if (val != inf->hw.accl_config2) {
+               err = inv_i2c_single_write(inf, 0x1D, val);
+               if (!err) {
+                       inf->hw.accl_config2 = val;
+                       err = 1; /* flag change made */
+               }
+       }
+       return err;
+}
+
+/* Register ACCEL_CONFIG (0x1C) */
+static int nvi_accel_config_wr(struct inv_gyro_state_s *inf, u8 fsr, u8 hpf)
+{
+       u8 val;
+       int err;
+       int err_t = 0;
+
+       val = (fsr << 3);
+       if (inf->chip_type == INV_MPU6500)
+               err_t = nvi_accel_config2_wr(inf, hpf);
+       else
+               val |= hpf;
        if (val != inf->hw.accl_config) {
-               err = inv_i2c_single_write(inf, inf->reg->accl_config,
-                                          val);
+               err = inv_i2c_single_write(inf, inf->reg->accl_config, val);
                if (!err) {
                        inf->hw.accl_config = val;
-                       err = 1; /* flag change made */
-                       if (hpf != 7)
-                               inf->mot_enable = false;
+                       err_t |= 1; /* flag change made */
+               } else {
+                       err_t |= err;
                }
        }
+       return err_t;
+}
+
+/* Register LP_ACCEL_ODR (0x1E) */
+static int nvi_lp_accel_odr_wr(struct inv_gyro_state_s *inf, u8 lposc_clksel)
+{
+       int err = 0;
+
+       if (lposc_clksel != inf->hw.lposc_clksel) {
+               err = inv_i2c_single_write(inf, REG_6500_LP_ACCEL_ODR,
+                                          lposc_clksel);
+               if (!err)
+                       inf->hw.lposc_clksel = lposc_clksel;
+       }
        return err;
 }
 
 /* Register MOT_THR (0x1F) */
-static int nvi_mot_thr_wr(struct inv_gyro_state_s *inf, unsigned char mot_thr)
+static int nvi_mot_thr_wr(struct inv_gyro_state_s *inf, u8 mot_thr)
 {
        int err = 0;
 
@@ -269,7 +332,7 @@ static int nvi_mot_thr_wr(struct inv_gyro_state_s *inf, unsigned char mot_thr)
 }
 
 /* Register MOT_DUR (0x20) */
-static int nvi_mot_dur_wr(struct inv_gyro_state_s *inf, unsigned char mot_dur)
+static int nvi_mot_dur_wr(struct inv_gyro_state_s *inf, u8 mot_dur)
 {
        int err = 0;
 
@@ -282,7 +345,7 @@ static int nvi_mot_dur_wr(struct inv_gyro_state_s *inf, unsigned char mot_dur)
 }
 
 /* Register FIFO_EN (0x23) */
-static int nvi_fifo_en_wr(struct inv_gyro_state_s *inf, unsigned char fifo_en)
+static int nvi_fifo_en_wr(struct inv_gyro_state_s *inf, u8 fifo_en)
 {
        int err = 0;
 
@@ -298,11 +361,14 @@ static int nvi_fifo_en_wr(struct inv_gyro_state_s *inf, unsigned char fifo_en)
 static int nvi_i2c_mst_ctrl_wr(struct inv_gyro_state_s *inf,
                               bool port3_fifo_en)
 {
-       unsigned char val;
+       u8 val;
        int err = 0;
 
+       if (inf->chip_type == INV_MPU3050)
+               return 0;
+
        val = inf->aux.clock_i2c;
-       val |= BIT_WAIT_FOR_ES | BIT_I2C_MST_P_NSR;
+       val |= BIT_WAIT_FOR_ES;
        if (port3_fifo_en)
                val |= BIT_SLV3_FIFO_EN;
        if (val != inf->hw.i2c_mst_ctrl) {
@@ -313,10 +379,68 @@ static int nvi_i2c_mst_ctrl_wr(struct inv_gyro_state_s *inf,
        return err;
 }
 
+/* Register I2C_SLV0_CTRL (0x25) */
+/* Register I2C_SLV1_CTRL (0x28) */
+/* Register I2C_SLV2_CTRL (0x2B) */
+/* Register I2C_SLV3_CTRL (0x2E) */
+/* Register I2C_SLV4_CTRL (0x31) */
+static int nvi_i2c_slv_addr_wr(struct inv_gyro_state_s *inf, int port, u8 addr)
+{
+       u8 reg;
+       int err = 0;
+
+       reg = (REG_I2C_SLV0_ADDR + (port * 3));
+       if (addr != inf->hw.i2c_slv_addr[port]) {
+               err = inv_i2c_single_write(inf, reg, addr);
+               if (!err)
+                       inf->hw.i2c_slv_addr[port] = addr;
+       }
+       return err;
+}
+
+/* Register I2C_SLV0_CTRL (0x26) */
+/* Register I2C_SLV1_CTRL (0x29) */
+/* Register I2C_SLV2_CTRL (0x2C) */
+/* Register I2C_SLV3_CTRL (0x2F) */
+/* Register I2C_SLV4_CTRL (0x32) */
+static int nvi_i2c_slv_reg_wr(struct inv_gyro_state_s *inf, int port, u8 val)
+{
+       u8 reg;
+       int err = 0;
+
+       reg = (REG_I2C_SLV0_REG + (port * 3));
+       if (val != inf->hw.i2c_slv_reg[port]) {
+               err = inv_i2c_single_write(inf, reg, val);
+               if (!err)
+                       inf->hw.i2c_slv_reg[port] = val;
+       }
+       return err;
+}
+
+/* Register I2C_SLV0_CTRL (0x27) */
+/* Register I2C_SLV1_CTRL (0x2A) */
+/* Register I2C_SLV2_CTRL (0x2D) */
+/* Register I2C_SLV3_CTRL (0x30) */
+static int nvi_i2c_slv_ctrl_wr(struct inv_gyro_state_s *inf, int port, u8 val)
+{
+       u8 reg;
+       int err = 0;
+
+       reg = (REG_I2C_SLV0_CTRL + (port * 3));
+       if (val != inf->hw.i2c_slv_ctrl[port]) {
+               err = inv_i2c_single_write(inf, reg, val);
+               if (!err) {
+                       inf->hw.i2c_slv_ctrl[port] = val;
+                       err = 1; /* flag change made */
+               }
+       }
+       return err;
+}
+
 /* Register I2C_SLV4_CTRL (0x34) */
 static int nvi_i2c_slv4_ctrl_wr(struct inv_gyro_state_s *inf, bool slv4_en)
 {
-       unsigned char val;
+       u8 val;
        int err = 0;
 
        val = inf->aux.delay_hw;
@@ -326,14 +450,16 @@ static int nvi_i2c_slv4_ctrl_wr(struct inv_gyro_state_s *inf, bool slv4_en)
                val |= BIT_SLV_EN;
        if (val != inf->hw.i2c_slv4_ctrl) {
                err = inv_i2c_single_write(inf, REG_I2C_SLV4_CTRL, val);
-               if (!err)
+               if (!err) {
                        inf->hw.i2c_slv4_ctrl = val;
+                       err = 1; /* flag change made */
+               }
        }
        return err;
 }
 
 /* Register INT_PIN_CFG (0x37) */
-static int nvi_int_pin_cfg_wr(struct inv_gyro_state_s *inf, unsigned char val)
+static int nvi_int_pin_cfg_wr(struct inv_gyro_state_s *inf, u8 val)
 {
        int err = 0;
 
@@ -348,18 +474,23 @@ static int nvi_int_pin_cfg_wr(struct inv_gyro_state_s *inf, unsigned char val)
 /* Register INT_ENABLE (0x38) */
 static int nvi_int_enable_wr(struct inv_gyro_state_s *inf, bool enable)
 {
-       unsigned char val = 0;
+       u8 val = 0;
        int err = 0;
 
        if (enable) {
                if ((inf->hw.user_ctrl & BIT_I2C_MST_EN) ||
                                                inf->chip_config.gyro_enable) {
-                       val = BIT_DATA_RDY_EN;
+                       if ((inf->hw.user_ctrl & BIT_FIFO_EN) &&
+                                                 (!inf->chip_config.fifo_thr))
+                               val = BIT_FIFO_OVERFLOW;
+                       else
+                               val = BIT_DATA_RDY_EN;
                } else if (inf->chip_config.accl_enable) {
-                       if (inf->mot_enable && (!inf->mot_cnt)) {
+                       if (((inf->hw.accl_config & 0x07) == 0x07) &&
+                                                            (!inf->mot_cnt)) {
                                val = BIT_MOT_EN;
-                               if (inf->mot_dbg)
-                                       pr_info("%s motion detect on",
+                               if (inf->chip_config.mot_enable == NVI_MOT_DBG)
+                                       pr_info("%s motion detect on\n",
                                                __func__);
                        } else {
                                val = BIT_DATA_RDY_EN;
@@ -368,15 +499,44 @@ static int nvi_int_enable_wr(struct inv_gyro_state_s *inf, bool enable)
        }
        if ((val != inf->hw.int_enable) && (inf->pm > NVI_PM_OFF)) {
                err = inv_i2c_single_write(inf, inf->reg->int_enable, val);
-               if (!err)
+               if (!err) {
                        inf->hw.int_enable = val;
+                       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, val);
+               }
+       }
+       return err;
+}
+
+/* Register I2C_SLV0_CTRL (0x63) */
+/* Register I2C_SLV1_CTRL (0x64) */
+/* Register I2C_SLV2_CTRL (0x65) */
+/* Register I2C_SLV3_CTRL (0x66) */
+/* Register I2C_SLV4_CTRL (0x33) */
+static int nvi_i2c_slv_do_wr(struct inv_gyro_state_s *inf,
+                            int port, u8 data_out)
+{
+       u8 *hw;
+       u8 reg;
+       int err = 0;
+
+       if (port == AUX_PORT_SPECIAL) {
+               hw = &inf->hw.i2c_slv4_do;
+               reg = REG_I2C_SLV4_DO;
+       } else {
+               hw = &inf->hw.i2c_slv_do[port];
+               reg = REG_I2C_SLV0_DO + port;
+       }
+       if (data_out != *hw) {
+               err = inv_i2c_single_write(inf, reg, data_out);
+               if (!err)
+                       *hw = data_out;
        }
        return err;
 }
 
 /* Register I2C_MST_DELAY_CTRL (0x67) */
 static int nvi_i2c_mst_delay_ctrl_wr(struct inv_gyro_state_s *inf,
-                                    unsigned char i2c_mst_delay_ctrl)
+                                    u8 i2c_mst_delay_ctrl)
 {
        int err = 0;
 
@@ -390,8 +550,7 @@ static int nvi_i2c_mst_delay_ctrl_wr(struct inv_gyro_state_s *inf,
 }
 
 /* Register MOT_DETECT_CTRL (0x69) */
-static int nvi_mot_detect_ctrl_wr(struct inv_gyro_state_s *inf,
-                                 unsigned char val)
+static int nvi_mot_detect_ctrl_wr(struct inv_gyro_state_s *inf, u8 val)
 {
        int err = 0;
 
@@ -404,8 +563,7 @@ static int nvi_mot_detect_ctrl_wr(struct inv_gyro_state_s *inf,
 }
 
 /* Register USER_CTRL (0x6A) */
-static int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf,
-                                 unsigned char val)
+static int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf, u8 val)
 {
        int i;
        int err;
@@ -415,7 +573,7 @@ static int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf,
        for (i = 0; i < POWER_UP_TIME; i++) {
                val = -1;
                err = inv_i2c_read(inf, inf->reg->user_ctrl, 1, &val);
-               if (!(val & (BIT_FIFO_RST | BIT_I2C_MST_RST)))
+               if (!(val & (inf->reg->fifo_reset | inf->reg->i2c_mst_reset)))
                        break;
 
                mdelay(1);
@@ -429,61 +587,95 @@ static int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf,
 static int nvi_user_ctrl_en_wr(struct inv_gyro_state_s *inf,
                               bool fifo_enable, bool i2c_enable)
 {
-       unsigned char val;
+       u8 val;
+       u16 fifo_sample_size;
        bool en;
        int i;
-       int err = 0;
+       int err;
+       int err_t = 0;
 
-       if (inf->lpa_enable)
+       dev_dbg(&inf->i2c->dev, "%s: FIFO=%x I2C=%x\n",
+               __func__, fifo_enable, i2c_enable);
+       if (inf->hw.pwr_mgmt_1 & inf->reg->cycle)
                fifo_enable = false;
        val = 0;
+       fifo_sample_size = 0;
+       inf->fifo_sample_size = 0;
+       en = false;
        if (fifo_enable) {
-               for (i = 0; i < (AUX_PORT_SPECIAL - 1); i++) {
-                       if (inf->aux.port[i].fifo_en && inf->aux.port[i].hw_en)
-                               val |= (1 << i);
+               if (inf->chip_type == INV_MPU3050) {
+                       val |= BIT_3050_FIFO_FOOTER;
+                       fifo_sample_size += 2;
+               }
+               if (inf->chip_config.accl_fifo_enable) {
+                       val |= inf->reg->accl_fifo_en;
+                       fifo_sample_size += 6;
                }
-               if (inf->chip_config.gyro_fifo_enable)
-                       val |= (inf->chip_config.gyro_enable << 4);
-               if (inf->chip_config.accl_fifo_enable)
-                       val |= BIT_ACCEL_OUT;
-               if (inf->chip_config.temp_fifo_enable)
+               if (inf->chip_config.temp_enable &&
+                                          inf->chip_config.temp_fifo_enable) {
                        val |= BIT_TEMP_FIFO_EN;
-               if (inf->aux.port[3].fifo_en && inf->aux.port[3].hw_en)
-                       en = true;
-               else
-                       en = false;
-               err |= nvi_i2c_mst_ctrl_wr(inf, en);
+                       fifo_sample_size += 2;
+               }
+               if (inf->chip_config.gyro_fifo_enable) {
+                       val |= (inf->chip_config.gyro_fifo_enable << 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 (inf->aux.port[i].fifo_en &&
+                                 (inf->aux.port[i].nmp.addr & BIT_I2C_READ) &&
+                                     (inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN)) {
+                               if (i == 3)
+                                       en = true;
+                               else
+                                       val |= (1 << i);
+                               fifo_sample_size += inf->aux.port[i].nmp.ctrl &
+                                                   BITS_I2C_SLV_CTRL_LEN;
+                       }
+               }
+               err_t |= nvi_i2c_mst_ctrl_wr(inf, en);
                if (val || en)
                        en = true;
                else
                        en = false;
+               inf->fifo_sample_size = fifo_sample_size;
        } else {
-               err |= nvi_i2c_mst_ctrl_wr(inf, false);
-               en = false;
+               err_t |= nvi_i2c_mst_ctrl_wr(inf, false);
        }
-       err |= nvi_fifo_en_wr(inf, val);
+       err_t |= nvi_fifo_en_wr(inf, val);
        val = 0;
        if (fifo_enable && en)
                val |= BIT_FIFO_EN;
-       if (i2c_enable && inf->aux.enable)
+       if (i2c_enable && (inf->aux.enable || inf->aux.en3050))
                val |= BIT_I2C_MST_EN;
        if (val != inf->hw.user_ctrl) {
-               err |= inv_i2c_single_write(inf, inf->reg->user_ctrl, val);
-               if (!err)
+               err = inv_i2c_single_write(inf, inf->reg->user_ctrl, val);
+               if (err) {
+                       err_t |= err;
+                       dev_err(&inf->i2c->dev, "%s ERR FIFO=%x I2C=%x",
+                               __func__, fifo_enable, i2c_enable);
+               } else {
                        inf->hw.user_ctrl = val;
+                       dev_dbg(&inf->i2c->dev, "%s FIFO=%x I2C=%x", __func__,
+                               (val & BIT_FIFO_EN), (val & BIT_I2C_MST_EN));
+               }
        }
-       return err;
+       return err_t;
 }
 
 /* Register PWR_MGMT_1 (0x6B) */
-static int nvi_pwr_mgmt_1_wr(struct inv_gyro_state_s *inf, unsigned char pm1)
+static int nvi_pwr_mgmt_1_war(struct inv_gyro_state_s *inf)
 {
-       unsigned char val;
+       u8 val;
        int i;
        int err;
 
        for (i = 0; i < POWER_UP_TIME; i++) {
-               inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, pm1);
+               inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, 0);
                val = -1;
                err = inv_i2c_read(inf, inf->reg->pwr_mgmt_1, 1, &val);
                if (!val)
@@ -495,28 +687,65 @@ static int nvi_pwr_mgmt_1_wr(struct inv_gyro_state_s *inf, unsigned char pm1)
        return err;
 }
 
-static bool nvi_lpa_able(struct inv_gyro_state_s *inf)
+/* Register PWR_MGMT_1 (0x6B) */
+static int nvi_pwr_mgmt_1_wr(struct inv_gyro_state_s *inf, u8 pwr_mgmt_1)
 {
-       bool lpa_enable;
-       int i;
+       int err = 0;
 
-       if (inf->mot_enable) {
-               lpa_enable = true;
-       } else if (inf->chip_config.lpa_delay_us) {
-               if (inf->chip_config.accl_delay_us <
-                                                inf->chip_config.lpa_delay_us)
-                       lpa_enable = false;
-               else
-                       lpa_enable = true;
-       } else {
-               lpa_enable = false;
+       if (pwr_mgmt_1 != inf->hw.pwr_mgmt_1) {
+               err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1,
+                                          pwr_mgmt_1);
+               if (!err)
+                       inf->hw.pwr_mgmt_1 = pwr_mgmt_1;
        }
-       for (i = 0; i < ARRAY_SIZE(nvi_lpa_delay_us_tbl); i++) {
-               if (inf->chip_config.accl_delay_us >= nvi_lpa_delay_us_tbl[i])
-                       break;
+       return err;
+}
+
+/* Register PWR_MGMT_2 (0x6C) */
+static int nvi_pwr_mgmt_2_wr(struct inv_gyro_state_s *inf, u8 pwr_mgmt_2)
+{
+       int err = 0;
+
+       if (pwr_mgmt_2 != inf->hw.pwr_mgmt_2) {
+               err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_2,
+                                          pwr_mgmt_2);
+               if (!err)
+                       inf->hw.pwr_mgmt_2 = pwr_mgmt_2;
        }
-       inf->lpa_hw = i;
-       return lpa_enable;
+       return err;
+}
+
+static int nvi_motion_detect_enable(struct inv_gyro_state_s *inf, u8 mot_thr)
+{
+       int err;
+       int err_t = 0;
+
+       if (inf->chip_type != INV_MPU6050)
+               return 0;
+
+       if (mot_thr) {
+               err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0);
+               if (err < 0)
+                       err_t |= err;
+               err = nvi_config_wr(inf, 0);
+               if (err < 0)
+                       err_t |= err;
+               err_t |= nvi_mot_dur_wr(inf, inf->chip_config.mot_dur);
+               err_t |= nvi_mot_detect_ctrl_wr(inf,
+                                               inf->chip_config.mot_ctrl);
+               err_t |= nvi_mot_thr_wr(inf, mot_thr);
+               mdelay(5);
+               err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 7);
+               if (err < 0)
+                       err_t |= err;
+               if (err_t)
+                       nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0);
+       } else {
+               err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0);
+               if (err < 0)
+                       err_t |= err;
+       }
+       return err_t;
 }
 
 static int nvi_vreg_dis(struct inv_gyro_state_s *inf, unsigned int i)
@@ -613,16 +842,17 @@ static int nvi_vreg_init(struct inv_gyro_state_s *inf)
        return err;
 }
 
-static int nvi_pm_wr_on(struct inv_gyro_state_s *inf, u8 pm1, u8 pm2)
+static int nvi_pm_wr(struct inv_gyro_state_s *inf,
+                    u8 pwr_mgmt_1, u8 pwr_mgmt_2, u8 lpa)
 {
-       unsigned char val;
+       u8 val;
        int i;
        int err;
        int err_t = 0;
 
        err = nvi_vreg_en_all(inf);
        if (err) {
-               err_t |= nvi_pwr_mgmt_1_wr(inf, 0);
+               err_t |= nvi_pwr_mgmt_1_war(inf);
                err_t |= inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1,
                                              BIT_RESET);
                for (i = 0; i < POWER_UP_TIME; i++) {
@@ -634,110 +864,91 @@ static int nvi_pm_wr_on(struct inv_gyro_state_s *inf, u8 pm1, u8 pm2)
                        mdelay(1);
                }
                err_t |= err;
-               err_t |= nvi_pwr_mgmt_1_wr(inf, 0);
-               err_t |= nvi_user_ctrl_reset_wr(inf, (BIT_FIFO_RST |
-                                                     BIT_I2C_MST_RST));
-               for (i = 0; i < AUX_PORT_MAX; i++) {
-                       inf->aux.port[i].hw_valid = false;
-                       inf->aux.port[i].hw_en = false;
-               }
+               err_t |= nvi_pwr_mgmt_1_war(inf);
+               err_t |= nvi_user_ctrl_reset_wr(inf, (inf->reg->fifo_reset |
+                                                    inf->reg->i2c_mst_reset));
                memset(&inf->hw, 0, sizeof(struct nvi_hw));
                inf->sample_delay_us = 0;
-               inf->mot_enable = false;
        } else {
-               err_t |= nvi_pwr_mgmt_1_wr(inf, 0);
-       }
-       if (pm2 != inf->hw.pwr_mgmt_2) {
-               err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_2, pm2);
+               err_t |= nvi_pwr_mgmt_1_war(inf);
+       }
+       switch (inf->chip_type) {
+       case INV_MPU3050:
+               pwr_mgmt_1 &= (BIT_SLEEP | INV_CLK_PLL);
+               if (pwr_mgmt_1 & INV_CLK_PLL) {
+                       err = nvi_pwr_mgmt_1_wr(inf, (BITS_3050_POWER1 |
+                                                     INV_CLK_PLL));
+                       err |= nvi_pwr_mgmt_1_wr(inf, (BITS_3050_POWER2 |
+                                                      INV_CLK_PLL));
+                       err |= nvi_pwr_mgmt_1_wr(inf, INV_CLK_PLL);
+               } else {
+                       pwr_mgmt_1 |= (pwr_mgmt_2 & 0x07) << 3;
+                       err = nvi_pwr_mgmt_1_wr(inf, pwr_mgmt_1);
+               }
                if (err)
                        err_t |= err;
                else
-                       inf->hw.pwr_mgmt_2 = pm2;
-       }
-       if (pm1 != inf->hw.pwr_mgmt_1) {
-               err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, pm1);
+                       inf->hw.pwr_mgmt_2 = pwr_mgmt_2;
+               break;
+
+       case INV_MPU6050:
+               pwr_mgmt_2 |= lpa << 6;
+               err = nvi_pwr_mgmt_2_wr(inf, pwr_mgmt_2);
                if (err)
                        err_t |= err;
                else
-                       inf->hw.pwr_mgmt_1 = pm1;
-       }
-       return err_t;
-}
-
-static int nvi_pm_wr(struct inv_gyro_state_s *inf, int pm, int stby)
-{
-       int err = 0;
-
-       if ((pm == inf->pm) && (stby == inf->stby))
-               return err;
-
-       switch (pm) {
-       case NVI_PM_OFF_FORCE:
-       case NVI_PM_OFF:
-               err = nvi_pm_wr_on(inf, BIT_SLEEP, stby);
-               err |= nvi_vreg_dis_all(inf);
-               break;
-
-       case NVI_PM_STDBY:
-               err = nvi_pm_wr_on(inf, BIT_SLEEP, stby);
+                       inf->hw.lposc_clksel = lpa;
+               err_t |= nvi_pwr_mgmt_1_wr(inf, pwr_mgmt_1);
                break;
 
-       case NVI_PM_ON_CYCLE:
-               err = nvi_pm_wr_on(inf, BIT_CYCLE, stby);
-               break;
-
-       case NVI_PM_ON:
-               err = nvi_pm_wr_on(inf, INV_CLK_INTERNAL, stby);
-               break;
-
-       case NVI_PM_ON_FULL:
-               err = nvi_pm_wr_on(inf, INV_CLK_PLL, stby);
-               break;
-
-       default:
-               err = -EINVAL;
+       default: /* INV_MPU6500 */
+               err_t |= nvi_lp_accel_odr_wr(inf, lpa);
+               err_t |= nvi_pwr_mgmt_2_wr(inf, pwr_mgmt_2);
+               err_t |= nvi_pwr_mgmt_1_wr(inf, pwr_mgmt_1);
                break;
        }
 
-       if (err < 0) {
-               dev_err(&inf->i2c->dev, "%s requested pm=%d ERR=%d\n",
-                       __func__, pm, err);
-               pm = NVI_PM_ERR;
-       } else {
-               inf->stby = stby;
-       }
-       inf->pm = pm;
-       dev_dbg(&inf->i2c->dev, "%s pm=%d stby=%x\n", __func__, pm, stby);
-       if (err > 0)
-               err = 0;
-       return err;
+       return err_t;
 }
 
 static int nvi_pm(struct inv_gyro_state_s *inf, int pm_req)
 {
        bool irq;
-       int stby;
+       bool mot_det_dis;
+       u8 pwr_mgmt_2;
+       u8 lpa;
        int pm;
-       int err;
+       int err = 0;
 
-       nvi_int_enable_wr(inf, false);
-       inf->lpa_enable = false;
-       if ((pm_req == NVI_PM_OFF_FORCE) || (pm_req == NVI_PM_OFF)) {
-               stby = 0x3F;
+       mot_det_dis = false;
+       lpa = inf->hw.lposc_clksel;
+       if ((pm_req == NVI_PM_OFF_FORCE) || (pm_req == NVI_PM_OFF) ||
+                                                               inf->suspend) {
+               pwr_mgmt_2 = 0x3F;
                pm = NVI_PM_OFF_FORCE;
        } else {
-               stby = ((~inf->chip_config.accl_enable) & 0x07) << 3;
-               stby |= (~inf->chip_config.gyro_enable) & 0x07;
+               pwr_mgmt_2 = ((~inf->chip_config.accl_enable) & 0x07) << 3;
+               pwr_mgmt_2 |= (~inf->chip_config.gyro_enable) & 0x07;
                if (inf->chip_config.gyro_enable ||
+                               (inf->chip_config.temp_enable & NVI_TEMP_EN) ||
                                        (inf->hw.user_ctrl & BIT_I2C_MST_EN)) {
+                       mot_det_dis = true;
                        if (inf->chip_config.gyro_enable)
                                pm = NVI_PM_ON_FULL;
                        else
                                pm = NVI_PM_ON;
                } else if (inf->chip_config.accl_enable) {
-                       if (nvi_lpa_able(inf)) {
-                               inf->lpa_enable = true;
-                               stby |= (inf->lpa_hw << 6);
+                       if (((inf->hw.accl_config & 0x07) == 0x07) ||
+                                             (inf->chip_config.lpa_delay_us &&
+                                              inf->hal.lpa_tbl_n &&
+                                            (inf->chip_config.accl_delay_us >=
+                                            inf->chip_config.lpa_delay_us))) {
+                               for (lpa = 0; lpa < inf->hal.lpa_tbl_n;
+                                                                      lpa++) {
+                                       if (inf->chip_config.accl_delay_us >=
+                                                        inf->hal.lpa_tbl[lpa])
+                                               break;
+                               }
                                pm = NVI_PM_ON_CYCLE;
                        } else {
                                pm = NVI_PM_ON;
@@ -750,9 +961,59 @@ static int nvi_pm(struct inv_gyro_state_s *inf, int pm_req)
        }
        if (pm_req > pm)
                pm = pm_req;
-       err = nvi_pm_wr(inf, pm, stby);
-       if (pm_req == NVI_PM_AUTO) {
+       if ((pm != inf->pm) || (pwr_mgmt_2 != inf->hw.pwr_mgmt_2) ||
+                                              (lpa != inf->hw.lposc_clksel)) {
+               nvi_int_enable_wr(inf, false);
+               if (((pwr_mgmt_2 & 0x38) != (inf->hw.pwr_mgmt_2 & 0x38)) ||
+                                                       (pm != NVI_PM_ON_FULL))
+                       inf->gyro_start_ts = 0;
+               switch (pm) {
+               case NVI_PM_OFF_FORCE:
+               case NVI_PM_OFF:
+                       err = nvi_pm_wr(inf, BIT_SLEEP, pwr_mgmt_2, lpa);
+                       err |= nvi_vreg_dis_all(inf);
+                       break;
+
+               case NVI_PM_STDBY:
+                       err = nvi_pm_wr(inf, BIT_SLEEP, pwr_mgmt_2, lpa);
+                       break;
+
+               case NVI_PM_ON_CYCLE:
+                       err = nvi_pm_wr(inf, inf->reg->cycle, pwr_mgmt_2, lpa);
+                       break;
+
+               case NVI_PM_ON:
+                       err = nvi_pm_wr(inf, INV_CLK_INTERNAL, pwr_mgmt_2, lpa);
+                       if (mot_det_dis)
+                               nvi_motion_detect_enable(inf, 0);
+                       break;
+
+               case NVI_PM_ON_FULL:
+                       err = nvi_pm_wr(inf, INV_CLK_PLL, pwr_mgmt_2, lpa);
+                       nvi_motion_detect_enable(inf, 0);
+                       break;
+
+               default:
+                       err = -EINVAL;
+                       break;
+               }
+
+               if (err < 0) {
+                       dev_err(&inf->i2c->dev, "%s requested pm=%d ERR=%d\n",
+                               __func__, pm, err);
+                       pm = NVI_PM_ERR;
+               }
+               inf->pm = pm;
+               dev_dbg(&inf->i2c->dev, "%s pm=%d pwr_mgmt_2=%x lpa=%x\n",
+                       __func__, pm, pwr_mgmt_2, lpa);
+               if (err > 0)
+                       err = 0;
+       }
+       if ((pm_req == NVI_PM_AUTO) && (pm > NVI_PM_STDBY)) {
                nvi_user_ctrl_en_wr(inf, true, true);
+               if ((pm == NVI_PM_ON_FULL) && (!inf->gyro_start_ts))
+                       inf->gyro_start_ts = nvi_ts_ns() +
+                                         inf->chip_config.gyro_start_delay_ns;
                irq = true;
        } else {
                irq = false;
@@ -773,62 +1034,40 @@ static int nvi_pm_init(struct inv_gyro_state_s *inf)
 
        nvi_vreg_init(inf);
        inf->pm = NVI_PM_ERR;
-       inf->stby = 0;
        err = nvi_pm(inf, NVI_PM_ON_FULL);
        return err;
 }
 
-static int nvi_motion_detect_enable(struct inv_gyro_state_s *inf, u8 mot_thr)
-{
-       int err;
-       int err_t = 0;
-
-       if (mot_thr) {
-               err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0);
-               if (err < 0)
-                       err_t |= err;
-               err_t |= nvi_config_wr(inf, 0);
-               if (!inf->hw.mot_dur)
-                       err_t |= nvi_mot_dur_wr(inf, 1);
-               err_t |= nvi_mot_detect_ctrl_wr(inf, inf->chip_config.mot_ctrl);
-               err_t |= nvi_mot_thr_wr(inf, mot_thr);
-               mdelay(5);
-               err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 7);
-               if (err < 0)
-                       err_t |= err;
-               if (!err_t)
-                       inf->mot_enable = true;
-       } else {
-               err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0);
-               if (err < 0)
-                       err_t |= err;
-       }
-       return err_t;
-}
-
 static int nvi_aux_delay(struct inv_gyro_state_s *inf,
                         int port, unsigned int delay_ms)
 {
-       unsigned char val;
-       unsigned char i;
-       unsigned int ms;
+       struct aux_port *ap;
+       u8 val;
+       u8 i;
        unsigned int delay_new;
        int delay_rtn;
-       int err;
 
        if (port != AUX_PORT_BYPASS)
                inf->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_MAX; i++) {
-               if (delay_rtn < inf->aux.port[i].nmp.delay_ms)
-                       delay_rtn = inf->aux.port[i].nmp.delay_ms;
-               if (inf->aux.port[i].hw_en) {
-                       if (delay_new < inf->aux.port[i].nmp.delay_ms)
-                               delay_new = inf->aux.port[i].nmp.delay_ms;
+       for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+               ap = &inf->aux.port[i];
+               if (delay_rtn < ap->nmp.delay_ms)
+                       delay_rtn = ap->nmp.delay_ms;
+               if (inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) {
+                       if (delay_new < ap->nmp.delay_ms)
+                               delay_new = ap->nmp.delay_ms;
                }
        }
+       ap = &inf->aux.port[AUX_PORT_SPECIAL];
+       if (delay_rtn < ap->nmp.delay_ms)
+               delay_rtn = ap->nmp.delay_ms;
+       if (inf->hw.i2c_slv4_ctrl & BIT_SLV_EN) {
+               if (delay_new < ap->nmp.delay_ms)
+                       delay_new = ap->nmp.delay_ms;
+       }
        if (!(inf->hw.user_ctrl & BIT_I2C_MST_EN)) {
                /* delay will execute when re-enabled */
                if (delay_ms)
@@ -838,18 +1077,16 @@ static int nvi_aux_delay(struct inv_gyro_state_s *inf,
        }
 
        /* HW global delay */
-       for (i = 1; i < (BITS_I2C_MST_DLY - 1); i++) {
-               ms = (inf->sample_delay_us - (inf->sample_delay_us /
-                                             (i + 1))) * 1000;
-               if (ms >= delay_new)
-                       break;
-       }
-       inf->aux.delay_hw = i;
-       err = nvi_i2c_slv4_ctrl_wr(inf, inf->aux.port[AUX_PORT_SPECIAL].hw_en);
+       delay_new *= 1000;
+       delay_new /= inf->sample_delay_us;
+       delay_new++;
+       inf->aux.delay_hw = (u8)delay_new;
+       nvi_i2c_slv4_ctrl_wr(inf, (bool)(inf->hw.i2c_slv4_ctrl & BIT_SLV_EN));
        /* HW port delay enable */
        val = BIT_DELAY_ES_SHADOW;
        for (i = 0; i < AUX_PORT_MAX; i++) {
-               if (inf->aux.port[i].nmp.delay_ms)
+               ap = &inf->aux.port[i];
+               if (ap->nmp.delay_ms)
                        val |= (1 << i);
        }
        nvi_i2c_mst_delay_ctrl_wr(inf, val);
@@ -862,10 +1099,13 @@ static int nvi_aux_delay(struct inv_gyro_state_s *inf,
 static int nvi_global_delay(struct inv_gyro_state_s *inf)
 {
        unsigned long delay_us;
-       unsigned long delay_min;
-       unsigned char val;
+       unsigned long delay_us_old;
+       unsigned long fs_hz;
+       u8 dlpf;
+       u8 smplrt_div;
        int i;
-       int err = 0;
+       int err;
+       int err_t = 0;
 
        /* find the fastest polling of all the devices */
        delay_us = -1;
@@ -885,45 +1125,59 @@ static int nvi_global_delay(struct inv_gyro_state_s *inf)
        }
        if (delay_us == -1)
                delay_us = NVI_DELAY_DEFAULT; /* default if nothing found */
-       /* find what the fastest the external devices will let MPU go */
-       delay_min = 0;
-       for (i = 0; i < AUX_PORT_MAX; i++) {
-               if (inf->aux.port[i].enable) {
-                       if (inf->aux.port[i].nmp.delay_ms > delay_min)
-                               delay_min = inf->aux.port[i].nmp.delay_ms;
-               }
-       }
-       if (!delay_min)
-               /* default if nothing found */
-               delay_min = inf->chip_config.min_delay_us;
-       else
-               delay_min *= 1000L; /* ms => us */
        /* set the limits */
-       if (delay_us < delay_min)
-               delay_us = delay_min;
+       if (delay_us < inf->chip_config.min_delay_us)
+               delay_us = inf->chip_config.min_delay_us;
        if (delay_us > MAX_FIFO_RATE)
                delay_us = MAX_FIFO_RATE;
        /* program if new value */
        if (delay_us != inf->sample_delay_us) {
-               dev_dbg(&inf->i2c->dev, "%s %lu\n", __func__, delay_us);
+               if (inf->aux.dbg)
+                       pr_info("%s %lu\n", __func__, delay_us);
+               delay_us_old = inf->sample_delay_us;
                inf->sample_delay_us = delay_us;
-               inf->irq_dur_us = delay_us;
                delay_us <<= 1;
-               for (val = 1; val < ARRAY_SIZE(nvi_lpf_us_tbl); val++) {
-                       if (delay_us < nvi_lpf_us_tbl[val])
+               for (dlpf = 0; dlpf < ARRAY_SIZE(nvi_lpf_us_tbl); dlpf++) {
+                       if (delay_us < nvi_lpf_us_tbl[dlpf])
                                break;
                }
-               err |= nvi_config_wr(inf, val);
-               if (val)
-                       delay_min = 1000;
+               if (dlpf)
+                       fs_hz = 1000;
                else
-                       delay_min = 8000;
-               val = inf->sample_delay_us / delay_min - 1;
-               err |= nvi_smplrt_div_wr(inf, val);
-               inf->last_isr_time = get_time_ns();
+                       fs_hz = 8000;
+               smplrt_div = inf->sample_delay_us / fs_hz - 1;
+               dlpf |= (inf->hw.config & 0xF8);
+               fs_hz = 1000000 / inf->sample_delay_us;
+               if (inf->sample_delay_us < delay_us_old) {
+                       /* go faster */
+                       if (inf->chip_type == INV_MPU3050) {
+                               if (inf->mpu_slave != NULL)
+                                       inf->mpu_slave->set_lpf(inf, fs_hz);
+                               dlpf |= (inf->hw.config & 0xE7);
+                       } else {
+                               nvi_aux_delay(inf, AUX_PORT_BYPASS, 0);
+                       }
+                       err = nvi_config_wr(inf, dlpf);
+                       if (err < 0)
+                               err_t |= err;
+                       err_t |= nvi_smplrt_div_wr(inf, smplrt_div);
+               } else {
+                       /* go slower */
+                       err_t |= nvi_smplrt_div_wr(inf, smplrt_div);
+                       if (inf->chip_type == INV_MPU3050) {
+                               if (inf->mpu_slave != NULL)
+                                       inf->mpu_slave->set_lpf(inf, fs_hz);
+                               dlpf |= (inf->hw.config & 0xE7);
+                               err = nvi_config_wr(inf, dlpf);
+                       } else {
+                               err = nvi_config_wr(inf, dlpf);
+                               nvi_aux_delay(inf, AUX_PORT_BYPASS, 0);
+                       }
+                       if (err < 0)
+                               err_t |= err;
+               }
        }
-       nvi_aux_delay(inf, AUX_PORT_BYPASS, 0);
-       return err;
+       return err_t;
 }
 
 static void nvi_aux_dbg(struct inv_gyro_state_s *inf, char *tag, int val)
@@ -931,7 +1185,7 @@ static void nvi_aux_dbg(struct inv_gyro_state_s *inf, char *tag, int val)
        struct nvi_mpu_port *n;
        struct aux_port *p;
        struct aux_ports *a;
-       unsigned char data[4];
+       u8 data[4];
        int i;
 
        if (!inf->aux.dbg)
@@ -941,20 +1195,22 @@ static void nvi_aux_dbg(struct inv_gyro_state_s *inf, char *tag, int val)
        for (i = 0; i < AUX_PORT_MAX; i++) {
                inv_i2c_read(inf, (REG_I2C_SLV0_ADDR + (i * 3)), 3, data);
                inv_i2c_read(inf, (REG_I2C_SLV0_DO + i), 1, &data[3]);
-               pr_info("PT=%d AD=%x RG=%x CL=%x DO=%x\n",
+               /* 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 = &inf->aux.port[i].nmp;
-               pr_info("PT=%d AD=%x RG=%x CL=%x DO=%x MS=%u US=%lu SB=%x\n",
+               /* 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 = &inf->aux.port[i];
-               pr_info("PT=%d OF=%u EN=%x FE=%x HE=%x HD=%x HV=%x NS=%lld\n",
-                       i, p->ext_data_offset, p->enable, p->fifo_en, p->hw_en,
-                       p->hw_do, p->hw_valid, p->delay_ns);
+               /* PS = port structure */
+               pr_info("PS: P%d OF=%u EN=%x FE=%x HD=%x\n", i,
+                       p->ext_data_offset, p->enable, p->fifo_en, p->hw_do);
        }
        a = &inf->aux;
-       pr_info("EN=%x GE=%x MD=%x GD=%lu DN=%u BE=%x BL=%d SB=%d\n",
-               a->enable, (inf->hw.user_ctrl & BIT_I2C_MST_EN),
+       pr_info("AUX: EN=%x GE=%x MD=%x GD=%lu DN=%u BE=%x BL=%d MX=%d\n",
+               a->enable, (bool)(inf->hw.user_ctrl & BIT_I2C_MST_EN),
                (inf->hw.i2c_slv4_ctrl & BITS_I2C_MST_DLY),
                inf->sample_delay_us, a->ext_data_n,
                (inf->hw.int_pin_cfg & BIT_BYPASS_EN), a->bypass_lock,
@@ -975,27 +1231,24 @@ static void nvi_aux_read(struct inv_gyro_state_s *inf)
                                       (!(inf->hw.user_ctrl & BIT_I2C_MST_EN)))
                return;
 
-       timestamp1 = get_time_ns();
+       timestamp1 = nvi_ts_ns();
        err = inv_i2c_read(inf, REG_EXT_SENS_DATA_00,
                           inf->aux.ext_data_n,
                           (unsigned char *)&inf->aux.ext_data);
        if (err)
                return;
 
-       timestamp2 = get_time_ns();
+       timestamp2 = nvi_ts_ns();
        timestamp1 = timestamp1 + ((timestamp2 - timestamp1) / 2);
        for (i = 0; i < AUX_PORT_SPECIAL; i++) {
                ap = &inf->aux.port[i];
-               if ((ap->nmp.addr & BIT_I2C_READ) &&
+               if ((inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) && (!ap->fifo_en) &&
+                                              (ap->nmp.addr & BIT_I2C_READ) &&
                                                   (ap->nmp.handler != NULL)) {
-                       if ((unsigned long)(timestamp2 - ap->delay_ns)
-                                            >= (ap->nmp.delay_us * 1000)) {
-                               ap->delay_ns = timestamp2;
-                               p = &inf->aux.ext_data[ap->ext_data_offset];
-                               len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
-                               ap->nmp.handler(p, len, timestamp1,
-                                               ap->nmp.ext_driver);
-                       }
+                       p = &inf->aux.ext_data[ap->ext_data_offset];
+                       len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
+                       ap->nmp.handler(p, len, timestamp1,
+                                       ap->nmp.ext_driver);
                }
        }
 }
@@ -1007,8 +1260,8 @@ static void nvi_aux_ext_data_offset(struct inv_gyro_state_s *inf)
 
        offset = 0;
        for (i = 0; i < AUX_PORT_SPECIAL; i++) {
-               if ((inf->aux.port[i].hw_en) && (inf->aux.port[i].nmp.addr &
-                                                BIT_I2C_READ)) {
+               if ((inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) &&
+                                 (inf->aux.port[i].nmp.addr & BIT_I2C_READ)) {
                        inf->aux.port[i].ext_data_offset = offset;
                        offset += (inf->aux.port[i].nmp.ctrl &
                                   BITS_I2C_SLV_CTRL_LEN);
@@ -1023,26 +1276,12 @@ static void nvi_aux_ext_data_offset(struct inv_gyro_state_s *inf)
        return;
 }
 
-static int nvi_aux_port_do(struct inv_gyro_state_s *inf,
-                          int port, u8 data_out)
-{
-       unsigned char reg;
-       int err;
-
-       if (port == AUX_PORT_SPECIAL)
-               reg = REG_I2C_SLV4_DO;
-       else
-               reg = (REG_I2C_SLV0_DO + port);
-       err = inv_i2c_single_write(inf, reg, data_out);
-       return err;
-}
-
 static int nvi_aux_port_data_out(struct inv_gyro_state_s *inf,
                                 int port, u8 data_out)
 {
        int err;
 
-       err = nvi_aux_port_do(inf, port, data_out);
+       err = nvi_i2c_slv_do_wr(inf, port, data_out);
        if (!err) {
                inf->aux.port[port].nmp.data_out = data_out;
                inf->aux.port[port].hw_do = true;
@@ -1058,11 +1297,9 @@ static int nvi_aux_port_wr(struct inv_gyro_state_s *inf, int port)
        int err;
 
        ap = &inf->aux.port[port];
-       err = inv_i2c_single_write(inf, (REG_I2C_SLV0_ADDR + (port * 3)),
-                                  ap->nmp.addr);
-       err |= inv_i2c_single_write(inf, (REG_I2C_SLV0_REG + (port * 3)),
-                                   ap->nmp.reg);
-       err |= nvi_aux_port_do(inf, port, ap->nmp.data_out);
+       err = nvi_i2c_slv_addr_wr(inf, port, ap->nmp.addr);
+       err |= nvi_i2c_slv_reg_wr(inf, port, ap->nmp.reg);
+       err |= nvi_i2c_slv_do_wr(inf, port, ap->nmp.data_out);
        return err;
 }
 
@@ -1070,47 +1307,45 @@ static int nvi_aux_port_en(struct inv_gyro_state_s *inf,
                           int port, bool en)
 {
        struct aux_port *ap;
-       unsigned char reg;
-       unsigned char val;
+       u8 val;
        int err = 0;
 
        inf->aux.ext_data_n = 0;
        ap = &inf->aux.port[port];
-       if ((!ap->hw_valid) && en) {
+       if ((!(inf->hw.i2c_slv_addr[port])) && en) {
                err = nvi_aux_port_wr(inf, port);
-               if (!err) {
-                       ap->hw_valid = true;
+               if (!err)
                        ap->hw_do = true;
-               }
        }
        if ((!ap->hw_do) && en)
                nvi_aux_port_data_out(inf, port, ap->nmp.data_out);
        if (port == AUX_PORT_SPECIAL) {
                err = nvi_i2c_slv4_ctrl_wr(inf, en);
        } else {
-               reg = (REG_I2C_SLV0_CTRL + (port * 3));
                if (en)
                        val = (ap->nmp.ctrl | BIT_SLV_EN);
                else
                        val = 0;
-               err = inv_i2c_single_write(inf, reg, val);
+               err = nvi_i2c_slv_ctrl_wr(inf, port, val);
        }
-       if (!err) {
-               ap->hw_en = en;
+       if (err > 0) {
                nvi_aux_ext_data_offset(inf);
+               err = 0;
        }
        return err;
 }
 
+static int nvi_reset(struct inv_gyro_state_s *inf,
+                    bool reset_fifo, bool reset_i2c);
+
 static int nvi_aux_enable(struct inv_gyro_state_s *inf, bool enable)
 {
        bool en;
        unsigned int i;
-       int err;
+       int err = 0;
 
        if (inf->hw.int_pin_cfg & BIT_BYPASS_EN)
                enable = false;
-
        en = false;
        if (enable) {
                /* global enable is honored only if a port is enabled */
@@ -1120,29 +1355,33 @@ static int nvi_aux_enable(struct inv_gyro_state_s *inf, bool enable)
                                break;
                        }
                }
-               if (en == (inf->hw.user_ctrl & BIT_I2C_MST_EN)) {
+               if (en == (bool)(inf->hw.user_ctrl & BIT_I2C_MST_EN))
                        /* if already on then just update delays */
                        nvi_global_delay(inf);
-               }
        }
        inf->aux.enable = en;
-       if ((inf->hw.user_ctrl & BIT_I2C_MST_EN) == en)
+       if ((bool)(inf->hw.user_ctrl & BIT_I2C_MST_EN) == en) {
+               if (inf->aux.reset_fifo)
+                       nvi_reset(inf, true, false);
                return 0;
+       }
 
        if (en) {
                for (i = 0; i < AUX_PORT_MAX; i++) {
                        if (inf->aux.port[i].enable)
                                err |= nvi_aux_port_en(inf, i, true);
                }
-               nvi_motion_detect_enable(inf, 0);
        } else {
                for (i = 0; i < AUX_PORT_MAX; i++) {
-                       if (inf->aux.port[i].hw_valid)
+                       if (inf->hw.i2c_slv_addr[i])
                                nvi_aux_port_en(inf, i, false);
                }
        }
-       err = nvi_global_delay(inf);
-       err |= nvi_user_ctrl_en_wr(inf, true, en);
+       err |= nvi_global_delay(inf);
+       if (inf->aux.reset_fifo)
+               err |= nvi_reset(inf, true, false);
+       else
+               err |= nvi_user_ctrl_en_wr(inf, true, en);
        return err;
 }
 
@@ -1154,7 +1393,11 @@ static int nvi_aux_port_enable(struct inv_gyro_state_s *inf,
 
        ap = &inf->aux.port[port];
        ap->enable = enable;
-       ap->fifo_en = false;
+       if ((!enable) || (!(ap->nmp.addr & BIT_I2C_READ)))
+               fifo_enable = false;
+       if (ap->fifo_en != fifo_enable)
+               inf->aux.reset_fifo = true;
+       ap->fifo_en = fifo_enable;
        if (enable && (inf->hw.int_pin_cfg & BIT_BYPASS_EN))
                return 0;
 
@@ -1166,58 +1409,51 @@ static int nvi_aux_port_enable(struct inv_gyro_state_s *inf,
 static int nvi_reset(struct inv_gyro_state_s *inf,
                     bool reset_fifo, bool reset_i2c)
 {
+       u8 val;
        unsigned long flags;
-       unsigned char val;
-       int i;
        int err;
 
+       dev_dbg(&inf->i2c->dev, "%s FIFO=%x I2C=%x\n",
+               __func__, reset_fifo, reset_i2c);
        err = nvi_int_enable_wr(inf, false);
        val = 0;
        if (reset_i2c) {
+               inf->aux.reset_i2c = false;
                /* nvi_aux_bypass_enable(inf, false)? */
                err |= nvi_aux_enable(inf, false);
-               for (i = 0; i < AUX_PORT_MAX; i++) {
-                       inf->aux.port[i].hw_valid = false;
-                       inf->aux.port[i].hw_en = false;
-               }
-               inf->aux.need_reset = false;
-               val |= BIT_I2C_MST_RST;
+               val |= inf->reg->i2c_mst_reset;
        }
        if (reset_fifo)
-               val |= BIT_FIFO_RST;
-       err |= nvi_user_ctrl_en_wr(inf, ~reset_fifo, ~reset_i2c);
+               val |= inf->reg->fifo_reset;
+       err |= nvi_user_ctrl_en_wr(inf, !reset_fifo, !reset_i2c);
        val |= inf->hw.user_ctrl;
        err |= nvi_user_ctrl_reset_wr(inf, val);
-       if (reset_fifo) {
+       if (reset_i2c)
+               err |= nvi_aux_enable(inf, true);
+       err |= nvi_user_ctrl_en_wr(inf, true, true);
+       if (reset_fifo && (inf->hw.user_ctrl & BIT_FIFO_EN)) {
                spin_lock_irqsave(&inf->time_stamp_lock, flags);
                kfifo_reset(&inf->trigger.timestamps);
                spin_unlock_irqrestore(&inf->time_stamp_lock, flags);
-               inf->last_isr_time = get_time_ns();
+               inf->fifo_ts = nvi_ts_ns();
+               inf->fifo_reset_3050 = true;
        }
-       if (reset_i2c)
-               err |= nvi_aux_enable(inf, true);
-       else
-               err |= nvi_user_ctrl_en_wr(inf, true, true);
        err |= nvi_int_enable_wr(inf, true);
        return err;
 }
 
 static int nvi_aux_port_free(struct inv_gyro_state_s *inf, int port)
 {
-       bool hw_valid;
-       int err = 0;
-
-       hw_valid = inf->aux.port[port].hw_valid;
        memset(&inf->aux.port[port], 0, sizeof(struct aux_port));
-       if (hw_valid) {
+       if (inf->hw.i2c_slv_addr[port]) {
                nvi_aux_port_wr(inf, port);
                nvi_aux_port_en(inf, port, false);
                nvi_aux_enable(inf, false);
                nvi_aux_enable(inf, true);
                if (port != AUX_PORT_SPECIAL)
-                       inf->aux.need_reset = true;
+                       inf->aux.reset_i2c = true;
        }
-       return err;
+       return 0;
 }
 
 static int nvi_aux_port_alloc(struct inv_gyro_state_s *inf,
@@ -1225,7 +1461,7 @@ static int nvi_aux_port_alloc(struct inv_gyro_state_s *inf,
 {
        int i;
 
-       if (inf->aux.need_reset)
+       if (inf->aux.reset_i2c)
                nvi_reset(inf, false, true);
        if (port < 0) {
                for (i = 0; i < AUX_PORT_SPECIAL; i++) {
@@ -1248,7 +1484,7 @@ static int nvi_aux_port_alloc(struct inv_gyro_state_s *inf,
 
 static int nvi_aux_bypass_enable(struct inv_gyro_state_s *inf, bool enable)
 {
-       unsigned char val;
+       u8 val;
        int err;
 
        if ((bool)(inf->hw.int_pin_cfg & BIT_BYPASS_EN) == enable)
@@ -1272,14 +1508,25 @@ static int nvi_aux_bypass_enable(struct inv_gyro_state_s *inf, bool enable)
 
 static int nvi_aux_bypass_request(struct inv_gyro_state_s *inf, bool enable)
 {
+       s64 ns;
+       s64 to;
        int err = 0;
 
        if ((bool)(inf->hw.int_pin_cfg & BIT_BYPASS_EN) == enable) {
+               inf->aux.bypass_timeout_ns = nvi_ts_ns();
                inf->aux.bypass_lock++;
+               if (!inf->aux.bypass_lock)
+                       dev_err(&inf->i2c->dev, "%s rollover ERR\n", __func__);
        } else {
                if (inf->aux.bypass_lock) {
-                       err = -EBUSY;
-               } else {
+                       ns = nvi_ts_ns() - inf->aux.bypass_timeout_ns;
+                       to = inf->chip_config.bypass_timeout_ms * 1000000;
+                       if (ns > to)
+                               inf->aux.bypass_lock = 0;
+                       else
+                               err = -EBUSY;
+               }
+               if (!inf->aux.bypass_lock) {
                        err = nvi_aux_bypass_enable(inf, enable);
                        if (err)
                                dev_err(&inf->i2c->dev, "%s ERR=%d\n",
@@ -1293,7 +1540,7 @@ static int nvi_aux_bypass_request(struct inv_gyro_state_s *inf, bool enable)
 
 static int nvi_aux_bypass_release(struct inv_gyro_state_s *inf)
 {
-       int err;
+       int err = 0;
 
        if (inf->aux.bypass_lock)
                inf->aux.bypass_lock--;
@@ -1302,7 +1549,7 @@ static int nvi_aux_bypass_release(struct inv_gyro_state_s *inf)
                if (err)
                        dev_err(&inf->i2c->dev, "%s ERR=%d\n", __func__, err);
        }
-       return 0;
+       return err;
 }
 
 static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf,
@@ -1326,6 +1573,7 @@ static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf,
 
        /* enable it */
        inf->aux.port[AUX_PORT_SPECIAL].nmp.delay_ms = 0;
+       inf->aux.port[AUX_PORT_SPECIAL].nmp.delay_us = NVI_DELAY_US_MIN;
        err = nvi_aux_port_enable(inf, AUX_PORT_SPECIAL, true, false);
        if (err) {
                nvi_aux_port_free(inf, AUX_PORT_SPECIAL);
@@ -1335,12 +1583,12 @@ static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf,
 
        /* now turn off all the other ports for fastest response */
        for (i = 0; i < AUX_PORT_SPECIAL; i++) {
-               if (inf->aux.port[i].hw_valid)
+               if (inf->hw.i2c_slv_addr[i])
                        nvi_aux_port_en(inf, i, false);
        }
        /* start reading the results */
-       for (i = 0; i < AUX_DEV_VALID_READ_MAX; i++) {
-               mdelay(1);
+       for (i = 0; i < AUX_DEV_VALID_READ_LOOP_MAX; i++) {
+               mdelay(AUX_DEV_VALID_READ_DELAY_MS);
                val = 0;
                err = inv_i2c_read(inf, REG_I2C_MST_STATUS, 1, &val);
                if (err)
@@ -1352,7 +1600,7 @@ static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf,
        /* these will restore all previously disabled ports */
        nvi_aux_bypass_release(inf);
        nvi_aux_port_free(inf, AUX_PORT_SPECIAL);
-       if (i == AUX_DEV_VALID_READ_MAX)
+       if (i == AUX_DEV_VALID_READ_LOOP_MAX)
                return -ENODEV;
 
        if (val & 0x10) /* NACK */
@@ -1378,7 +1626,7 @@ static int nvi_aux_mpu_call_pre(struct inv_gyro_state_s *inf, int port)
        if ((port < 0) || (port >= AUX_PORT_SPECIAL))
                return -EINVAL;
 
-       if (inf->shutdown)
+       if (inf->shutdown || inf->suspend)
                return -EPERM;
 
        if (!inf->aux.port[port].nmp.addr)
@@ -1410,7 +1658,7 @@ int nvi_mpu_dev_valid(struct nvi_mpu_port *nmp, u8 *data)
                if (inf->aux.dbg)
                        pr_info("%s\n", __func__);
        } else {
-               pr_debug("%s\n", __func__);
+               pr_debug("%s ERR -EAGAIN\n", __func__);
                return -EAGAIN;
        }
 
@@ -1420,7 +1668,7 @@ int nvi_mpu_dev_valid(struct nvi_mpu_port *nmp, u8 *data)
        if ((nmp->addr & BIT_I2C_READ) && (data == NULL))
                return -EINVAL;
 
-       if (inf->shutdown)
+       if (inf->shutdown || inf->suspend)
                return -EPERM;
 
        mutex_lock(&inf->mutex);
@@ -1443,7 +1691,7 @@ int nvi_mpu_port_alloc(struct nvi_mpu_port *nmp)
                if (inf->aux.dbg)
                        pr_info("%s\n", __func__);
        } else {
-               pr_debug("%s\n", __func__);
+               pr_debug("%s ERR -EAGAIN\n", __func__);
                return -EAGAIN;
        }
 
@@ -1453,7 +1701,7 @@ int nvi_mpu_port_alloc(struct nvi_mpu_port *nmp)
        if (!(nmp->ctrl & BITS_I2C_SLV_CTRL_LEN))
                return -EINVAL;
 
-       if (inf->shutdown)
+       if (inf->shutdown || inf->suspend)
                return -EPERM;
 
        mutex_lock(&inf->mutex);
@@ -1475,7 +1723,7 @@ int nvi_mpu_port_free(int port)
                if (inf->aux.dbg)
                        pr_info("%s port %d\n", __func__, port);
        } else {
-               pr_debug("%s port %d\n", __func__, port);
+               pr_debug("%s port %d ERR -EAGAIN\n", __func__, port);
                return -EAGAIN;
        }
 
@@ -1501,7 +1749,8 @@ int nvi_mpu_enable(int port, bool enable, bool fifo_enable)
                if (inf->aux.dbg)
                        pr_info("%s port %d: %x\n", __func__, port, enable);
        } else {
-               pr_debug("%s port %d: %x\n", __func__, port, enable);
+               pr_debug("%s port %d: %x ERR -EAGAIN\n",
+                        __func__, port, enable);
                return -EAGAIN;
        }
 
@@ -1527,7 +1776,8 @@ int nvi_mpu_delay_ms(int port, u8 delay_ms)
                if (inf->aux.dbg)
                        pr_info("%s port %d: %u\n", __func__, port, delay_ms);
        } else {
-               pr_debug("%s port %d: %u\n", __func__, port, delay_ms);
+               pr_debug("%s port %d: %u ERR -EAGAIN\n",
+                        __func__, port, delay_ms);
                return -EAGAIN;
        }
 
@@ -1535,7 +1785,7 @@ int nvi_mpu_delay_ms(int port, u8 delay_ms)
        if (err)
                return err;
 
-       if (inf->aux.port[port].hw_en) {
+       if (inf->hw.i2c_slv_ctrl[port] & BIT_SLV_EN) {
                err = nvi_aux_delay(inf, port, delay_ms);
                nvi_global_delay(inf);
        } else {
@@ -1556,7 +1806,8 @@ int nvi_mpu_delay_us(int port, unsigned long delay_us)
                if (inf->aux.dbg)
                        pr_info("%s port %d: %lu\n", __func__, port, delay_us);
        } else {
-               pr_debug("%s port %d: %lu\n", __func__, port, delay_us);
+               pr_debug("%s port %d: %lu ERR -EAGAIN\n",
+                       __func__, port, delay_us);
                return -EAGAIN;
        }
 
@@ -1565,7 +1816,7 @@ int nvi_mpu_delay_us(int port, unsigned long delay_us)
                return err;
 
        inf->aux.port[port].nmp.delay_us = delay_us;
-       if (inf->aux.port[port].hw_en)
+       if (inf->hw.i2c_slv_ctrl[port] & BIT_SLV_EN)
                err = nvi_global_delay(inf);
        err = nvi_aux_mpu_call_post(inf, "nvi_mpu_delay_us err: ", err);
        return err;
@@ -1575,7 +1826,7 @@ EXPORT_SYMBOL(nvi_mpu_delay_us);
 int nvi_mpu_data_out(int port, u8 data_out)
 {
        struct inv_gyro_state_s *inf;
-       int err = 0;
+       int err;
 
        inf = inf_local;
        if (inf == NULL)
@@ -1585,7 +1836,7 @@ int nvi_mpu_data_out(int port, u8 data_out)
        if (err)
                return err;
 
-       if (inf->aux.port[port].hw_en) {
+       if (inf->hw.i2c_slv_ctrl[port] & BIT_SLV_EN) {
                err = nvi_aux_port_data_out(inf, port, data_out);
        } else {
                inf->aux.port[port].nmp.data_out = data_out;
@@ -1608,11 +1859,11 @@ int nvi_mpu_bypass_request(bool enable)
                if (inf->aux.dbg)
                        pr_info("%s\n", __func__);
        } else {
-               pr_debug("%s\n", __func__);
+               pr_debug("%s ERR -EAGAIN\n", __func__);
                return -EAGAIN;
        }
 
-       if (inf->shutdown)
+       if (inf->shutdown || inf->suspend)
                return -EPERM;
 
        mutex_lock(&inf->mutex);
@@ -1627,7 +1878,6 @@ EXPORT_SYMBOL(nvi_mpu_bypass_request);
 int nvi_mpu_bypass_release(void)
 {
        struct inv_gyro_state_s *inf;
-       int err;
 
        inf = inf_local;
        if (inf != NULL) {
@@ -1635,24 +1885,24 @@ int nvi_mpu_bypass_release(void)
                        pr_info("%s\n", __func__);
        } else {
                pr_debug("%s\n", __func__);
-               return -EAGAIN;
+               return 0;
        }
 
-       if (inf->shutdown)
-               return -EPERM;
+       if (inf->shutdown || inf->suspend)
+               return 0;
 
        mutex_lock(&inf->mutex);
        nvi_pm(inf, NVI_PM_ON);
-       err = nvi_aux_bypass_release(inf);
+       nvi_aux_bypass_release(inf);
        nvi_pm(inf, NVI_PM_AUTO);
-       err = nvi_aux_mpu_call_post(inf, "nvi_mpu_bypass_release err: ", err);
-       return err;
+       nvi_aux_mpu_call_post(inf, "nvi_mpu_bypass_release", 0);
+       return 0;
 }
 EXPORT_SYMBOL(nvi_mpu_bypass_release);
 
 
-static int nvi_gyro_enable(struct inv_gyro_state_s *inf,
-                          unsigned char enable, unsigned char fifo_enable)
+int nvi_gyro_enable(struct inv_gyro_state_s *inf,
+                   unsigned char enable, unsigned char fifo_enable)
 {
        unsigned char enable_old;
        unsigned char fifo_enable_old;
@@ -1670,7 +1920,6 @@ static int nvi_gyro_enable(struct inv_gyro_state_s *inf,
                                                 inf->chip_config.gyro_fsr);
                        if (err < 0)
                                err_t |= err;
-                       nvi_motion_detect_enable(inf, 0);
                }
                nvi_global_delay(inf);
        }
@@ -1680,12 +1929,16 @@ static int nvi_gyro_enable(struct inv_gyro_state_s *inf,
                inf->chip_config.gyro_enable = enable_old;
                inf->chip_config.gyro_fifo_enable = fifo_enable_old;
        }
+       if (inf->chip_config.gyro_enable)
+               inf->chip_config.temp_enable |= NVI_TEMP_GYRO;
+       else
+               inf->chip_config.temp_enable &= ~NVI_TEMP_GYRO;
        err_t |= nvi_pm(inf, NVI_PM_AUTO);
        return err_t;
 }
 
-static int nvi_accl_enable(struct inv_gyro_state_s *inf,
-                          unsigned char enable, unsigned char fifo_enable)
+int nvi_accl_enable(struct inv_gyro_state_s *inf,
+                   unsigned char enable, unsigned char fifo_enable)
 {
        unsigned char enable_old;
        unsigned char fifo_enable_old;
@@ -1698,20 +1951,36 @@ static int nvi_accl_enable(struct inv_gyro_state_s *inf,
        inf->chip_config.accl_enable = enable;
        err_t = nvi_pm(inf, NVI_PM_ON);
        if (enable != enable_old) {
-               if (enable) {
-                       err = nvi_accel_config_wr(inf,
+               if (inf->chip_type == INV_MPU3050) {
+                       if (inf->mpu_slave != NULL) {
+                               if (enable) {
+                                       inf->mpu_slave->resume(inf);
+                                       inf->mpu_slave->set_fs(inf,
+                                                   inf->chip_config.accl_fsr);
+                               } else {
+                                       inf->mpu_slave->suspend(inf);
+                               }
+                       }
+               } else {
+                       if (enable) {
+                               err = nvi_accel_config_wr(inf,
                                                 inf->chip_config.accl_fsr, 0);
-                       if (err < 0)
-                               err_t |= err;
+                               if (err < 0)
+                                       err_t |= err;
+                       }
                }
                nvi_global_delay(inf);
        }
        if (fifo_enable_old != fifo_enable)
-               err_t = nvi_reset(inf, true, false);
+               err_t |= nvi_reset(inf, true, false);
        if (err_t) {
                inf->chip_config.accl_enable = enable_old;
                inf->chip_config.accl_fifo_enable = fifo_enable_old;
        }
+       if (inf->chip_config.accl_enable)
+               inf->chip_config.temp_enable |= NVI_TEMP_ACCL;
+       else
+               inf->chip_config.temp_enable &= ~NVI_TEMP_ACCL;
        err_t |= nvi_pm(inf, NVI_PM_AUTO);
        return err_t;
 }
@@ -1732,15 +2001,11 @@ static ssize_t nvi_gyro_enable_store(struct device *dev,
                return -EINVAL;
 
        if (enable > 7)
-               return -EINVAL;
-
+               enable = 7;
        mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
        if (enable != inf->chip_config.gyro_enable) {
-               if (enable)
-                       fifo_enable = inf->chip_config.gyro_fifo_enable;
-               else
-                       fifo_enable = 0;
+               dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
+               fifo_enable = inf->chip_config.gyro_fifo_enable & enable;
                err = nvi_gyro_enable(inf, enable, fifo_enable);
        }
        mutex_unlock(&inf->mutex);
@@ -1775,16 +2040,14 @@ ssize_t nvi_gyro_fifo_enable_store(struct device *dev,
        if (err)
                return -EINVAL;
 
+       if (fifo_enable > 7)
+               fifo_enable = 7;
        mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
-       enable = inf->chip_config.gyro_enable;
-       if (fifo_enable) {
-               fifo_enable = 1;
-               if (!enable)
-                       enable = 7;
-       }
-       if (fifo_enable != inf->chip_config.gyro_fifo_enable)
+       if (fifo_enable != inf->chip_config.gyro_fifo_enable) {
+               dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
+               enable = inf->chip_config.gyro_enable | fifo_enable;
                err = nvi_gyro_enable(inf, enable, fifo_enable);
+       }
        mutex_unlock(&inf->mutex);
        if (err) {
                dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
@@ -1817,11 +2080,11 @@ static ssize_t inv_gyro_delay_store(struct device *dev,
        if (err)
                return err;
 
-       mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, gyro_delay_us);
        if (gyro_delay_us < NVI_INPUT_GYRO_DELAY_US_MIN)
                gyro_delay_us = NVI_INPUT_GYRO_DELAY_US_MIN;
+       mutex_lock(&inf->mutex);
        if (gyro_delay_us != inf->chip_config.gyro_delay_us) {
+               dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, gyro_delay_us);
                gyro_delay_us_old = inf->chip_config.gyro_delay_us;
                inf->chip_config.gyro_delay_us = gyro_delay_us;
                if (inf->chip_config.gyro_enable) {
@@ -1841,6 +2104,18 @@ static ssize_t inv_gyro_delay_store(struct device *dev,
        return count;
 }
 
+static ssize_t nvi_gyro_delay_show(struct device *dev,
+                                  struct device_attribute *attr, char *buf)
+{
+       struct inv_gyro_state_s *inf;
+
+       inf = dev_get_drvdata(dev);
+       if (inf->chip_config.gyro_enable)
+               return sprintf(buf, "%lu\n", inf->sample_delay_us);
+
+       return sprintf(buf, "%d\n", NVI_INPUT_GYRO_DELAY_US_MIN);
+}
+
 static ssize_t nvi_gyro_resolution_store(struct device *dev,
                                         struct device_attribute *attr,
                                         const char *buf, size_t count)
@@ -1852,7 +2127,7 @@ static ssize_t nvi_gyro_resolution_store(struct device *dev,
        if (kstrtouint(buf, 10, &resolution))
                return -EINVAL;
 
-       dev_dbg(&inf->i2c->dev, "%s %u", __func__, resolution);
+       dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, resolution);
        inf->chip_config.gyro_resolution = resolution;
        return count;
 }
@@ -1872,18 +2147,6 @@ static ssize_t nvi_gyro_resolution_show(struct device *dev,
        return sprintf(buf, "%u\n", resolution);
 }
 
-static ssize_t nvi_gyro_delay_show(struct device *dev,
-                                  struct device_attribute *attr, char *buf)
-{
-       struct inv_gyro_state_s *inf;
-
-       inf = dev_get_drvdata(dev);
-       if (inf->chip_config.gyro_enable)
-               return sprintf(buf, "%lu\n", inf->sample_delay_us);
-
-       return sprintf(buf, "%d\n", NVI_INPUT_GYRO_DELAY_US_MIN);
-}
-
 static ssize_t nvi_gyro_max_range_store(struct device *dev,
                                        struct device_attribute *attr,
                                        const char *buf, size_t count)
@@ -1901,12 +2164,11 @@ static ssize_t nvi_gyro_max_range_store(struct device *dev,
                return -EINVAL;
 
        mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
        if (fsr != inf->chip_config.gyro_fsr) {
+               dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
                if (inf->chip_config.gyro_enable) {
                        err = nvi_gyro_config_wr(inf, fsr);
-                       if (err > 0)
-                               /* reset fifo to purge old data */
+                       if ((err > 0) && (inf->hw.fifo_en & BITS_GYRO_OUT))
                                nvi_reset(inf, true, false);
                }
                if (err >= 0)
@@ -1949,15 +2211,11 @@ static ssize_t nvi_accl_enable_store(struct device *dev,
                return -EINVAL;
 
        if (enable > 7)
-               return -EINVAL;
-
+               enable = 7;
        mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
        if (enable != inf->chip_config.accl_enable) {
-               if (enable)
-                       fifo_enable = inf->chip_config.accl_fifo_enable;
-               else
-                       fifo_enable = 0;
+               dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
+               fifo_enable = inf->chip_config.accl_fifo_enable & enable;
                err = nvi_accl_enable(inf, enable, fifo_enable);
        }
        mutex_unlock(&inf->mutex);
@@ -1970,8 +2228,8 @@ static ssize_t nvi_accl_enable_store(struct device *dev,
        return count;
 }
 
-ssize_t nvi_accl_enable_show(struct device *dev,
-                            struct device_attribute *attr, char *buf)
+static ssize_t nvi_accl_enable_show(struct device *dev,
+                                   struct device_attribute *attr, char *buf)
 {
        struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
 
@@ -1992,16 +2250,14 @@ static ssize_t nvi_accl_fifo_enable_store(struct device *dev,
        if (err)
                return -EINVAL;
 
+       if (fifo_enable > 7)
+               fifo_enable = 7;
        mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
-       enable = inf->chip_config.accl_enable;
-       if (fifo_enable) {
-               fifo_enable = 1;
-               if (!enable)
-                       enable = 7;
-       }
-       if (fifo_enable != inf->chip_config.accl_fifo_enable)
+       if (fifo_enable != inf->chip_config.accl_fifo_enable) {
+               dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
+               enable = inf->chip_config.accl_enable | fifo_enable;
                err = nvi_accl_enable(inf, enable, fifo_enable);
+       }
        mutex_unlock(&inf->mutex);
        if (err) {
                dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
@@ -2012,8 +2268,9 @@ static ssize_t nvi_accl_fifo_enable_store(struct device *dev,
        return count;
 }
 
-ssize_t nvi_accl_fifo_enable_show(struct device *dev,
-                                 struct device_attribute *attr, char *buf)
+static ssize_t nvi_accl_fifo_enable_show(struct device *dev,
+                                        struct device_attribute *attr,
+                                        char *buf)
 {
        struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
 
@@ -2034,14 +2291,16 @@ static ssize_t nvi_accl_delay_store(struct device *dev,
        if (err)
                return err;
 
-       mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, accl_delay_us);
        if (accl_delay_us < NVI_INPUT_ACCL_DELAY_US_MIN)
                accl_delay_us = NVI_INPUT_ACCL_DELAY_US_MIN;
+       mutex_lock(&inf->mutex);
        if (accl_delay_us != inf->chip_config.accl_delay_us) {
+               dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, accl_delay_us);
                accl_delay_us_old = inf->chip_config.accl_delay_us;
                inf->chip_config.accl_delay_us = accl_delay_us;
                if (inf->chip_config.accl_enable) {
+                       if (inf->hw.pwr_mgmt_1 & inf->reg->cycle)
+                               nvi_pm(inf, NVI_PM_ON);
                        err = nvi_global_delay(inf);
                        if (err)
                                inf->chip_config.accl_delay_us =
@@ -2083,7 +2342,7 @@ static ssize_t nvi_accl_resolution_store(struct device *dev,
        if (kstrtouint(buf, 10, &resolution))
                return -EINVAL;
 
-       dev_dbg(&inf->i2c->dev, "%s %u", __func__, resolution);
+       dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, resolution);
        inf->chip_config.accl_resolution = resolution;
        return count;
 }
@@ -2120,12 +2379,21 @@ static ssize_t nvi_accl_max_range_store(struct device *dev,
                return -EINVAL;
 
        mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
        if (fsr != inf->chip_config.accl_fsr) {
+               dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
                if (inf->chip_config.accl_enable) {
-                       err = nvi_accel_config_wr(inf, fsr, 0);
-                       if (err > 0)
-                               /* reset fifo to purge old data */
+                       if (inf->hw.pwr_mgmt_1 & inf->reg->cycle)
+                               nvi_pm(inf, NVI_PM_ON);
+                       if (inf->chip_type == INV_MPU3050) {
+                               if (inf->mpu_slave != NULL) {
+                                       inf->mpu_slave->set_fs(inf, fsr);
+                                       err = 1;
+                               }
+                       } else {
+                               err = nvi_accel_config_wr(inf, fsr, 0);
+                       }
+                       if ((err > 0) && (inf->hw.fifo_en &
+                                         inf->reg->accl_fifo_en))
                                nvi_reset(inf, true, false);
                        nvi_pm(inf, NVI_PM_AUTO);
                }
@@ -2167,8 +2435,8 @@ static ssize_t nvi_lpa_delay_enable_store(struct device *dev,
        if (err)
                return err;
 
-       mutex_lock(&inf->mutex);
        dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, lpa_delay_us);
+       mutex_lock(&inf->mutex);
        inf->chip_config.lpa_delay_us = lpa_delay_us;
        err = nvi_pm(inf, NVI_PM_AUTO);
        mutex_unlock(&inf->mutex);
@@ -2187,6 +2455,49 @@ static ssize_t nvi_lpa_delay_enable_show(struct device *dev,
        return sprintf(buf, "%lu\n", inf->chip_config.lpa_delay_us);
 }
 
+static ssize_t nvi_mot_enable_store(struct device *dev,
+                                   struct device_attribute *attr,
+                                   const char *buf, size_t count)
+{
+       struct inv_gyro_state_s *inf;
+       unsigned char mot_enable;
+       int err;
+
+       inf = dev_get_drvdata(dev);
+       err = kstrtou8(buf, 10, &mot_enable);
+       if (err)
+               return -EINVAL;
+
+       if (mot_enable > NVI_MOT_DBG)
+               return -EINVAL;
+
+       dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_enable);
+       inf->chip_config.mot_enable = mot_enable;
+       if (!mot_enable) {
+               mutex_lock(&inf->mutex);
+               nvi_pm(inf, NVI_PM_ON);
+               err = nvi_motion_detect_enable(inf, 0);
+               err |= nvi_pm(inf, NVI_PM_AUTO);
+               mutex_unlock(&inf->mutex);
+               if (err) {
+                       dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
+                               __func__, mot_enable, err);
+                       return err;
+               }
+       }
+
+       return count;
+}
+
+static ssize_t nvi_mot_enable_show(struct device *dev,
+                                  struct device_attribute *attr, char *buf)
+{
+       struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
+
+       return sprintf(buf, "%x (0=dis 1=en 2=dbg)\n",
+                      inf->chip_config.mot_enable);
+}
+
 static ssize_t nvi_motion_thr_store(struct device *dev,
                                    struct device_attribute *attr,
                                    const char *buf, size_t count)
@@ -2201,10 +2512,15 @@ static ssize_t nvi_motion_thr_store(struct device *dev,
                return -EINVAL;
 
        mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_thr);
-       nvi_pm(inf, NVI_PM_ON);
-       err = nvi_motion_detect_enable(inf, mot_thr);
-       err |= nvi_pm(inf, NVI_PM_AUTO);
+       if (((!inf->chip_config.gyro_enable) && (!inf->aux.enable) &&
+                                               inf->chip_config.mot_enable)) {
+               dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_thr);
+               if (inf->chip_config.mot_enable == NVI_MOT_DBG)
+                       pr_info("%s: %u\n", __func__, mot_thr);
+               nvi_pm(inf, NVI_PM_ON);
+               err = nvi_motion_detect_enable(inf, mot_thr);
+               err |= nvi_pm(inf, NVI_PM_AUTO);
+       }
        mutex_unlock(&inf->mutex);
        if (err) {
                dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
@@ -2222,7 +2538,7 @@ static ssize_t nvi_motion_thr_show(struct device *dev,
        unsigned char mot_thr;
 
        inf = dev_get_drvdata(dev);
-       if (inf->mot_enable)
+       if ((inf->hw.accl_config & 0x07) == 0x07)
                mot_thr = inf->hw.mot_thr;
        else
                mot_thr = 0;
@@ -2307,6 +2623,95 @@ static ssize_t nvi_motion_count_show(struct device *dev,
        return sprintf(buf, "%u\n", inf->chip_config.mot_cnt);
 }
 
+static ssize_t nvi_bypass_timeout_ms_store(struct device *dev,
+                                          struct device_attribute *attr,
+                                          const char *buf, size_t count)
+{
+       struct inv_gyro_state_s *inf;
+       unsigned int bypass_timeout_ms;
+       int err;
+
+       inf = dev_get_drvdata(dev);
+       err = kstrtouint(buf, 10, &bypass_timeout_ms);
+       if (err)
+               return -EINVAL;
+
+       dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, bypass_timeout_ms);
+       inf->chip_config.bypass_timeout_ms = bypass_timeout_ms;
+       return count;
+}
+
+static ssize_t nvi_bypass_timeout_ms_show(struct device *dev,
+                                         struct device_attribute *attr,
+                                         char *buf)
+{
+       struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
+
+       return sprintf(buf, "%u\n", inf->chip_config.bypass_timeout_ms);
+}
+
+static ssize_t nvi_min_delay_us_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+       struct inv_gyro_state_s *inf;
+       unsigned long min_delay_us;
+       int err;
+
+       inf = dev_get_drvdata(dev);
+       err = kstrtoul(buf, 10, &min_delay_us);
+       if (err)
+               return -EINVAL;
+
+       dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, min_delay_us);
+       inf->chip_config.min_delay_us = min_delay_us;
+       return count;
+}
+
+static ssize_t nvi_min_delay_us_show(struct device *dev,
+                                    struct device_attribute *attr, char *buf)
+{
+       struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
+
+       return sprintf(buf, "%lu\n", inf->chip_config.min_delay_us);
+}
+
+static ssize_t nvi_fifo_thr_store(struct device *dev,
+                                 struct device_attribute *attr,
+                                 const char *buf, size_t count)
+{
+       struct inv_gyro_state_s *inf;
+       unsigned int fifo_thr;
+       int err;
+
+       inf = dev_get_drvdata(dev);
+       err = kstrtouint(buf, 10, &fifo_thr);
+       if (err)
+               return -EINVAL;
+
+       dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, fifo_thr);
+       mutex_lock(&inf->mutex);
+       inf->chip_config.fifo_thr = fifo_thr;
+       err = nvi_int_enable_wr(inf, true);
+       mutex_unlock(&inf->mutex);
+       if (err) {
+               dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
+                       __func__, fifo_thr, err);
+               return err;
+       }
+
+       return count;
+}
+
+static ssize_t nvi_fifo_thr_show(struct device *dev,
+                                struct device_attribute *attr, char *buf)
+{
+       struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
+
+       return sprintf(buf, "%u  0=batch_mode 1=disable %u=limit\n",
+                      inf->chip_config.fifo_thr, inf->hal.fifo_size);
+}
+
 static ssize_t nvi_enable_store(struct device *dev,
                                struct device_attribute *attr,
                                const char *buf, size_t count)
@@ -2321,10 +2726,10 @@ static ssize_t nvi_enable_store(struct device *dev,
                return -EINVAL;
 
        mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, enable);
        if (enable)
                enable = 1;
        if (enable != inf->chip_config.enable) {
+               dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, enable);
                inf->chip_config.enable = enable;
                err = nvi_pm(inf, NVI_PM_AUTO);
        }
@@ -2335,8 +2740,8 @@ static ssize_t nvi_enable_store(struct device *dev,
        return count;
 }
 
-ssize_t nvi_enable_show(struct device *dev,
-                       struct device_attribute *attr, char *buf)
+static ssize_t nvi_enable_show(struct device *dev,
+                              struct device_attribute *attr, char *buf)
 {
        struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
 
@@ -2369,7 +2774,7 @@ static ssize_t inv_raw_gyro_show(struct device *dev,
                (signed short)(be16_to_cpup((short *)&data[0])),
                (signed short)(be16_to_cpup((short *)&data[2])),
                (signed short)(be16_to_cpup((short *)&data[4])),
-               get_time_ns());
+               nvi_ts_ns());
 }
 
 /**
@@ -2380,8 +2785,9 @@ static ssize_t inv_raw_accl_show(struct device *dev,
 {
        struct inv_gyro_state_s *st;
        struct inv_reg_map_s *reg;
+       u8 data[6];
+       s16 out[3];
        int result;
-       unsigned char data[6];
 
        st = dev_get_drvdata(dev);
        reg = st->reg;
@@ -2389,19 +2795,117 @@ static ssize_t inv_raw_accl_show(struct device *dev,
                return -EPERM;
 
        result = inv_i2c_read(st, reg->raw_accl, 6, data);
-       if (result) {
-               printk(KERN_ERR "Could not read raw registers.\n");
+       if (result)
                return result;
+
+       if (st->chip_type == INV_MPU3050) {
+               if (st->mpu_slave != NULL) {
+                       if (0 == st->mpu_slave->get_mode(st))
+                               return -EINVAL;
+
+                       st->mpu_slave->combine_data(data, out);
+               } else {
+                       memcpy(out, data, sizeof(data));
+               }
+               return sprintf(buf, "%d %d %d %lld\n",
+                              out[0], out[1], out[2], nvi_ts_ns());
        }
 
        return sprintf(buf, "%d %d %d %lld\n",
-               ((signed short)(be16_to_cpup((short *)&data[0]))*
-                               st->chip_info.multi),
-               ((signed short)(be16_to_cpup((short *)&data[2]))*
-                               st->chip_info.multi),
-               ((signed short)(be16_to_cpup((short *)&data[4]))*
-                               st->chip_info.multi),
-               get_time_ns());
+                      ((signed short)(be16_to_cpup((short *)&data[0])) *
+                       st->chip_info.multi),
+                      ((signed short)(be16_to_cpup((short *)&data[2])) *
+                       st->chip_info.multi),
+                      ((signed short)(be16_to_cpup((short *)&data[4])) *
+                       st->chip_info.multi),
+                      nvi_ts_ns());
+}
+
+static ssize_t nvi_temp_enable_store(struct device *dev,
+                                    struct device_attribute *attr,
+                                    const char *buf, size_t count)
+{
+       struct inv_gyro_state_s *inf;
+       unsigned char enable;
+       int err;
+
+       inf = dev_get_drvdata(dev);
+       err = kstrtou8(buf, 10, &enable);
+       if (err)
+               return -EINVAL;
+
+       if (enable)
+               enable = NVI_TEMP_EN;
+       mutex_lock(&inf->mutex);
+       if (enable != (inf->chip_config.temp_enable & NVI_TEMP_EN)) {
+               dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
+               if (enable)
+                       inf->chip_config.temp_enable |= NVI_TEMP_EN;
+               else
+                       inf->chip_config.temp_enable &= ~NVI_TEMP_EN;
+               err = nvi_pm(inf, NVI_PM_AUTO);
+       }
+       mutex_unlock(&inf->mutex);
+       if (err) {
+               dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
+                       __func__, enable, err);
+               return err;
+       }
+
+       return count;
+}
+
+ssize_t nvi_temp_enable_show(struct device *dev,
+                            struct device_attribute *attr, char *buf)
+{
+       struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
+
+       return sprintf(buf, "%u\n", inf->chip_config.temp_enable);
+}
+
+ssize_t nvi_temp_fifo_enable_store(struct device *dev,
+                                  struct device_attribute *attr,
+                                  const char *buf, size_t count)
+{
+       struct inv_gyro_state_s *inf;
+       unsigned char fifo_enable;
+       unsigned char fifo_enable_old;
+       int err;
+
+       inf = dev_get_drvdata(dev);
+       err = kstrtou8(buf, 10, &fifo_enable);
+       if (err)
+               return -EINVAL;
+
+       if (fifo_enable)
+               fifo_enable = 1;
+       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
+       mutex_lock(&inf->mutex);
+       if (fifo_enable != inf->chip_config.temp_fifo_enable) {
+               fifo_enable_old = inf->chip_config.temp_fifo_enable;
+               inf->chip_config.temp_fifo_enable = fifo_enable;
+               err = nvi_pm(inf, NVI_PM_ON);
+               err |= nvi_reset(inf, true, false);
+               err |= nvi_pm(inf, NVI_PM_AUTO);
+               if (err)
+                       inf->chip_config.temp_fifo_enable = fifo_enable_old;
+       }
+       mutex_unlock(&inf->mutex);
+       if (err) {
+               dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
+                       __func__, fifo_enable, err);
+               return err;
+       }
+
+       return count;
+}
+
+ssize_t nvi_temp_fifo_enable_show(struct device *dev,
+                                 struct device_attribute *attr, char *buf)
+{
+       struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
+
+       return sprintf(buf, "%u\n", inf->chip_config.temp_fifo_enable);
 }
 
 /**
@@ -2516,7 +3020,7 @@ static ssize_t inv_gyro_orientation_show(struct device *dev,
 /**
  * inv_accl_matrix_show() - show orientation matrix
  */
-ssize_t inv_accl_matrix_show(struct device *dev,
+static ssize_t inv_accl_matrix_show(struct device *dev,
                             struct device_attribute *attr, char *buf)
 {
        struct inv_gyro_state_s *st = dev_get_drvdata(dev);
@@ -2531,6 +3035,45 @@ ssize_t inv_accl_matrix_show(struct device *dev,
 }
 
 /**
+ * inv_self_test_show() - self test result. 0 for fail; 1 for success.
+ *                        calling this function will trigger self test
+ *                        and return test result.
+ */
+static ssize_t inv_self_test_show(struct device *dev,
+                                 struct device_attribute *attr, char *buf)
+{
+       int result;
+       int bias[3];
+       struct inv_gyro_state_s *st = dev_get_drvdata(dev);
+
+       if (INV_MPU3050 == st->chip_type) {
+               bias[0] = bias[1] = bias[2] = 0;
+               result = 0;
+       } else {
+               result = inv_hw_self_test(st, bias);
+       }
+       return sprintf(buf, "%d, %d, %d, %d\n",
+               bias[0], bias[1], bias[2], result);
+}
+
+/**
+ * inv_get_accl_bias_show() - show accl bias value
+ */
+static ssize_t inv_get_accl_bias_show(struct device *dev,
+                                     struct device_attribute *attr, char *buf)
+{
+       int result;
+       int bias[3];
+       struct inv_gyro_state_s *st = dev_get_drvdata(dev);
+
+       result = inv_get_accl_bias(st, bias);
+       if (result)
+               return -EINVAL;
+
+       return sprintf(buf, "%d, %d, %d\n", bias[0], bias[1], bias[2]);
+}
+
+/**
  * inv_key_show() -  calling this function will show the key
  *
  */
@@ -2548,17 +3091,53 @@ static ssize_t inv_key_show(struct device *dev, struct device_attribute *attr,
                key[13], key[14], key[15]);
 }
 
+/**
+ *  OBSOLETE
+ */
+static ssize_t inv_power_state_store(struct device *dev,
+                                    struct device_attribute *attr,
+                                    const char *buf, size_t count)
+{
+       struct inv_gyro_state_s *st;
+       unsigned long power_state;
+
+       st = dev_get_drvdata(dev);
+       if (kstrtoul(buf, 10, &power_state))
+               return -EINVAL;
+
+       if (power_state)
+               st->chip_config.is_asleep = 0;
+       else
+               st->chip_config.is_asleep = 1;
+       return count;
+}
+
+/**
+ *  OBSOLETE
+ */
+static ssize_t inv_power_state_show(struct device *dev,
+                                   struct device_attribute *attr, char *buf)
+{
+       struct inv_gyro_state_s *st = dev_get_drvdata(dev);
+
+       if (st->chip_config.is_asleep)
+               return sprintf(buf, "0\n");
+
+       else
+               return sprintf(buf, "1\n");
+}
+
 #if DEBUG_SYSFS_INTERFACE
 static ssize_t nvi_dbg_i2c_addr_store(struct device *dev,
                                      struct device_attribute *attr,
                                      const char *buf, size_t count)
 {
        struct inv_gyro_state_s *inf;
-       unsigned char dbg_i2c_addr;
+       u16 dbg_i2c_addr;
        int err;
 
        inf = dev_get_drvdata(dev);
-       err = kstrtou8(buf, 16, &dbg_i2c_addr);
+       err = kstrtou16(buf, 16, &dbg_i2c_addr);
        if (err)
                return -EINVAL;
 
@@ -2569,12 +3148,17 @@ static ssize_t nvi_dbg_i2c_addr_store(struct device *dev,
 static ssize_t nvi_dbg_i2c_addr_show(struct device *dev,
                                     struct device_attribute *attr, char *buf)
 {
-       struct inv_gyro_state_s *st = dev_get_drvdata(dev);
-       ssize_t bytes_printed = 0;
+       struct inv_gyro_state_s *inf;
+       ssize_t ret = 0;
+       u16 dbg_i2c_addr;
 
-       bytes_printed += sprintf(buf + bytes_printed,
-                                "%#2x\n", st->dbg_i2c_addr);
-       return bytes_printed;
+       inf = dev_get_drvdata(dev);
+       if (inf->dbg_i2c_addr)
+               dbg_i2c_addr = inf->dbg_i2c_addr;
+       else
+               dbg_i2c_addr = inf->i2c->addr;
+       ret += sprintf(buf + ret, "%#2x\n", dbg_i2c_addr);
+       return ret;
 }
 
 static ssize_t nvi_dbg_reg_store(struct device *dev,
@@ -2582,7 +3166,7 @@ static ssize_t nvi_dbg_reg_store(struct device *dev,
                                 const char *buf, size_t count)
 {
        struct inv_gyro_state_s *inf;
-       unsigned char dbg_reg;
+       u8 dbg_reg;
        int err;
 
        inf = dev_get_drvdata(dev);
@@ -2597,11 +3181,12 @@ static ssize_t nvi_dbg_reg_store(struct device *dev,
 static ssize_t nvi_dbg_reg_show(struct device *dev,
                                struct device_attribute *attr, char *buf)
 {
-       struct inv_gyro_state_s *st = dev_get_drvdata(dev);
-       ssize_t bytes_printed = 0;
+       struct inv_gyro_state_s *inf;
+       ssize_t ret = 0;
 
-       bytes_printed += sprintf(buf + bytes_printed, "%#2x\n", st->dbg_reg);
-       return bytes_printed;
+       inf = dev_get_drvdata(dev);
+       ret += sprintf(buf + ret, "%#2x\n", inf->dbg_reg);
+       return ret;
 }
 
 static ssize_t nvi_dbg_dat_store(struct device *dev,
@@ -2609,8 +3194,8 @@ static ssize_t nvi_dbg_dat_store(struct device *dev,
                                 const char *buf, size_t count)
 {
        struct inv_gyro_state_s *inf;
-       unsigned short dbg_i2c_addr;
-       unsigned char dbg_dat;
+       u16 dbg_i2c_addr;
+       u8 dbg_dat;
        int err;
 
        inf = dev_get_drvdata(dev);
@@ -2618,14 +3203,12 @@ static ssize_t nvi_dbg_dat_store(struct device *dev,
        if (err)
                return -EINVAL;
 
-       if (inf->dbg_i2c_addr) {
-               err = inv_i2c_single_write_base(inf, inf->dbg_i2c_addr,
-                                               inf->dbg_reg, dbg_dat);
+       if (inf->dbg_i2c_addr)
                dbg_i2c_addr = inf->dbg_i2c_addr;
-       } else {
-               err = inv_i2c_single_write(inf, inf->dbg_reg, dbg_dat);
+       else
                dbg_i2c_addr = inf->i2c->addr;
-       }
+       err = inv_i2c_single_write_base(inf, dbg_i2c_addr,
+                                       inf->dbg_reg, dbg_dat);
        pr_info("%s dev=%x reg=%x data=%x err=%d\n",
                __func__, dbg_i2c_addr, inf->dbg_reg, dbg_dat, err);
        return count;
@@ -2635,25 +3218,23 @@ static ssize_t nvi_dbg_dat_show(struct device *dev,
                                struct device_attribute *attr, char *buf)
 {
        struct inv_gyro_state_s *inf;
-       unsigned short dbg_i2c_addr;
-       unsigned char data;
-       ssize_t bytes_printed = 0;
+       ssize_t ret = 0;
+       u16 dbg_i2c_addr;
+       u8 dbg_dat = 0;
+       int err;
 
        inf = dev_get_drvdata(dev);
-       if (inf->dbg_i2c_addr) {
-               inv_i2c_read_base(inf, inf->dbg_i2c_addr,
-                                 inf->dbg_reg, 1, &data);
+       if (inf->dbg_i2c_addr)
                dbg_i2c_addr = inf->dbg_i2c_addr;
-       } else {
-               inv_i2c_read(inf, inf->dbg_reg, 1, &data);
+       else
                dbg_i2c_addr = inf->i2c->addr;
-       }
-       bytes_printed += sprintf(buf + bytes_printed, "%#2x:%#2x=%#2x\n",
-                                dbg_i2c_addr, inf->dbg_reg, data);
-       return bytes_printed;
+       err = inv_i2c_read_base(inf, dbg_i2c_addr, inf->dbg_reg, 1, &dbg_dat);
+       ret += sprintf(buf + ret, "%s dev=%x reg=%x data=%x err=%d\n",
+                      __func__, dbg_i2c_addr, inf->dbg_reg, dbg_dat, err);
+       return ret;
 }
 
-static ssize_t nvi_aux_dbg_store(struct device *dev,
+static ssize_t nvi_dbg_aux_store(struct device *dev,
                                 struct device_attribute *attr,
                                 const char *buf, size_t count)
 {
@@ -2675,363 +3256,532 @@ static ssize_t nvi_aux_dbg_store(struct device *dev,
        return count;
 }
 
-static ssize_t nvi_aux_dbg_show(struct device *dev,
+static ssize_t nvi_dbg_aux_show(struct device *dev,
                                struct device_attribute *attr, char *buf)
 {
        struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
 
        return sprintf(buf, "%x\n", inf->aux.dbg);
 }
+#endif /* DEBUG_SYSFS_INTERFACE */
 
-static ssize_t nvi_mot_dbg_store(struct device *dev,
-                                struct device_attribute *attr,
-                                const char *buf, size_t count)
-{
-       struct inv_gyro_state_s *inf;
-       unsigned int mot_dbg;
-       int err;
 
-       inf = dev_get_drvdata(dev);
-       err = kstrtouint(buf, 10, &mot_dbg);
-       if (err)
-               return err;
+static u16 nvi_report_accl(struct inv_gyro_state_s *inf, u8 *data)
+{
+       u16 val[3];
+       u16 buf_index;
 
-       if (mot_dbg)
-               inf->mot_dbg = true;
-       else
-               inf->mot_dbg = false;
-       return count;
+       if (inf->chip_type == INV_MPU3050) {
+               if (inf->mpu_slave != NULL)
+                       inf->mpu_slave->combine_data(data, val);
+       } else {
+               val[0] = ((data[0] << 8) | data[1]) * inf->chip_info.multi;
+               val[1] = ((data[2] << 8) | data[3]) * inf->chip_info.multi;
+               val[2] = ((data[4] << 8) | data[5]) * inf->chip_info.multi;
+       }
+       buf_index = 0;
+       if (!(inf->hw.pwr_mgmt_2 & BIT_STBY_XA)) {
+               input_report_rel(inf->idev, REL_RX, val[0]);
+               buf_index += 2;
+       }
+       if (!(inf->hw.pwr_mgmt_2 & BIT_STBY_YA)) {
+               input_report_rel(inf->idev, REL_RY, val[1]);
+               buf_index += 2;
+       }
+       if (!(inf->hw.pwr_mgmt_2 & BIT_STBY_ZA)) {
+               input_report_rel(inf->idev, REL_RZ, val[2]);
+               buf_index += 2;
+       }
+       return buf_index;
 }
 
-static ssize_t nvi_mot_dbg_show(struct device *dev,
-                               struct device_attribute *attr, char *buf)
+static void nvi_report_temp(struct inv_gyro_state_s *inf, u8 *data, s64 ts)
 {
-       struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
-
-       return sprintf(buf, "%x\n", inf->mot_dbg);
+       mutex_lock(&inf->mutex_temp);
+       inf->temp_val = (data[0] << 8) | data[1];
+       inf->temp_ts = ts;
+       mutex_unlock(&inf->mutex_temp);
 }
-#endif /* DEBUG_SYSFS_INTERFACE */
 
-static ssize_t nvi_min_delay_us_store(struct device *dev,
-                                     struct device_attribute *attr,
-                                     const char *buf, size_t count)
+static u16 nvi_report_gyro(struct inv_gyro_state_s *inf,
+                          u8 *data, u8 mask, s64 ts)
 {
-       struct inv_gyro_state_s *inf;
-       unsigned long min_delay_us;
-       int err;
+       u16 val;
+       u16 buf_index;
+       bool report;
 
-       inf = dev_get_drvdata(dev);
-       err = kstrtoul(buf, 10, &min_delay_us);
-       if (err)
-               return -EINVAL;
+       if (ts < inf->gyro_start_ts)
+               report = false;
+       else
+               report = true;
+       buf_index = 0;
+       if (mask & 4) {
+               if (report && (!(inf->hw.pwr_mgmt_2 & BIT_STBY_XG))) {
+                       val = (data[buf_index] << 8) | data[buf_index + 1];
+                       input_report_rel(inf->idev, REL_X, val);
+               }
+               buf_index += 2;
+       }
+       if (mask & 2) {
+               if (report && (!(inf->hw.pwr_mgmt_2 & BIT_STBY_YG))) {
+                       val = (data[buf_index] << 8) | data[buf_index + 1];
+                       input_report_rel(inf->idev, REL_Y, val);
+               }
+               buf_index += 2;
+       }
+       if (mask & 1) {
+               if (report && (!(inf->hw.pwr_mgmt_2 & BIT_STBY_ZG))) {
+                       val = (data[buf_index] << 8) | data[buf_index + 1];
+                       input_report_rel(inf->idev, REL_Z, val);
+               }
+               buf_index += 2;
+       }
+       return buf_index;
+}
 
-       dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, min_delay_us);
-       inf->chip_config.min_delay_us = min_delay_us;
-       return count;
+static void nvi_sync(struct inv_gyro_state_s *inf, s64 ts)
+{
+       input_report_rel(inf->idev, REL_MISC, (unsigned int)(ts >> 32));
+       input_report_rel(inf->idev, REL_WHEEL,
+                        (unsigned int)(ts & 0xffffffff));
+       input_sync(inf->idev);
 }
 
-static ssize_t nvi_min_delay_us_show(struct device *dev,
-                                    struct device_attribute *attr, char *buf)
+static int nvi_accl_read(struct inv_gyro_state_s *inf)
 {
-       struct inv_gyro_state_s *inf = dev_get_drvdata(dev);
+       u8 data[6];
+       int err;
 
-       return sprintf(buf, "%lu\n", inf->chip_config.min_delay_us);
+       err = inv_i2c_read(inf, inf->reg->raw_accl, 6, data);
+       if (!err)
+               err = nvi_report_accl(inf, data);
+       return err;
 }
 
+static u16 nvi_fifo_read_accl(struct inv_gyro_state_s *inf,
+                             u16 buf_index)
+{
+       if (inf->hw.fifo_en & inf->reg->accl_fifo_en) {
+               nvi_report_accl(inf, &inf->buf[buf_index]);
+#if DEBUG_FIFO_DATA_SPEW
+               pr_info("%s %x %x %x\n", __func__,
+                       (inf->buf[buf_index+0] << 8) | inf->buf[buf_index+1],
+                       (inf->buf[buf_index+2] << 8) | inf->buf[buf_index+3],
+                       (inf->buf[buf_index+4] << 8) | inf->buf[buf_index+5]);
+#endif /* DEBUG_FIFO_DATA_SPEW */
+               buf_index += 6;
+       }
+       return buf_index;
+}
 
-static void inv_report_gyro_accl(struct inv_gyro_state_s *st, s64 t,
-                                unsigned char *data)
+static u16 nvi_fifo_read_gyro(struct inv_gyro_state_s *inf,
+                             u16 buf_index, s64 ts)
 {
-       short x, y, z;
-       int ind;
-       struct inv_chip_config_s *conf;
+       u8 mask;
 
-       conf = &st->chip_config;
-       ind = 0;
-       if (conf->accl_fifo_enable) {
-               x = ((data[ind] << 8)|data[ind + 1])*st->chip_info.multi;
-               y = ((data[ind + 2] << 8)|data[ind + 3])*st->chip_info.multi;
-               z = ((data[ind + 4] << 8)|data[ind + 5])*st->chip_info.multi;
-               if (conf->accl_fifo_enable) {
-                       /*it is possible that accl disabled when dmp is on*/
-                       input_report_rel(st->idev, REL_RX,  x);
-                       input_report_rel(st->idev, REL_RY,  y);
-                       input_report_rel(st->idev, REL_RZ,  z);
-               }
-               ind += 6;
-       }
-       if (conf->gyro_fifo_enable) {
-               x = (data[ind] << 8)     | data[ind + 1];
-               y = (data[ind + 2] << 8) | data[ind + 3];
-               z = (data[ind + 4] << 8) | data[ind + 5];
-               if (conf->gyro_fifo_enable) {
-                       /*it is possible that gyro disabled when dmp is on*/
-                       input_report_rel(st->idev, REL_X,  x);
-                       input_report_rel(st->idev, REL_Y,  y);
-                       input_report_rel(st->idev, REL_Z,  z);
-               }
-               ind += 6;
+       if (inf->hw.fifo_en & BIT_TEMP_FIFO_EN) {
+               nvi_report_temp(inf, &inf->buf[buf_index], ts);
+#if DEBUG_FIFO_DATA_SPEW
+               pr_info("%s %x\n", __func__,
+                       (inf->buf[buf_index+0] << 8) | inf->buf[buf_index+1]);
+#endif /* DEBUG_FIFO_DATA_SPEW */
+               buf_index += 2;
        }
-       if (conf->accl_fifo_enable | conf->gyro_fifo_enable) {
-               input_report_rel(st->idev, REL_MISC, (unsigned int)(t >> 32));
-               input_report_rel(st->idev, REL_WHEEL,
-                       (unsigned int)(t & 0xffffffff));
-               input_sync(st->idev);
+       mask = inf->hw.fifo_en;
+       mask &= (BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
+       mask >>= 4;
+       if (mask) {
+#if DEBUG_FIFO_DATA_SPEW
+               pr_info("%s %x %x %x\n", __func__,
+                       (inf->buf[buf_index+0] << 8) | inf->buf[buf_index+1],
+                       (inf->buf[buf_index+2] << 8) | inf->buf[buf_index+3],
+                       (inf->buf[buf_index+4] << 8) | inf->buf[buf_index+5]);
+#endif /* DEBUG_FIFO_DATA_SPEW */
+               buf_index += nvi_report_gyro(inf, &inf->buf[buf_index],
+                                            mask, ts);
        }
+       return buf_index;
 }
 
-/**
- *  inv_read_fifo() - Transfer data from FIFO to ring buffer.
- */
-static irqreturn_t inv_read_fifo(int irq, void *dev_id)
+static irqreturn_t nvi_irq_thread(int irq, void *dev_id)
 {
-       struct inv_gyro_state_s *st;
-       unsigned char bytes_per_datum;
-       const unsigned short fifo_thresh = 500;
-       int result;
-       unsigned char data[16];
-       unsigned short fifo_count;
+       struct inv_gyro_state_s *inf;
+       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 sync;
+       unsigned int ts_len;
+       unsigned int sample_count;
        unsigned int copied;
-       s64 timestamp;
-       struct inv_reg_map_s *reg;
-
-       st = (struct inv_gyro_state_s *)dev_id;
-       reg = st->reg;
+       unsigned int len;
+       int i;
+       int err;
 
-       timestamp = get_time_ns();
-       if ((!(st->hw.fifo_en & BIT_TEMP_FIFO_EN)) &&
-                                                st->chip_config.gyro_enable) {
-               result = inv_i2c_read(st, st->reg->temperature, 2, data);
-               if (!result) {
-                       mutex_lock(&st->mutex_temp);
-                       st->temp_val = (data[0] << 8) | data[1];
-                       st->temp_ts = timestamp;
-                       mutex_unlock(&st->mutex_temp);
+       inf = (struct inv_gyro_state_s *)dev_id;
+       if (inf->mot_cnt)
+               inf->mot_cnt--;
+       /* if only accelermeter data */
+       if ((inf->hw.pwr_mgmt_1 & inf->reg->cycle) || (inf->hw.int_enable &
+                                                      BIT_MOT_EN)) {
+               if (inf->hw.int_enable & BIT_MOT_EN) {
+                       inf->mot_cnt = inf->chip_config.mot_cnt;
+                       nvi_motion_detect_enable(inf, 0);
+                       nvi_int_enable_wr(inf, true);
+                       if (inf->chip_config.mot_enable == NVI_MOT_DBG)
+                               pr_info("%s motion detect off\n", __func__);
                }
+               ts = nvi_ts_ns();
+               err = nvi_accl_read(inf);
+               if (err < 0)
+                       goto nvi_irq_thread_exit;
+
+               nvi_sync(inf, ts);
+               if (inf->mot_cnt && (inf->chip_config.mot_enable ==
+                                    NVI_MOT_DBG))
+                       pr_info("%s SENDING MOTION DETECT DATA\n", __func__);
+               if (((inf->hw.accl_config & 0x07) == 0x07) && (!inf->mot_cnt))
+                       nvi_int_enable_wr(inf, true);
+               goto nvi_irq_thread_exit;
+       }
+
+       /* handle FIFO disabled data */
+       sync = false;
+       ts = nvi_ts_ns();
+       if (inf->chip_config.accl_enable && (!(inf->hw.fifo_en &
+                                              inf->reg->accl_fifo_en))) {
+               err = nvi_accl_read(inf);
+               if (err > 0)
+                       sync = true;
+       }
+       if (inf->chip_config.temp_enable &&
+                                    (!(inf->hw.fifo_en & BIT_TEMP_FIFO_EN))) {
+               err = inv_i2c_read(inf, inf->reg->temperature, 2, inf->buf);
+               if (!err)
+                       nvi_report_temp(inf, inf->buf, ts);
        }
-       if (st->mot_cnt)
-               st->mot_cnt--;
-       if (st->lpa_enable || (st->hw.int_enable & BIT_MOT_EN)) {
-               if (st->hw.int_enable & BIT_MOT_EN) {
-                       st->mot_cnt = st->chip_config.mot_cnt;
-                       st->mot_enable = false;
-                       nvi_int_enable_wr(st, true);
-                       if (st->mot_dbg)
-                               pr_info("%s motion detect off", __func__);
+       mask = (BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
+       mask &= ~inf->hw.fifo_en;
+       mask >>= 4;
+       if (inf->chip_config.gyro_enable && mask) {
+               buf_index = 0;
+               err = 0;
+               if (mask & 4) {
+                       err = inv_i2c_read(inf, inf->reg->raw_gyro,
+                                          2, &inf->buf[buf_index]);
+                       buf_index = 2;
+               }
+               if (mask & 2) {
+                       err |= inv_i2c_read(inf, inf->reg->raw_gyro + 2,
+                                           2, &inf->buf[buf_index]);
+                       buf_index += 2;
+               }
+               if (mask & 1)
+                       err |= inv_i2c_read(inf, inf->reg->raw_gyro + 4,
+                                           2, &inf->buf[buf_index]);
+               if (!err) {
+                       buf_index = nvi_report_gyro(inf, inf->buf, mask, ts);
+                       if (buf_index)
+                               sync = true;
                }
-               result = inv_i2c_read(st, reg->raw_accl, 6, data);
-               if (result)
-                       goto end_session;
-
-               inv_report_gyro_accl(st, timestamp, data);
-               if (st->mot_enable && st->mot_dbg)
-                       pr_info("%s SENDING MOTION DETECT DATA", __func__);
-               if (st->mot_enable && (!st->mot_cnt))
-                       nvi_int_enable_wr(st, true);
-               goto end_session;
        }
+       if (sync)
+               nvi_sync(inf, ts);
+       nvi_aux_read(inf);
+       if (!inf->fifo_sample_size)
+               goto nvi_irq_thread_exit;
 
-       bytes_per_datum = (st->chip_config.accl_fifo_enable +
-                          st->chip_config.gyro_fifo_enable)*BYTES_PER_SENSOR;
-       fifo_count = 0;
-       if (bytes_per_datum != 0) {
-               result = inv_i2c_read(st, reg->fifo_count_h, 2, data);
-               if (result)
-                       goto end_session;
-
-               fifo_count = (data[0] << 8) + data[1];
-               if (fifo_count < bytes_per_datum)
-                       goto end_session;
-
-               if (fifo_count%2)
-                       goto flush_fifo;
-
-               if (fifo_count > fifo_thresh)
-                       goto flush_fifo;
+       /* handle FIFO enabled data */
+       fifo_sample_size = inf->fifo_sample_size;
+       if (!fifo_sample_size)
+               goto nvi_irq_thread_exit;
 
-               /* Timestamp mismatch. */
-               if (kfifo_len(&st->trigger.timestamps) < (fifo_count /
-                                                         bytes_per_datum))
-                       goto flush_fifo;
-
-               if (kfifo_len(&st->trigger.timestamps) > (fifo_count /
-                                                         bytes_per_datum +
-                                                         TIME_STAMP_TOR))
-                       goto flush_fifo;
+       /* must get IRQ timestamps first for timestamp best-fit algorithm */
+       ts_len = kfifo_len(&inf->trigger.timestamps);
+       err = inv_i2c_read(inf, inf->reg->fifo_count_h, 2, inf->buf);
+       if (err)
+               goto nvi_irq_thread_exit;
+
+       fifo_count = be16_to_cpup((__be16 *)(&inf->buf));
+       /* FIFO threshold */
+       if (inf->chip_config.fifo_thr > fifo_sample_size) {
+               if (fifo_count > inf->chip_config.fifo_thr) {
+                       dev_dbg(&inf->i2c->dev, "FIFO threshold exceeded\n");
+                       goto nvi_irq_thread_exit_reset;
+               }
        }
 
-       if (bytes_per_datum == 0) {
-               result = kfifo_to_user(&st->trigger.timestamps,
-                       &timestamp, sizeof(timestamp), &copied);
-               if (result)
-                       goto flush_fifo;
+       fifo_align = fifo_count % fifo_sample_size;
+       if (fifo_count < fifo_sample_size + fifo_align)
+               goto nvi_irq_thread_exit;
+
+       if (inf->chip_type == INV_MPU3050) {
+               /* FIFO HW BUG WAR:
+                * The MPU3050 will fire an IRQ on incomplete sampling of data
+                * to the FIFO causing misalignment of data.  The WAR is to
+                * simply wait for the next IRQ when this misalignment problem
+                * usually works itself out with the next data sample.
+                * The safety net is a FIFO reset, should the problem not work
+                * itself out by the time the FIFO threshold is reached.
+                */
+               if (inf->fifo_reset_3050) {
+                       if (fifo_align)
+                               goto nvi_irq_thread_exit;
+               } else {
+                       if (fifo_align != 2)
+                               goto nvi_irq_thread_exit;
+               }
        }
 
-       while ((bytes_per_datum != 0) && (fifo_count >= bytes_per_datum)) {
-               result = inv_i2c_read(st, reg->fifo_r_w, bytes_per_datum,
-                       data);
-               if (result)
-                       goto flush_fifo;
+       ts = inf->fifo_ts;
+       delay = inf->sample_delay_us * 1000;
+       sample_count = (fifo_count / fifo_sample_size);
+       /* Ideally sample_count >= ts_len. If not, pull excess timestamps */
+       if (sample_count < ts_len) {
+               len = ts_len - sample_count;
+               for (i = 0; i < len; i++) {
+                       err = kfifo_to_user(&inf->trigger.timestamps,
+                                           &ts_irq, sizeof(ts_irq), &copied);
+                       if (err)
+                               goto nvi_irq_thread_exit_reset;
 
-               result = kfifo_to_user(&st->trigger.timestamps,
-                       &timestamp, sizeof(timestamp), &copied);
-               if (result)
-                       goto flush_fifo;
+                       ts_len--;
+               }
+               ts = ts_irq + delay;
+               dev_dbg(&inf->i2c->dev, "%s SYNC FIFO to TS %lld\n",
+                       __func__, ts);
+       }
+#if DEBUG_FIFO_DATA_SPEW
+       pr_info("fifo_count=%u sample_size=%u fifo_align=%u sample_count=%u\n",
+               fifo_count, fifo_sample_size, fifo_align, sample_count);
+#endif /* DEBUG_FIFO_DATA_SPEW */
+       fifo_rd_n = 0;
+       buf_index = 0;
+       while (sample_count) {
+               if (buf_index >= fifo_rd_n) {
+                       fifo_rd_n = (ARRAY_SIZE(inf->buf) - fifo_align) /
+                                                             fifo_sample_size;
+                       if (sample_count < fifo_rd_n)
+                               fifo_rd_n = sample_count;
+                       fifo_rd_n *= fifo_sample_size;
+                       fifo_rd_n += fifo_align;
+                       if (inf->chip_type == INV_MPU3050)
+                               fifo_rd_n -= 2; /* FIFO_FOOTER */
+                       err = inv_i2c_read(inf, inf->reg->fifo_r_w,
+                                          fifo_rd_n, inf->buf);
+                       if (err)
+                               goto nvi_irq_thread_exit;
 
-               inv_report_gyro_accl(st, timestamp, data);
-               fifo_count -= bytes_per_datum;
-       }
+                       buf_index = fifo_align;
+                       if (inf->chip_type == INV_MPU3050) {
+                               if (inf->fifo_reset_3050) {
+                                       inf->fifo_reset_3050 = false;
+                                       fifo_align += 2;
+                               }
+                       }
+               }
+               if (ts_len) {
+                       len = ts_len;
+                       for (i = 0; i < len; i++) {
+                               err = kfifo_out_peek(&inf->trigger.timestamps,
+                                                    &ts_irq, 1);
+                               if (err != 1)
+                                       goto nvi_irq_thread_exit_reset;
+
+                               if (ts < (ts_irq - delay))
+                                       break;
 
-       nvi_aux_read(st);
+                               err = kfifo_to_user(&inf->trigger.timestamps,
+                                                   &ts_irq, sizeof(ts_irq),
+                                                   &copied);
+                               if (err)
+                                       goto nvi_irq_thread_exit_reset;
 
-end_session:
+                               ts_len--;
+                               if (ts < (ts_irq + delay)) {
+                                       ts = ts_irq;
+                                       break;
+                               }
+                       }
+               }
+               if (ts != ts_irq)
+                       dev_dbg(&inf->i2c->dev, "%s NOSYNC TS=%lld IRQ=%lld\n",
+                               __func__, ts, ts_irq);
+               if (inf->chip_type == INV_MPU3050) {
+                       buf_index = nvi_fifo_read_gyro(inf, buf_index, ts);
+                       buf_index = nvi_fifo_read_accl(inf, buf_index);
+                       buf_index += 2; /* FIFO_FOOTER */
+               } else {
+                       buf_index = nvi_fifo_read_accl(inf, buf_index);
+                       buf_index = nvi_fifo_read_gyro(inf, buf_index, ts);
+               }
+               nvi_sync(inf, ts);
+               for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+                       ap = &inf->aux.port[i];
+                       if ((inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) &&
+                                   (inf->hw.i2c_slv_addr[i] & BIT_I2C_READ) &&
+                                                                ap->fifo_en) {
+                               len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN;
+                               if (ap->nmp.handler != NULL)
+                                       ap->nmp.handler(&inf->buf[buf_index],
+                                                       len, ts,
+                                                       ap->nmp.ext_driver);
+                               buf_index += len;
+                       }
+               }
+               ts += delay;
+               sample_count--;
+       }
+       inf->fifo_ts = ts;
+nvi_irq_thread_exit:
        return IRQ_HANDLED;
 
-flush_fifo:
-       /* Flush HW and SW FIFOs. */
-       nvi_reset(st, true, false);
+nvi_irq_thread_exit_reset:
+       dev_dbg(&inf->i2c->dev, "%s fifo_count=%u fifo_sample_size=%u\n",
+               __func__, fifo_count, fifo_sample_size);
+       nvi_reset(inf, true, false);
        return IRQ_HANDLED;
 }
 
-/**
- *  inv_irq_handler() - Cache a timestamp at each data ready interrupt.
- */
-static irqreturn_t inv_irq_handler(int irq, void *dev_id)
+static irqreturn_t nvi_irq_handler(int irq, void *dev_id)
 {
-       struct inv_gyro_state_s *st;
+       struct inv_gyro_state_s *inf;
        long long timestamp;
-       int result, catch_up;
-       unsigned int time_since_last_irq;
-
-       st = (struct inv_gyro_state_s *)dev_id;
-       timestamp = get_time_ns();
-       time_since_last_irq = ((unsigned int)(timestamp - st->last_isr_time)) /
-                             ONE_K_HZ;
-       spin_lock(&st->time_stamp_lock);
-       catch_up = 0;
-       while ((time_since_last_irq > st->irq_dur_us*2) &&
-              (catch_up < MAX_CATCH_UP) && (!st->lpa_enable)) {
-               st->last_isr_time += st->irq_dur_us * ONE_K_HZ;
-               result = kfifo_in(&st->trigger.timestamps,
-                                 &st->last_isr_time, 1);
-               time_since_last_irq = ((unsigned int)(timestamp -
-                                       st->last_isr_time)) / ONE_K_HZ;
-               catch_up++;
-       }
-       result = kfifo_in(&st->trigger.timestamps, &timestamp, 1);
-       st->last_isr_time = timestamp;
-       spin_unlock(&st->time_stamp_lock);
-       return IRQ_WAKE_THREAD;
-}
-
-static int inv_pm(struct inv_gyro_state_s *inf, int pm_req)
-{
-       int err;
 
-       if (inf->nvi) {
-               err = nvi_pm(inf, pm_req);
-       } else {
-               if ((pm_req > NVI_PM_OFF) || (pm_req == NVI_PM_AUTO))
-                       err = set_power_mpu3050(inf, 1);
-               else
-                       err = set_power_mpu3050(inf, 0);
+       inf = (struct inv_gyro_state_s *)dev_id;
+       spin_lock(&inf->time_stamp_lock);
+       if (inf->hw.user_ctrl & BIT_FIFO_EN) {
+               timestamp = nvi_ts_ns();
+               kfifo_in(&inf->trigger.timestamps, &timestamp, 1);
        }
-       return err;
+       spin_unlock(&inf->time_stamp_lock);
+       return IRQ_WAKE_THREAD;
 }
 
 #ifdef CONFIG_PM
-static int inv_suspend(struct device *dev)
+static int nvi_suspend(struct device *dev)
 {
-       int result;
-       struct inv_gyro_state_s *st = dev_get_drvdata(dev);
+       struct inv_gyro_state_s *inf;
+       int err;
 
-       result = inv_pm(st, NVI_PM_OFF_FORCE);
-       if (result)
+       inf = dev_get_drvdata(dev);
+       inf->suspend = true;
+       err = nvi_pm(inf, NVI_PM_OFF_FORCE);
+       if (err)
                dev_err(dev, "%s ERR\n", __func__);
-       return result;
+       dev_info(dev, "%s done\n", __func__);
+       return 0;
 }
 
-static int inv_resume(struct device *dev)
+static int nvi_resume(struct device *dev)
 {
-       int result;
-       struct inv_gyro_state_s *st = dev_get_drvdata(dev);
+       struct inv_gyro_state_s *inf;
 
-       result = inv_pm(st, NVI_PM_AUTO);
-       if (result)
-               dev_err(dev, "%s ERR\n", __func__);
-       return result;
+       inf = dev_get_drvdata(dev);
+       inf->suspend = false;
+       dev_info(dev, "%s done\n", __func__);
+       return 0;
 }
 
-static const struct dev_pm_ops inv_pm_ops = {
-       .suspend = inv_suspend,
-       .resume = inv_resume,
+static const struct dev_pm_ops nvi_pm_ops = {
+       .suspend = nvi_suspend,
+       .resume = nvi_resume,
 };
-#endif
+#endif /* CONFIG_PM */
 
-static DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_gyro_enable_show, nvi_gyro_enable_store);
-static DEVICE_ATTR(gyro_fifo_enable, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(gyro_fifo_enable, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_gyro_fifo_enable_show, nvi_gyro_fifo_enable_store);
-static DEVICE_ATTR(gyro_delay, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(gyro_delay, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_gyro_delay_show, inv_gyro_delay_store);
-static DEVICE_ATTR(gyro_resolution, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(gyro_resolution, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_gyro_resolution_show, nvi_gyro_resolution_store);
-static DEVICE_ATTR(gyro_max_range, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(gyro_max_range, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_gyro_max_range_show, nvi_gyro_max_range_store);
 static DEVICE_ATTR(gyro_orientation, S_IRUGO,
                   inv_gyro_orientation_show, NULL);
 static DEVICE_ATTR(raw_gyro, S_IRUGO,
                   inv_raw_gyro_show, NULL);
-static DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_accl_enable_show, nvi_accl_enable_store);
-static DEVICE_ATTR(accl_fifo_enable, S_IRUGO | S_IWUSR,
+static DEVICE_ATTR(accl_fifo_enable, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_accl_fifo_enable_show, nvi_accl_fifo_enable_store);
-static DEVICE_ATTR(accl_delay, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(accl_delay, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_accl_delay_show, nvi_accl_delay_store);
-static DEVICE_ATTR(accl_resolution, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(accl_resolution, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_accl_resolution_show, nvi_accl_resolution_store);
-static DEVICE_ATTR(accl_max_range, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(accl_max_range, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_accl_max_range_show, nvi_accl_max_range_store);
 static DEVICE_ATTR(accl_orientation, S_IRUGO,
                   inv_accl_matrix_show, NULL);
 static DEVICE_ATTR(raw_accl, S_IRUGO,
                   inv_raw_accl_show, NULL);
-static DEVICE_ATTR(lpa_delay, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(accl_bias, S_IRUGO,
+                  inv_get_accl_bias_show, NULL);
+static DEVICE_ATTR(lpa_delay, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_lpa_delay_enable_show, nvi_lpa_delay_enable_store);
-static DEVICE_ATTR(motion_threshold, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(motion_enable, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_mot_enable_show, nvi_mot_enable_store);
+static DEVICE_ATTR(motion_threshold, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_motion_thr_show, nvi_motion_thr_store);
-static DEVICE_ATTR(motion_duration, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(motion_duration, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_motion_dur_show, nvi_motion_dur_store);
-static DEVICE_ATTR(motion_count, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(motion_count, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_motion_count_show, nvi_motion_count_store);
-static DEVICE_ATTR(motion_control, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(motion_control, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_motion_ctrl_show, nvi_motion_ctrl_store);
+static DEVICE_ATTR(temp_enable, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_temp_enable_show, nvi_temp_enable_store);
+static DEVICE_ATTR(temp_fifo_enable, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_temp_fifo_enable_show, nvi_temp_fifo_enable_store);
 static DEVICE_ATTR(temp_scale, S_IRUGO,
                   inv_temp_scale_show, NULL);
 static DEVICE_ATTR(temp_offset, S_IRUGO,
                   inv_temp_offset_show, NULL);
 static DEVICE_ATTR(temperature, S_IRUGO,
                   inv_temperature_show, NULL);
-static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(bypass_timeout_ms, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_bypass_timeout_ms_show, nvi_bypass_timeout_ms_store);
+static DEVICE_ATTR(min_delay_us, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_min_delay_us_show, nvi_min_delay_us_store);
+static DEVICE_ATTR(fifo_threshold, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_fifo_thr_show, nvi_fifo_thr_store);
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_enable_show, nvi_enable_store);
-static DEVICE_ATTR(dbg_reg, S_IRUGO | S_IWUSR | S_IWOTH,
+static DEVICE_ATTR(self_test, S_IRUGO,
+                  inv_self_test_show, NULL);
+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_IWOTH,
+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_IWOTH,
+static DEVICE_ATTR(dbg_i2c_addr, S_IRUGO | S_IWUSR | S_IWGRP,
                   nvi_dbg_i2c_addr_show, nvi_dbg_i2c_addr_store);
-static DEVICE_ATTR(aux_dbg, S_IRUGO | S_IWUSR | S_IWOTH,
-                  nvi_aux_dbg_show, nvi_aux_dbg_store);
-static DEVICE_ATTR(mot_dbg, S_IRUGO | S_IWUSR | S_IWOTH,
-                  nvi_mot_dbg_show, nvi_mot_dbg_store);
-static DEVICE_ATTR(key, S_IRUGO | S_IWUSR,
-                  inv_key_show, inv_key_store);
+static DEVICE_ATTR(dbg_aux, S_IRUGO | S_IWUSR | S_IWGRP,
+                  nvi_dbg_aux_show, nvi_dbg_aux_store);
 static DEVICE_ATTR(reg_dump, S_IRUGO,
                   inv_reg_dump_show, NULL);
-static DEVICE_ATTR(min_delay_us, S_IRUGO | S_IWUSR | S_IWOTH,
-                  nvi_min_delay_us_show, nvi_min_delay_us_store);
+static DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR | S_IWGRP,
+                  inv_power_state_show, inv_power_state_store);
+static DEVICE_ATTR(key, S_IRUGO | S_IWUSR | S_IWGRP,
+                  inv_key_show, inv_key_store);
 
 static struct device_attribute *inv_attributes[] = {
+       &dev_attr_accl_enable,
+       &dev_attr_accl_fifo_enable,
+       &dev_attr_accl_delay,
+       &dev_attr_accl_max_range,
+       &dev_attr_accl_orientation,
+       &dev_attr_raw_accl,
+       &dev_attr_gyro_enable,
+       &dev_attr_gyro_fifo_enable,
+       &dev_attr_gyro_max_range,
        &dev_attr_gyro_delay,
        &dev_attr_gyro_orientation,
        &dev_attr_raw_gyro,
@@ -3039,6 +3789,9 @@ static struct device_attribute *inv_attributes[] = {
        &dev_attr_temp_offset,
        &dev_attr_temperature,
        &dev_attr_reg_dump,
+       &dev_attr_self_test,
+       &dev_attr_enable,
+       &dev_attr_power_state,
        &dev_attr_key,
 #if DEBUG_SYSFS_INTERFACE
        &dev_attr_dbg_reg,
@@ -3049,108 +3802,32 @@ static struct device_attribute *inv_attributes[] = {
 };
 
 static struct device_attribute *inv_mpu6050_attributes[] = {
-       &dev_attr_gyro_enable,
-       &dev_attr_gyro_fifo_enable,
-       &dev_attr_gyro_max_range,
        &dev_attr_gyro_resolution,
-       &dev_attr_accl_enable,
-       &dev_attr_accl_fifo_enable,
-       &dev_attr_accl_delay,
-       &dev_attr_accl_max_range,
+       &dev_attr_temp_enable,
+       &dev_attr_temp_fifo_enable,
        &dev_attr_accl_resolution,
-       &dev_attr_accl_orientation,
-       &dev_attr_raw_accl,
+       &dev_attr_accl_bias,
        &dev_attr_lpa_delay,
+       &dev_attr_motion_enable,
        &dev_attr_motion_threshold,
        &dev_attr_motion_duration,
        &dev_attr_motion_count,
        &dev_attr_motion_control,
-       &dev_attr_enable,
-#if DEBUG_SYSFS_INTERFACE
+       &dev_attr_bypass_timeout_ms,
        &dev_attr_min_delay_us,
-       &dev_attr_aux_dbg,
-       &dev_attr_mot_dbg,
+       &dev_attr_fifo_threshold,
+#if DEBUG_SYSFS_INTERFACE
+       &dev_attr_dbg_aux,
 #endif /* DEBUG_SYSFS_INTERFACE */
        NULL
 };
 
-static int inv_check_chip_type(struct inv_gyro_state_s *st,
-                              const struct i2c_device_id *id)
-{
-       struct inv_reg_map_s *reg;
-       int result = 0;
-
-       reg = st->reg;
-       st->mpu_slave = NULL;
-       if (!strcmp(id->name, "itg3500")) {
-               st->chip_type = INV_ITG3500;
-               st->nvi = false;
-       } else if (!strcmp(id->name, "mpu3050")) {
-               st->chip_type = INV_MPU3050;
-               inv_setup_reg_mpu3050(reg);
-               st->nvi = false;
-       } else if (!strcmp(id->name, "mpu6050")) {
-               st->chip_type = INV_MPU6050;
-               st->nvi = true;
-       } else if (!strcmp(id->name, "mpu9150")) {
-               st->chip_type = INV_MPU9150;
-               st->nvi = true;
-       }
-       if (SECONDARY_SLAVE_TYPE_ACCEL == st->plat_data.sec_slave_type) {
-               if (st->plat_data.sec_slave_id == ACCEL_ID_KXTF9)
-                       inv_register_kxtf9_slave(st);
-       }
-       if (st->nvi) {
-               nvi_pm_init(st);
-       } else {
-               nvi_vreg_init(st);
-               /*reset register to power up default*/
-               result = inv_i2c_single_write(st, reg->pwr_mgmt_1, 0);
-               result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_RESET);
-               if (!result)
-                       mdelay(POWER_UP_TIME);
-       }
-       return result;
-}
-
-/**
- *  inv_init_config() - Initialize hardware, disable FIFO.
- *  @st:       Device driver instance.
- *  Initial configuration:
- *  FSR: +/- 2000DPS
- *  DLPF: 42Hz
- *  FIFO rate: 50Hz
- *  Clock source: Gyro PLL
- */
-static int inv_init_config(struct inv_gyro_state_s *st)
-{
-       st->chip_config.min_delay_us = MIN_FIFO_RATE;
-       st->chip_config.lpf = INV_FILTER_42HZ;
-       st->chip_config.gyro_enable = 0;
-       st->chip_config.gyro_fifo_enable = 0;
-       st->chip_config.gyro_fsr = INV_FSR_2000DPS;
-       st->chip_config.accl_enable = 0;
-       st->chip_config.accl_fifo_enable = 0;
-       st->chip_config.accl_fsr = INV_FS_02G;
-       st->chip_config.mot_dur = 1;
-       st->chip_config.mot_ctrl = 1;
-       st->chip_config.mot_cnt = 10;
-       st->irq_dur_us = 20 * ONE_K_HZ;
-       st->chip_config.fifo_rate = 50;
-       st->chip_config.enable = 0;
-       st->chip_config.dmp_on = 0;
-       st->chip_config.firmware_loaded = 0;
-       st->chip_config.prog_start_addr = DMP_START_ADDR;
-       return 0;
-}
-
 static void inv_input_close(struct input_dev *d)
 {
-       struct inv_gyro_state_s *st;
+       struct inv_gyro_state_s *inf;
 
-       st = input_get_drvdata(d);
-       inv_pm(st, NVI_PM_OFF_FORCE);
-       nvi_vreg_exit(st);
+       inf = input_get_drvdata(d);
+       nvi_pm_exit(inf);
 }
 
 /**
@@ -3290,13 +3967,8 @@ static int create_sysfs_interfaces(struct inv_gyro_state_s *st)
        if (result < 0)
                goto exit_remove_device_attributes;
 
-       if (INV_MPU3050 == st->chip_type) {
-               result = inv_mpu3050_create_sysfs(st);
-               if (result)
-                       goto exit_remove_bin_file;
-
+       if (INV_MPU3050 == st->chip_type)
                return 0;
-       }
 
        result = create_device_attributes(st->inv_dev, inv_mpu6050_attributes);
        if (result < 0)
@@ -3325,14 +3997,138 @@ static void remove_sysfs_interfaces(struct inv_gyro_state_s *st)
                sysfs_remove_bin_file(&st->inv_dev->kobj, &dmp_firmware);
        if ((INV_ITG3500 != st->chip_type) && (INV_MPU3050 != st->chip_type))
                remove_device_attributes(st->inv_dev, inv_mpu6050_attributes);
-       if (INV_MPU3050 == st->chip_type)
-               inv_mpu3050_remove_sysfs(st);
        device_destroy(st->inv_class, inv_device_dev_t);
        class_destroy(st->inv_class);
        st->inv_dev = NULL;
        st->inv_class = NULL;
 }
 
+static void nvi_init_config(struct inv_gyro_state_s *inf)
+{
+       inf->chip_config.bypass_timeout_ms = NVI_BYPASS_TIMEOUT_MS;
+       inf->chip_config.min_delay_us = NVI_DELAY_US_MIN;
+       inf->chip_config.fifo_thr = 1;
+       inf->chip_config.temp_fifo_enable = 1;
+       inf->chip_config.lpf = INV_FILTER_42HZ;
+       inf->chip_config.gyro_enable = 0;
+       inf->chip_config.gyro_fifo_enable = 0;
+       inf->chip_config.gyro_fsr = INV_FSR_2000DPS;
+       inf->chip_config.gyro_start_delay_ns = 100000000;
+       inf->chip_config.accl_enable = 0;
+       inf->chip_config.accl_fifo_enable = 0;
+       inf->chip_config.accl_fsr = INV_FS_02G;
+       inf->chip_config.mot_enable = NVI_MOT_DIS;
+       inf->chip_config.mot_dur = 1;
+       inf->chip_config.mot_ctrl = 1;
+       inf->chip_config.mot_cnt = 10;
+       inf->chip_config.enable = 0;
+       inf->chip_config.prog_start_addr = DMP_START_ADDR;
+}
+
+static int nvi_dev_init(struct inv_gyro_state_s *inf,
+                       const struct i2c_device_id *id)
+{
+       u8 dev_id;
+       u8 val;
+       int err = 0;
+
+       dev_id = 0;
+       if (!strcmp(id->name, "itg3500")) {
+               inf->chip_type = INV_ITG3500;
+       } else if (!strcmp(id->name, "mpu3050")) {
+               inf->chip_type = INV_MPU3050;
+               inv_setup_reg_mpu3050(inf->reg);
+       } else if (!strcmp(id->name, "mpu6050")) {
+               inf->chip_type = INV_MPU6050;
+               dev_id = MPU6050_ID;
+       } else if (!strcmp(id->name, "mpu9150")) {
+               inf->chip_type = INV_MPU6050;
+               dev_id = MPU6050_ID;
+       } else if (!strcmp(id->name, "mpu6500")) {
+               inf->chip_type = INV_MPU6500;
+               dev_id = MPU6500_ID;
+       } else if (!strcmp(id->name, "mpu9250")) {
+               inf->chip_type = INV_MPU6500;
+               dev_id = MPU9250_ID;
+       } else if (!strcmp(id->name, "mpu6xxx")) {
+               inf->chip_type = INV_MPU6050;
+               dev_id = 0xFF;
+       } else {
+               return -ENODEV;
+       }
+
+       nvi_pm_init(inf);
+       if (dev_id) {
+               err = inv_i2c_read(inf, inf->reg->who_am_i, 1, &val);
+               if (err) {
+                       dev_err(&inf->i2c->dev, "%s I2C ID READ ERR\n",
+                               __func__);
+                       if (dev_id == 0xFF) {
+                               dev_err(&inf->i2c->dev, "%s AUTO ID FAILED\n",
+                                       __func__);
+                               return -EPERM;
+                       }
+               } else {
+                       if ((dev_id != 0xFF) && (dev_id != val))
+                               dev_err(&inf->i2c->dev, "%s %s_ID %x != %x\n",
+                                       __func__, id->name, dev_id, val);
+                       switch (val) {
+                       case MPU6050_ID:
+                               inf->chip_type = INV_MPU6050;
+                               break;
+
+                       case MPU6500_ID:
+                               inf->chip_type = INV_MPU6500;
+                               break;
+
+                       case MPU9250_ID:
+                               inf->chip_type = INV_MPU6500;
+                               break;
+
+                       default:
+                               dev_err(&inf->i2c->dev, "%s ERR: NO ID %x\n",
+                                       __func__, val);
+                       }
+               }
+       }
+
+       inf->hw_s = (struct inv_hw_s *)(hw_info + inf->chip_type);
+       dev_dbg(&inf->i2c->dev, "%s: BRD_CFG=%s ID=%x USING: %s\n",
+               __func__, id->name, val, inf->hw_s->name);
+       nvi_init_config(inf);
+       switch (inf->chip_type) {
+       case INV_ITG3500:
+               inf->hal.fifo_size = NVI_FIFO_SIZE_3050;
+               break;
+
+       case INV_MPU3050:
+               inf->hal.fifo_size = NVI_FIFO_SIZE_3050;
+               err = inv_init_config_mpu3050(inf);
+               break;
+
+       case INV_MPU6050:
+               inf->hal.fifo_size = NVI_FIFO_SIZE_6050;
+               inf->hal.lpa_tbl = &nvi_lpa_delay_us_tbl_6050[0];
+               inf->hal.lpa_tbl_n = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6050);
+               err = inv_get_silicon_rev_mpu6050(inf);
+               break;
+
+       case INV_MPU6500:
+               inf->hal.fifo_size = NVI_FIFO_SIZE_6500;
+               inf->hal.lpa_tbl = &nvi_lpa_delay_us_tbl_6500[0];
+               inf->hal.lpa_tbl_n = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6500);
+               err = inv_get_silicon_rev_mpu6500(inf);
+               break;
+
+       default:
+               err = -ENODEV;
+               break;
+       }
+
+       nvi_pm(inf, NVI_PM_OFF);
+       return err;
+}
+
 static void nvi_shutdown(struct i2c_client *client)
 {
        struct inv_gyro_state_s *inf;
@@ -3340,12 +4136,10 @@ static void nvi_shutdown(struct i2c_client *client)
 
        inf = i2c_get_clientdata(client);
        if (inf != NULL) {
-               if (inf->nvi) {
-                       for (i = 0; i < AUX_PORT_SPECIAL; i++) {
-                               if (inf->aux.port[i].nmp.shutdown_bypass) {
-                                       nvi_aux_bypass_enable(inf, true);
-                                       break;
-                               }
+               for (i = 0; i < AUX_PORT_SPECIAL; i++) {
+                       if (inf->aux.port[i].nmp.shutdown_bypass) {
+                               nvi_aux_bypass_enable(inf, true);
+                               break;
                        }
                }
                inf->shutdown = true;
@@ -3367,15 +4161,10 @@ static int nvi_remove(struct i2c_client *client)
        nvi_shutdown(client);
        inf = i2c_get_clientdata(client);
        if (inf != NULL) {
-               if (inf->nvi) {
-                       nvi_pm_exit(inf);
-               } else {
-                       inv_pm(inf, NVI_PM_OFF_FORCE);
-                       nvi_vreg_exit(inf);
-               }
+               nvi_pm_exit(inf);
                kfree(inf);
        }
-       dev_info(&client->dev, "Gyro module removed.\n");
+       dev_info(&client->dev, "%s\n", __func__);
        return 0;
 }
 
@@ -3407,38 +4196,10 @@ static int nvi_probe(struct i2c_client *client,
        st->plat_data =
                   *(struct mpu_platform_data *)dev_get_platdata(&client->dev);
 
-       /* power is turned on inside check chip type*/
-       result = inv_check_chip_type(st, id);
+       result = nvi_dev_init(st, id);
        if (result)
                goto out_free;
 
-       st->hw_s = (struct inv_hw_s *)(hw_info  + st->chip_type);
-       if (INV_MPU3050 == st->chip_type)
-               result = inv_init_config_mpu3050(st);
-       else
-               result = inv_init_config(st);
-       if (result) {
-               dev_err(&client->adapter->dev,
-                       "Could not initialize device.\n");
-               goto out_free;
-       }
-
-       if (INV_ITG3500 != st->chip_type && INV_MPU3050 != st->chip_type) {
-               result = inv_get_silicon_rev_mpu6050(st);
-               if (result) {
-                       dev_err(&client->adapter->dev,
-                               "%s get silicon error.\n", st->hw_s->name);
-                       goto out_free;
-               }
-       }
-
-       result = inv_pm(st, NVI_PM_OFF);
-       if (result) {
-               dev_err(&client->adapter->dev,
-                       "%s could not be turned off.\n", st->hw_s->name);
-               goto out_free;
-       }
-
        mutex_init(&st->mutex);
        mutex_init(&st->mutex_temp);
        INIT_KFIFO(st->trigger.timestamps);
@@ -3453,16 +4214,10 @@ static int nvi_probe(struct i2c_client *client,
        }
 
        st->trigger.irq = client->irq;
-       if (INV_MPU3050 == st->chip_type)
-               result = request_threaded_irq(client->irq, inv_irq_handler,
-                                             inv_read_fifo_mpu3050,
-                                             IRQF_TRIGGER_RISING |
-                                             IRQF_SHARED, "inv_irq", st);
-       else
-               result = request_threaded_irq(client->irq, inv_irq_handler,
-                                             inv_read_fifo,
-                                             IRQF_TRIGGER_RISING |
-                                             IRQF_SHARED, "inv_irq", st);
+       result = request_threaded_irq(client->irq,
+                                     nvi_irq_handler, nvi_irq_thread,
+                                     IRQF_TRIGGER_RISING | IRQF_SHARED,
+                                     "inv_irq", st);
        if (result)
                goto out_close_sysfs;
 
@@ -3474,7 +4229,7 @@ static int nvi_probe(struct i2c_client *client,
        }
 
        inf_local = st;
-       dev_info(&client->adapter->dev, "%s is ready to go!\n", st->hw_s->name);
+       dev_info(&client->adapter->dev, "%s is ready to go\n", st->hw_s->name);
        return 0;
 
 out_close_sysfs:
@@ -3482,12 +4237,7 @@ out_close_sysfs:
 out_free_kfifo:
        kfifo_free(&st->trigger.timestamps);
 out_free:
-       if (st->nvi) {
-               nvi_pm_exit(st);
-       } else {
-               inv_pm(st, NVI_PM_OFF_FORCE);
-               nvi_vreg_exit(st);
-       }
+       nvi_pm_exit(st);
        kfree(st);
 out_no_free:
        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
@@ -3499,26 +4249,29 @@ static unsigned short normal_i2c[] = { I2C_CLIENT_END };
 /* device id table is used to identify what device can be
  * supported by this driver
  */
-static struct i2c_device_id inv_mod_id[] = {
-       {"itg3500", 0},
-       {"mpu3050", 0},
-       {"mpu6050", 0},
-       {"mpu9150", 0},
+static struct i2c_device_id nvi_mpu_id[] = {
+       {"itg3500", INV_ITG3500},
+       {"mpu3050", INV_MPU3050},
+       {"mpu6050", INV_MPU6050},
+       {"mpu9150", INV_MPU9150},
+       {"mpu6500", INV_MPU6500},
+       {"mpu9250", INV_MPU9250},
+       {"mpu6xxx", INV_MPU6XXX},
        {}
 };
 
-MODULE_DEVICE_TABLE(i2c, inv_mod_id);
+MODULE_DEVICE_TABLE(i2c, nvi_mpu_id);
 
 static struct i2c_driver inv_mod_driver = {
        .class = I2C_CLASS_HWMON,
        .probe          =       nvi_probe,
        .remove         =       nvi_remove,
-       .id_table       =       inv_mod_id,
+       .id_table       =       nvi_mpu_id,
        .driver = {
                .owner  =       THIS_MODULE,
                .name   =       "inv_dev",
 #ifdef CONFIG_PM
-               .pm     =       &inv_pm_ops,
+               .pm     =       &nvi_pm_ops,
 #endif
        },
        .address_list = normal_i2c,
@@ -3545,7 +4298,7 @@ static void __exit inv_mod_exit(void)
 module_init(inv_mod_init);
 module_exit(inv_mod_exit);
 
-MODULE_AUTHOR("Invensense Corporation");
+MODULE_AUTHOR("NVIDIA Corporation");
 MODULE_DESCRIPTION("Invensense device driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("inv_dev");
index 9852064..d4b333d 100644 (file)
 
 #define GYRO_INPUT_RESOLUTION          (1)
 #define ACCL_INPUT_RESOLUTION          (1)
-
+#define NVI_BYPASS_TIMEOUT_MS          (1000)
+#define NVI_FIFO_SIZE_3050             (512)
+#define NVI_FIFO_SIZE_6050             (1024)
+#define NVI_FIFO_SIZE_6500             (4096)
+#define NVI_FIFO_SAMPLE_SIZE_MAX       (38)
+#define NVI_DELAY_US_MAX               (256000)
+#define NVI_DELAY_US_MIN               (15000)
+#define NVI_DELAY_DEFAULT              (50000)
+#define NVI_INPUT_GYRO_DELAY_US_MIN    (5000)
+#define NVI_INPUT_ACCL_DELAY_US_MIN    (5000)
+#define NVI_TEMP_EN                    (1 << 0)
+#define NVI_TEMP_GYRO                  (1 << 1)
+#define NVI_TEMP_ACCL                  (1 << 2)
+#define NVI_MOT_DIS                    (0)
+#define NVI_MOT_EN                     (1)
+#define NVI_MOT_DBG                    (2)
+
+#define NVI_PM_ERR                     (0)
+#define NVI_PM_AUTO                    (1)
+#define NVI_PM_OFF_FORCE               (2)
+#define NVI_PM_OFF                     (3)
+#define NVI_PM_STDBY                   (4)
+#define NVI_PM_ON_CYCLE                        (5)
+#define NVI_PM_ON                      (6)
+#define NVI_PM_ON_FULL                 (7)
 
 /**
  *  struct inv_reg_map_s - Notable slave registers.
  *  @prgm_strt_addrh   firmware program start address register
  */
 struct inv_reg_map_s {
-       unsigned char who_am_i;
-       unsigned char sample_rate_div;
-       unsigned char lpf;
-       unsigned char product_id;
-       unsigned char bank_sel;
-       unsigned char user_ctrl;
-       unsigned char fifo_en;
-       unsigned char gyro_config;
-       unsigned char accl_config;
-       unsigned char fifo_count_h;
-       unsigned char fifo_r_w;
-       unsigned char raw_gyro;
-       unsigned char raw_accl;
-       unsigned char temperature;
-       unsigned char int_enable;
-       unsigned char int_status;
-       unsigned char pwr_mgmt_1;
-       unsigned char pwr_mgmt_2;
-       unsigned char mem_start_addr;
-       unsigned char mem_r_w;
-       unsigned char prgm_strt_addrh;
+       u8 who_am_i;
+       u8 sample_rate_div;
+       u8 lpf;
+       u8 product_id;
+       u8 bank_sel;
+       u8 user_ctrl;
+       u8 fifo_en;
+       u8 gyro_config;
+       u8 accl_config;
+       u8 fifo_count_h;
+       u8 fifo_r_w;
+       u8 raw_gyro;
+       u8 raw_accl;
+       u8 temperature;
+       u8 int_enable;
+       u8 int_status;
+       u8 pwr_mgmt_1;
+       u8 pwr_mgmt_2;
+       u8 mem_start_addr;
+       u8 mem_r_w;
+       u8 prgm_strt_addrh;
+
+       u8 accl_fifo_en;
+       u8 fifo_reset;
+       u8 i2c_mst_reset;
+       u8 cycle;
 };
 
 enum inv_devices {
-       INV_ITG3500 = 0,
-       INV_MPU3050 = 1,
-       INV_MPU6050 = 2,
-       INV_MPU9150 = 3,
+       INV_ITG3500,
+       INV_MPU3050,
+       INV_MPU6050,
+       INV_MPU9150,
+       INV_MPU6500,
+       INV_MPU9250,
+       INV_MPU6XXX,
        INV_NUM_PARTS
 };
 
+struct nvi_hal {
+       unsigned int fifo_size;
+       unsigned long *lpa_tbl;
+       unsigned int lpa_tbl_n;
+};
+
 /**
  *  struct test_setup_t - set up parameters for self test.
  *  @sample_rate: sensitity for gyro.
@@ -158,15 +196,20 @@ struct inv_chip_config_s {
        unsigned int accl_resolution;
        unsigned char accl_fsr;
        unsigned long lpa_delay_us;
+       unsigned char temp_enable;
        unsigned char temp_fifo_enable;
        unsigned char dmp_on;
        unsigned char firmware_loaded;
+       unsigned char mot_enable;
        unsigned char mot_dur;
        unsigned char mot_ctrl;
        unsigned int mot_cnt;
-       unsigned char fifo_rate;
+       unsigned int fifo_thr;
        unsigned int  prog_start_addr;
        unsigned long min_delay_us;
+       long long gyro_start_delay_ns;
+       unsigned int bypass_timeout_ms;
+       unsigned char is_asleep;
 };
 
 /**
@@ -197,7 +240,7 @@ struct inv_chip_info_s {
  *  @timestamps:       Timestamp buffer.
  */
 struct inv_trigger_s {
-#define TIMESTAMP_FIFO_SIZE 16
+#define TIMESTAMP_FIFO_SIZE 32
        unsigned long irq;
        DECLARE_KFIFO(timestamps, long long, TIMESTAMP_FIFO_SIZE);
 };
@@ -242,37 +285,15 @@ struct inv_regulator_s {
        struct regulator *regulator_vdd;
 };
 
-struct nvi_hw {
-       unsigned char aux_vddio;
-       unsigned char smplrt_div;
-       unsigned char config;
-       unsigned char gyro_config;
-       unsigned char accl_config;
-       unsigned char mot_thr;
-       unsigned char mot_dur;
-       unsigned char zrmot_thr;
-       unsigned char zrmot_dur;
-       unsigned char fifo_en;
-       unsigned char i2c_mst_ctrl;
-       unsigned char i2c_slv4_ctrl;
-       unsigned char int_pin_cfg;
-       unsigned char int_enable;
-       unsigned char i2c_mst_delay_ctrl;
-       unsigned char mot_detect_ctrl;
-       unsigned char user_ctrl;
-       unsigned char pwr_mgmt_1;
-       unsigned char pwr_mgmt_2;
-};
-
-#define AUX_PORT_MAX            (5)
-#define AUX_PORT_SPECIAL        (4)
-#define AUX_PORT_BYPASS         (-1)
-#define AUX_EXT_DATA_REG_MAX    (24)
-#define AUX_DEV_VALID_READ_MAX  (10)
+#define AUX_PORT_MAX                   (5)
+#define AUX_PORT_SPECIAL               (4)
+#define AUX_PORT_BYPASS                        (-1)
+#define AUX_EXT_DATA_REG_MAX           (24)
+#define AUX_DEV_VALID_READ_LOOP_MAX    (20)
+#define AUX_DEV_VALID_READ_DELAY_MS    (5)
 
 struct aux_port {
        struct nvi_mpu_port nmp;
-       long long delay_ns;
        unsigned short ext_data_offset;
        bool hw_valid;
        bool hw_en;
@@ -283,25 +304,47 @@ struct aux_port {
 
 struct aux_ports {
        struct aux_port port[AUX_PORT_MAX];
-       int bypass_lock;
-       unsigned char delay_hw;
+       s64 bypass_timeout_ns;
+       unsigned int bypass_lock;
+       u8 delay_hw;
        unsigned short ext_data_n;
        unsigned char ext_data[AUX_EXT_DATA_REG_MAX];
        unsigned char clock_i2c;
-       bool need_reset;
+       bool reset_i2c;
+       bool reset_fifo;
        bool enable;
+       bool en3050;
        bool dbg;
 };
 
-#define NVI_PM_ERR                     0
-#define NVI_PM_AUTO                    1
-#define NVI_PM_OFF_FORCE               2
-#define NVI_PM_OFF                     3
-#define NVI_PM_STDBY                   4
-#define NVI_PM_ON_CYCLE                        5
-#define NVI_PM_ON                      6
-#define NVI_PM_ON_FULL                 7
-
+struct nvi_hw {
+       u8 aux_vddio;
+       u8 smplrt_div;
+       u8 config;
+       u8 gyro_config;
+       u8 accl_config;
+       u8 accl_config2;
+       u8 lposc_clksel;
+       u8 mot_thr;
+       u8 mot_dur;
+       u8 zrmot_thr;
+       u8 zrmot_dur;
+       u8 fifo_en;
+       u8 i2c_mst_ctrl;
+       u8 i2c_slv_addr[AUX_PORT_MAX];
+       u8 i2c_slv_reg[AUX_PORT_MAX];
+       u8 i2c_slv_ctrl[AUX_PORT_SPECIAL];
+       u8 i2c_slv4_do;
+       u8 i2c_slv4_ctrl;
+       u8 int_pin_cfg;
+       u8 int_enable;
+       u8 i2c_slv_do[AUX_PORT_SPECIAL];
+       u8 i2c_mst_delay_ctrl;
+       u8 mot_detect_ctrl;
+       u8 user_ctrl;
+       u8 pwr_mgmt_1;
+       u8 pwr_mgmt_2;
+};
 
 struct inv_mpu_slave;
 /**
@@ -354,33 +397,31 @@ struct inv_gyro_state_s {
        struct mpu_platform_data plat_data;
        struct inv_mpu_slave *mpu_slave;
        struct regulator_bulk_data vreg[2];
-       unsigned char fifo_counter;
+       bool fifo_reset_3050;
        unsigned char i2c_addr;
        unsigned char sample_divider;
        unsigned char fifo_divider;
        void *sl_handle;
-       unsigned int irq_dur_us;
-       long long last_isr_time;
        struct mutex mutex;
        struct mutex mutex_temp;
        struct nvi_hw hw;
+       struct nvi_hal hal;
        struct aux_ports aux;
        int pm;
-       int stby;
-       int lpa_hw;
        unsigned long sample_delay_us;
+       u16 fifo_sample_size;
        bool shutdown;
-       bool nvi;
-       bool lpa_enable;
-       bool mot_enable;
-       bool mot_dbg;
+       bool suspend;
        unsigned int mot_cnt;
        short temp_val;
        s64 temp_ts;
+       s64 fifo_ts;
+       s64 gyro_start_ts;
 #if DEBUG_SYSFS_INTERFACE
-       unsigned short dbg_i2c_addr;
-       unsigned char dbg_reg;
+       u16 dbg_i2c_addr;
+       u8 dbg_reg;
 #endif /* DEBUG_SYSFS_INTERFACE */
+       u8 buf[NVI_FIFO_SAMPLE_SIZE_MAX * 2]; /* (* 2)=FIFO OVERFLOW OFFSET */
 };
 
 /* produces an unique identifier for each device based on the
@@ -420,6 +461,7 @@ struct inv_mpu_slave {
 #define REG_3500_OTP            (0x00)
 #define REG_AUX_VDDIO           (0x01)
 #define REG_ST_GCT_X            (0x0D)
+#define REG_6500_LP_ACCEL_ODR   (0x1E)
 #define REG_MOT_THR             (0x1F)
 #define REG_MOT_DUR             (0x20)
 #define REG_ZMOT_THR            (0x21)
@@ -490,21 +532,28 @@ struct inv_mpu_slave {
 #define BITS_GYRO_OUT          (0x70)
 #define BITS_SELF_TEST_EN       (0xE0)
 #define BITS_3050_ACCL_OUT     (0x0E)
+#define BIT_3050_FIFO_FOOTER   (0x01)
 #define BITS_3050_POWER1        (0x30)
 #define BITS_3050_POWER2        (0x10)
 #define BITS_3050_GYRO_STANDBY  (0x38)
 #define BITS_FSR               (0x18)
 #define BITS_LPF               (0x07)
 #define BITS_CLK               (0x07)
-#define BIT_3500_FIFO_OVERFLOW (0x10)
 #define BIT_RESET               (0x80)
 #define BIT_SLEEP              (0x40)
 #define BIT_CYCLE               (0x20)
 #define BIT_LPA_FREQ            (0xC0)
+#define BIT_STBY_XA            (0x20)
+#define BIT_STBY_YA            (0x10)
+#define BIT_STBY_ZA            (0x08)
+#define BIT_STBY_XG            (0x04)
+#define BIT_STBY_YG            (0x02)
+#define BIT_STBY_ZG            (0x01)
 
 #define DMP_START_ADDR          (0x400)
 #define BYTES_FOR_DMP           (16)
 #define BYTES_PER_SENSOR        (6)
+#define FIFO_THRESHOLD           500
 #define POWER_UP_TIME           (40)
 #define MPU_MEM_BANK_SIZE        (256)
 #define MPL_PROD_KEY(ver, rev) (ver * 100 + rev)
@@ -513,6 +562,13 @@ struct inv_mpu_slave {
 #define MPU_SILICON_REV_A2              1       /* MPU6050A2 Device */
 #define MPU_SILICON_REV_B1              2       /* MPU6050B1 Device */
 
+#define MPU6050_ID                     (0x68)
+#define MPU6500_ID                     (0x70)
+#define MPU6500_PRODUCT_REVISION       (1)
+#define MPU6500_MEM_REV_ADDR           (0x17)
+#define MPU6500_REV                    (2)
+#define MPU9250_ID                     (0x71)
+
 #define BIT_PRFTCH_EN                           0x40
 #define BIT_CFG_USER_BANK                       0x20
 #define BITS_MEM_SEL                            0x1f
@@ -520,15 +576,10 @@ struct inv_mpu_slave {
 #define TIME_STAMP_TOR           (5)
 #define MAX_CATCH_UP             (5)
 #define DEFAULT_ACCL_TRIM        (16384)
+#define DEFAULT_GYRO_TRIM        (131)
 #define MAX_FIFO_RATE            (1000000)
 #define MIN_FIFO_RATE            (4000)
 #define ONE_K_HZ                 (1000)
-#define NVI_DELAY_US_MAX         (256000)
-#define NVI_DELAY_US_MIN         (125)
-#define NVI_DELAY_DEFAULT        (50000)
-#define NVI_INPUT_GYRO_DELAY_US_MIN    (125)
-#define NVI_INPUT_ACCL_DELAY_US_MIN    (1000)
-
 
 /* authenticate key */
 #define D_AUTH_OUT               (32)
@@ -642,6 +693,7 @@ enum inv_clock_sel_e {
 
 int inv_hw_self_test(struct inv_gyro_state_s *st, int *gyro_bias_regular);
 int inv_get_silicon_rev_mpu6050(struct inv_gyro_state_s *st);
+int inv_get_silicon_rev_mpu6500(struct inv_gyro_state_s *st);
 int inv_i2c_read_base(struct inv_gyro_state_s *st, unsigned short i2c_addr,
        unsigned char reg, unsigned short length, unsigned char *data);
 int inv_i2c_single_write_base(struct inv_gyro_state_s *st,
@@ -655,8 +707,11 @@ int inv_i2c_single_write_base(struct inv_gyro_state_s *st,
 #define inv_secondary_write(reg, data) \
        inv_i2c_single_write_base(st, st->plat_data.secondary_i2c_addr, \
                reg, data)
-int inv_set_power_state(struct inv_gyro_state_s *st, unsigned char power_on);
-int set_inv_enable(struct inv_gyro_state_s *st, unsigned long enable);
+int nvi_gyro_enable(struct inv_gyro_state_s *inf,
+                   unsigned char enable, unsigned char fifo_enable);
+int nvi_accl_enable(struct inv_gyro_state_s *inf,
+                   unsigned char enable, unsigned char fifo_enable);
+
 int mpu_memory_write(struct i2c_adapter *i2c_adap,
                            unsigned char mpu_addr,
                            unsigned short mem_addr,
@@ -667,40 +722,14 @@ int mpu_memory_read(struct i2c_adapter *i2c_adap,
                           unsigned int len, unsigned char *data);
 void inv_setup_reg_mpu3050(struct inv_reg_map_s *reg);
 int inv_init_config_mpu3050(struct inv_gyro_state_s *st);
-irqreturn_t inv_read_fifo_mpu3050(int irq, void *dev_id);
-int inv_setup_mpu3050(struct inv_gyro_state_s *st);
-int inv_register_kxtf9_slave(struct inv_gyro_state_s *st);
-int create_device_attributes(struct device *dev,
-       struct device_attribute **attrs);
-void remove_device_attributes(struct device *dev,
-       struct device_attribute **attrs);
 int set_3050_bypass(struct inv_gyro_state_s *st, int enable);
+int inv_register_kxtf9_slave(struct inv_gyro_state_s *st);
 s64 get_time_ns(void);
-int inv_mpu3050_create_sysfs(struct inv_gyro_state_s *st);
-int inv_mpu3050_remove_sysfs(struct inv_gyro_state_s *st);
 int inv_get_accl_bias(struct inv_gyro_state_s *st, int *accl_bias_regular);
-int set_power_mpu3050(struct inv_gyro_state_s *st, unsigned char power_on);
+
 int inv_enable_tap_dmp(struct inv_gyro_state_s *st, unsigned char on);
 int inv_enable_orientation_dmp(struct inv_gyro_state_s *st);
 unsigned short inv_dmp_get_address(unsigned short key);
-ssize_t nvi_gyro_enable_show(struct device *dev,
-                            struct device_attribute *attr, char *buf);
-ssize_t nvi_gyro_fifo_enable_show(struct device *dev,
-                                 struct device_attribute *attr, char *buf);
-ssize_t nvi_gyro_max_range_show(struct device *dev,
-                               struct device_attribute *attr, char *buf);
-ssize_t nvi_accl_enable_show(struct device *dev,
-                            struct device_attribute *attr, char *buf);
-ssize_t nvi_accl_fifo_enable_show(struct device *dev,
-                                 struct device_attribute *attr, char *buf);
-ssize_t nvi_accl_delay_show(struct device *dev,
-                           struct device_attribute *attr, char *buf);
-ssize_t nvi_accl_max_range_show(struct device *dev,
-                               struct device_attribute *attr, char *buf);
-ssize_t inv_accl_matrix_show(struct device *dev,
-                            struct device_attribute *attr, char *buf);
-ssize_t nvi_enable_show(struct device *dev,
-                       struct device_attribute *attr, char *buf);
 
 ssize_t inv_dmp_firmware_write(struct file *fp, struct kobject *kobj,
        struct bin_attribute *attr, char *buf, loff_t pos, size_t size);
index 0a058de..491df7d 100644 (file)
@@ -297,6 +297,57 @@ int mpu_memory_read(struct i2c_adapter *i2c_adap,
        return 0;
 }
 
+int mpu_memory_read_6500(struct inv_gyro_state_s *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[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->sl_handle, msgs, 4);
+       if (res != 4) {
+               if (res >= 0)
+                       res = -EIO;
+       } else
+               res = 0;
+       return res;
+}
+
 /**
  *  @internal
  *  @brief  Inverse lookup of the index of an MPL product key .
@@ -315,6 +366,36 @@ static short index_of_key(unsigned short key)
        return -1;
 }
 
+int inv_get_silicon_rev_mpu6500(struct inv_gyro_state_s *st)
+{
+       struct inv_chip_info_s *chip_info = &st->chip_info;
+       int result;
+       u8 sw_rev;
+
+       /*memory read need more time after power up */
+       msleep(POWER_UP_TIME);
+       result = mpu_memory_read_6500(st, st->i2c_addr,
+                       MPU6500_MEM_REV_ADDR, 1, &sw_rev);
+       if (sw_rev == 0) {
+               pr_warning("Rev 0 of MPU6500\n");
+               pr_warning("can't sit with other devices in same I2C bus\n");
+       }
+       if (result)
+               return result;
+       if (sw_rev > MPU6500_REV)
+               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->accl_sens_trim = DEFAULT_ACCL_TRIM;
+       chip_info->multi = 1;
+       return 0;
+}
+
 int inv_get_silicon_rev_mpu6050(struct inv_gyro_state_s *st)
 {
        int result;
@@ -619,18 +700,18 @@ static int inv_do_test(struct inv_gyro_state_s *st, int self_test_flag,
                return result;
 
        result = inv_i2c_single_write(st, reg->sample_rate_div,
-               test_setup.sample_rate);
+                                     test_setup.sample_rate);
        if (result)
                return result;
 
        result = inv_i2c_single_write(st, reg->gyro_config,
-               self_test_flag | test_setup.gyro_fsr);
+                                     self_test_flag | test_setup.gyro_fsr);
        if (result)
                return result;
 
        if (has_accl) {
                result = inv_i2c_single_write(st, reg->accl_config,
-                       self_test_flag | test_setup.accl_fsr);
+                                        self_test_flag | test_setup.accl_fsr);
                if (result)
                        return result;
        }
@@ -713,21 +794,10 @@ static int inv_do_test(struct inv_gyro_state_s *st, int self_test_flag,
  */
 static void inv_recover_setting(struct inv_gyro_state_s *st)
 {
-       struct inv_reg_map_s *reg;
-       int data;
-
-       reg = st->reg;
-       set_inv_enable(st, st->chip_config.enable);
-       inv_i2c_single_write(st, reg->gyro_config,
-                            st->chip_config.gyro_fsr<<3);
-       inv_i2c_single_write(st, reg->lpf, st->chip_config.lpf);
-       data = ONE_K_HZ/st->chip_config.fifo_rate - 1;
-       inv_i2c_single_write(st, reg->sample_rate_div, data);
-       if (INV_ITG3500 != st->chip_type) {
-               inv_i2c_single_write(st, reg->accl_config,
-                       (st->chip_config.accl_fsr << 3)|0);
-       }
-       inv_set_power_state(st, 1);
+       nvi_gyro_enable(st, st->chip_config.gyro_enable,
+                       st->chip_config.gyro_fifo_enable);
+       nvi_accl_enable(st, st->chip_config.accl_enable,
+                       st->chip_config.accl_fifo_enable);
 }
 
 /**
@@ -745,8 +815,8 @@ int inv_hw_self_test(struct inv_gyro_state_s *st,
        accel_result = gyro_result = 0;
        test_times = 2;
        while (test_times > 0) {
-               result = inv_do_test(st, 0,  gyro_bias_regular,
-                       accl_bias_regular);
+               result = inv_do_test(st, 0, gyro_bias_regular,
+                                    accl_bias_regular);
                if (result == -EAGAIN)
                        test_times--;
                else
@@ -758,7 +828,7 @@ int inv_hw_self_test(struct inv_gyro_state_s *st,
        test_times = 2;
        while (test_times > 0) {
                result = inv_do_test(st, BITS_SELF_TEST_EN, gyro_bias_st,
-                                       accl_bias_st);
+                                    accl_bias_st);
                if (result == -EAGAIN)
                        test_times--;
                else
@@ -769,16 +839,16 @@ int inv_hw_self_test(struct inv_gyro_state_s *st,
 
        if (st->chip_type == INV_ITG3500) {
                gyro_result = !inv_check_3500_gyro_self_test(st,
-                       gyro_bias_regular, gyro_bias_st);
+                                             gyro_bias_regular, gyro_bias_st);
        } else {
                accel_result = !inv_check_accl_self_test(st,
-                       accl_bias_regular, accl_bias_st);
+                                             accl_bias_regular, accl_bias_st);
                gyro_result = !inv_check_6050_gyro_self_test(st,
-                       gyro_bias_regular, gyro_bias_st);
+                                             gyro_bias_regular, gyro_bias_st);
        }
 test_fail:
        inv_recover_setting(st);
-       return (accel_result<<1) | gyro_result;
+       return (accel_result << 1) | gyro_result;
 }
 
 /**
index 205bf45..6718e86 100644 (file)
 
 #include "inv_gyro.h"
 
-static unsigned long nvi_lpf_us_tbl[] = {
-       0,      /* N/A */
-       5319,   /* 188Hz */
-       10204,  /* 98Hz */
-       23810,  /* 42Hz */
-       50000,  /* 20Hz */
-       100000, /* 10Hz */
-       /* 200000, 5Hz */
-};
 
-
-/**
- *  inv_clear_kfifo() - clear time stamp fifo
- *  @st:       Device driver instance.
- */
-static void inv_clear_kfifo(struct inv_gyro_state_s *st)
-{
-       unsigned long flags;
-       spin_lock_irqsave(&st->time_stamp_lock, flags);
-       kfifo_reset(&st->trigger.timestamps);
-       spin_unlock_irqrestore(&st->time_stamp_lock, flags);
-}
-
-/**
- *  mpu3050_fifo_reset() - Reset FIFO related registers
- *  @st:       Device driver instance.
- */
-static int mpu3050_fifo_reset(struct inv_gyro_state_s *st)
-{
-       struct inv_reg_map_s *reg;
-       int result;
-       unsigned char val, user_ctrl;
-
-       reg = st->reg;
-       /* disable interrupt */
-       result = inv_i2c_single_write(st, reg->int_enable, 0);
-       if (result)
-               return result;
-
-       inv_clear_kfifo(st);
-       /* disable the sensor output to FIFO */
-       result = inv_i2c_single_write(st, reg->fifo_en, 0);
-       if (result)
-               goto reset_fifo_fail;
-       result = inv_i2c_read(st, reg->user_ctrl, 1, &user_ctrl);
-       if (result)
-               goto reset_fifo_fail;
-       /* disable fifo reading */
-       user_ctrl &= ~BIT_FIFO_EN;
-       st->fifo_counter = 0;
-       /* reset fifo */
-       val = (BIT_3050_FIFO_RST | user_ctrl);
-       result = inv_i2c_single_write(st, reg->user_ctrl, val);
-       if (result)
-               goto reset_fifo_fail;
-       mdelay(POWER_UP_TIME);
-       st->last_isr_time = get_time_ns();
-       if (st->chip_config.dmp_on) {
-               /* enable interrupt when DMP is done */
-               result = inv_i2c_single_write(st, reg->int_enable,
-                                       BIT_DMP_INT_EN);
-               if (result)
-                       return result;
-
-               result = inv_i2c_single_write(st, reg->user_ctrl,
-                       BIT_FIFO_EN|user_ctrl);
-               if (result)
-                       return result;
-       } else {
-               /* enable interrupt */
-               if (st->chip_config.accl_fifo_enable ||
-                       st->chip_config.gyro_fifo_enable){
-                       result = inv_i2c_single_write(st, reg->int_enable,
-                                                       BIT_DATA_RDY_EN);
-                       if (result)
-                               return result;
-               }
-               /* enable FIFO reading and I2C master interface*/
-               result = inv_i2c_single_write(st, reg->user_ctrl,
-                       BIT_FIFO_EN | user_ctrl);
-               if (result)
-                       return result;
-               /* enable sensor output to FIFO and FIFO footer*/
-               val = 1;
-               if (st->chip_config.accl_fifo_enable)
-                       val |= BITS_3050_ACCL_OUT;
-               if (st->chip_config.gyro_fifo_enable)
-                       val |= BITS_GYRO_OUT;
-               result = inv_i2c_single_write(st, reg->fifo_en, val);
-               if (result)
-                       return result;
-       }
-
-       return 0;
-reset_fifo_fail:
-       if (st->chip_config.dmp_on)
-               val = BIT_DMP_INT_EN;
-       else
-               val = BIT_DATA_RDY_EN;
-       inv_i2c_single_write(st, reg->int_enable, val);
-       pr_err("%s failed\n", __func__);
-       return result;
-}
-
-/**
- *  set_power_mpu3050() - set power of mpu3050.
- *  @st:       Device driver instance.
- *  @power_on:  on/off
- */
-int set_power_mpu3050(struct inv_gyro_state_s *st, unsigned char power_on)
-{
-       struct inv_reg_map_s *reg;
-       unsigned char data, p;
-       int result;
-       reg = st->reg;
-       if (power_on) {
-               data = 0;
-       } else {
-               if (st->mpu_slave) {
-                       result = st->mpu_slave->suspend(st);
-                       if (result)
-                               return result;
-               }
-
-               data = BIT_SLEEP;
-       }
-       if (st->chip_config.gyro_enable) {
-               p = (BITS_3050_POWER1 | INV_CLK_PLL);
-               result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data | p);
-               if (result)
-                       return result;
-
-               p = (BITS_3050_POWER2 | INV_CLK_PLL);
-               result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data | p);
-               if (result)
-                       return result;
-
-               p = INV_CLK_PLL;
-               result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data | p);
-               if (result)
-                       return result;
-
-               st->chip_config.clk_src = INV_CLK_PLL;
-       } else {
-               data |= (BITS_3050_GYRO_STANDBY | INV_CLK_INTERNAL);
-               result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data);
-               if (result)
-                       return result;
-               st->chip_config.clk_src = INV_CLK_INTERNAL;
-       }
-       if (power_on) {
-               mdelay(POWER_UP_TIME);
-               if (st->mpu_slave) {
-                       result = st->mpu_slave->resume(st);
-                       if (result)
-                               return result;
-               }
-       }
-
-       return 0;
-}
-
-/**
- *  inv_set_power_state() - Turn device on/off.
- *  @st:       Device driver instance.
- *  @power_on: 1 to turn on, 0 to suspend.
- */
-int inv_set_power_state(struct inv_gyro_state_s *st, unsigned char power_on)
-{
-       return set_power_mpu3050(st, power_on);
-}
-
-/**
- *  set_inv_enable() - Reset FIFO related registers.
- *  @st:       Device driver instance.
- *  @fifo_enable: enable/disable
- */
-int set_inv_enable(struct inv_gyro_state_s *st, unsigned long enable)
-{
-       struct inv_reg_map_s *reg;
-       int result;
-
-       reg = st->reg;
-       if (enable) {
-               result = mpu3050_fifo_reset(st);
-               if (result)
-                       return result;
-
-               st->chip_config.enable = 1;
-       } else {
-               result = inv_i2c_single_write(st, reg->fifo_en, 0);
-               if (result)
-                       return result;
-
-               result = inv_i2c_single_write(st, reg->int_enable, 0);
-               if (result)
-                       return result;
-
-               st->chip_config.enable = 0;
-       }
-
-       return 0;
-}
-
-static int mpu3050_global_delay(struct inv_gyro_state_s *inf)
-{
-       unsigned long delay_us;
-       unsigned char val;
-       int rate;
-       int err = 0;
-
-       /* find the fastest polling of all the devices */
-       delay_us = -1;
-       if (inf->chip_config.gyro_enable && inf->chip_config.gyro_delay_us) {
-               if (inf->chip_config.gyro_delay_us < delay_us)
-                       delay_us = inf->chip_config.gyro_delay_us;
-       }
-       if (inf->chip_config.accl_enable && inf->chip_config.accl_delay_us) {
-               if (inf->chip_config.accl_delay_us < delay_us)
-                       delay_us = inf->chip_config.accl_delay_us;
-       }
-       if (delay_us == -1)
-               delay_us = NVI_DELAY_DEFAULT; /* default if nothing found */
-       if (delay_us < MIN_FIFO_RATE)
-               delay_us = MIN_FIFO_RATE;
-       if (delay_us > MAX_FIFO_RATE)
-               delay_us = MAX_FIFO_RATE;
-       /* program if new value */
-       if (delay_us != inf->sample_delay_us) {
-               dev_dbg(&inf->i2c->dev, "%s %lu\n", __func__, delay_us);
-               inf->sample_delay_us = delay_us;
-               val = delay_us / ONE_K_HZ - 1;
-               err = inv_i2c_single_write(inf, inf->reg->sample_rate_div,
-                                          val);
-               rate = 1000000 / delay_us;
-               inf->irq_dur_us = delay_us;
-               delay_us <<= 1;
-               for (val = 1; val < ARRAY_SIZE(nvi_lpf_us_tbl); val++) {
-                       if (delay_us < nvi_lpf_us_tbl[val])
-                               break;
-               }
-               if (inf->mpu_slave != NULL)
-                       err |= inf->mpu_slave->set_lpf(inf, rate);
-               err |= inv_i2c_single_write(inf, inf->reg->lpf, val |
-                                           (inf->chip_config.gyro_fsr << 3));
-               inf->last_isr_time = get_time_ns();
-       }
-       return err;
-}
-
-static int mpu3050_gyro_enable(struct inv_gyro_state_s *inf,
-                              unsigned char enable, unsigned char fifo_enable)
-{
-       unsigned char enable_old;
-       unsigned char fifo_enable_old;
-       unsigned char val;
-       int err = 0;
-
-       enable_old = inf->chip_config.gyro_enable;
-       fifo_enable_old = inf->chip_config.gyro_fifo_enable;
-       inf->chip_config.gyro_fifo_enable = fifo_enable;
-       inf->chip_config.gyro_enable = enable;
-       set_power_mpu3050(inf, 1);
-       if (enable != enable_old) {
-               if (enable) {
-                       val = (inf->chip_config.gyro_fsr << 3);
-                       val |= inf->chip_config.lpf;
-                       err = inv_i2c_single_write(inf, inf->reg->lpf, val);
-               }
-               mpu3050_global_delay(inf);
-       }
-       if (fifo_enable_old != fifo_enable)
-               mpu3050_fifo_reset(inf);
-       if (err) {
-               inf->chip_config.gyro_enable = enable_old;
-               inf->chip_config.gyro_fifo_enable = fifo_enable_old;
-       }
-       set_power_mpu3050(inf, 1);
-       return err;
-}
-
-/**
- *  inv_raw_accl_show() - Read accel data directly from registers.
- */
-static ssize_t mpu3050_raw_accl_show(struct device *dev,
-       struct device_attribute *attr, char *buf)
-{
-       unsigned char data[6];
-       short out[3];
-       int result;
-       struct inv_gyro_state_s *st;
-
-       st = dev_get_drvdata(dev);
-       if (st->mpu_slave != NULL) {
-               if (0 == st->mpu_slave->get_mode(st))
-                       return -EINVAL;
-       }
-       result = inv_i2c_read(st, REG_3050_AUX_XOUT_H, 6, data);
-       out[0] = out[1] = out[2];
-       if ((0 == result) && (st->mpu_slave != NULL))
-               st->mpu_slave->combine_data(data, out);
-       return sprintf(buf, "%d %d %d %lld\n", out[0],
-               out[1], out[2], get_time_ns());
-}
-
-/**
- *  inv_accl_fifo_enable_store() - Enable/disable accl fifo output.
- */
-static ssize_t mpu3050_accl_fifo_enable_store(struct device *dev,
-       struct device_attribute *attr, const char *buf, size_t count)
-{
-       unsigned long en;
-       struct inv_gyro_state_s *st;
-       st = dev_get_drvdata(dev);
-       if (kstrtoul(buf, 10, &en))
-               return -EINVAL;
-       if (en == !(!(st->chip_config.accl_fifo_enable)))
-               return count;
-       st->chip_config.accl_fifo_enable = en;
-       if (en && (0 == st->chip_config.accl_enable)) {
-               st->chip_config.accl_enable = en;
-               set_power_mpu3050(st, 1);
-       }
-       return count;
-}
-/**
- *  mpu3050_accl_enable_store() - Enable/disable accl.
- */
-static ssize_t mpu3050_accl_enable_store(struct device *dev,
-       struct device_attribute *attr, const char *buf, size_t count)
-{
-       unsigned long en;
-       struct inv_gyro_state_s *st;
-       st = dev_get_drvdata(dev);
-       if (kstrtoul(buf, 10, &en))
-               return -EINVAL;
-       if (en == !(!(st->chip_config.accl_enable)))
-               return count;
-       st->chip_config.accl_enable = en;
-       if ((0 == en) && st->chip_config.accl_fifo_enable)
-               st->chip_config.accl_fifo_enable = 0;
-       if (st->mpu_slave != NULL) {
-               if (en)
-                       set_power_mpu3050(st, 1);
-               else
-                       set_power_mpu3050(st, 0);
-       }
-       return count;
-}
-
-static ssize_t mpu3050_accl_delay_store(struct device *dev,
-                                       struct device_attribute *attr,
-                                       const char *buf, size_t count)
-{
-       struct inv_gyro_state_s *inf;
-       unsigned long accl_delay_us;
-       unsigned long accl_delay_us_old;
-       int err;
-
-       inf = dev_get_drvdata(dev);
-       err = kstrtoul(buf, 10, &accl_delay_us);
-       if (err)
-               return err;
-
-       mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, accl_delay_us);
-       if (accl_delay_us < NVI_INPUT_ACCL_DELAY_US_MIN)
-               accl_delay_us = NVI_INPUT_ACCL_DELAY_US_MIN;
-       if (accl_delay_us != inf->chip_config.accl_delay_us) {
-               accl_delay_us_old = inf->chip_config.accl_delay_us;
-               inf->chip_config.accl_delay_us = accl_delay_us;
-               if (inf->chip_config.accl_enable) {
-                       err = mpu3050_global_delay(inf);
-                       if (!err)
-                               inf->chip_config.accl_delay_us =
-                                                            accl_delay_us_old;
-               }
-       }
-       mutex_unlock(&inf->mutex);
-       if (err) {
-               dev_err(&inf->i2c->dev, "%s: %lu ERR=%d\n",
-                       __func__, accl_delay_us, err);
-               return err;
-       }
-
-       return count;
-}
-
-static ssize_t mpu3050_accl_max_range_store(struct device *dev,
-                                           struct device_attribute *attr,
-                                           const char *buf, size_t count)
-{
-       struct inv_gyro_state_s *inf;
-       unsigned char fsr;
-       int err;
-
-       inf = dev_get_drvdata(dev);
-       err = kstrtou8(buf, 10, &fsr);
-       if (err)
-               return -EINVAL;
-
-       if (fsr > 3)
-               return -EINVAL;
-
-       mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
-       if (fsr != inf->chip_config.accl_fsr) {
-               inf->chip_config.accl_fsr = fsr;
-               if (inf->chip_config.accl_enable) {
-                       if ((inf->chip_type == INV_MPU3050) &&
-                           (inf->mpu_slave != NULL)) {
-                               err = inf->mpu_slave->set_fs(inf, fsr);
-                               /* reset fifo to purge old data */
-                               mpu3050_fifo_reset(inf);
-                       }
-               }
-       }
-       mutex_unlock(&inf->mutex);
-       if (err < 0) {
-               dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", __func__, fsr, err);
-               return err;
-       }
-
-       return count;
-}
-
-static ssize_t mpu3050_gyro_enable_store(struct device *dev,
-                                        struct device_attribute *attr,
-                                        const char *buf, size_t count)
-{
-       struct inv_gyro_state_s *inf;
-       unsigned char enable;
-       unsigned char fifo_enable;
-       int err;
-
-       inf = dev_get_drvdata(dev);
-       err = kstrtou8(buf, 10, &enable);
-       if (err)
-               return -EINVAL;
-
-       if (enable > 7)
-               return -EINVAL;
-
-       mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable);
-       if (enable != inf->chip_config.gyro_enable) {
-               if (enable)
-                       fifo_enable = inf->chip_config.gyro_fifo_enable;
-               else
-                       fifo_enable = 0;
-               err = mpu3050_gyro_enable(inf, enable, fifo_enable);
-       }
-       mutex_unlock(&inf->mutex);
-       if (err) {
-               dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
-                       __func__, enable, err);
-               return err;
-       }
-
-       return count;
-}
-
-static ssize_t mpu3050_gyro_fifo_enable_store(struct device *dev,
-                                             struct device_attribute *attr,
-                                             const char *buf, size_t count)
-{
-       struct inv_gyro_state_s *inf;
-       unsigned char fifo_enable;
-       unsigned char enable;
-       int err;
-
-       inf = dev_get_drvdata(dev);
-       err = kstrtou8(buf, 10, &fifo_enable);
-       if (err)
-               return -EINVAL;
-
-       mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable);
-       enable = inf->chip_config.gyro_enable;
-       if (fifo_enable) {
-               fifo_enable = 1;
-               if (!enable)
-                       enable = 7;
-       }
-       if (fifo_enable != inf->chip_config.gyro_fifo_enable)
-               err = mpu3050_gyro_enable(inf, enable, fifo_enable);
-       mutex_unlock(&inf->mutex);
-       if (err) {
-               dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n",
-                       __func__, fifo_enable, err);
-               return err;
-       }
-
-       return count;
-}
-
-static ssize_t mpu3050_gyro_max_range_store(struct device *dev,
-                                           struct device_attribute *attr,
-                                           const char *buf, size_t count)
-{
-       struct inv_gyro_state_s *inf;
-       unsigned char fsr;
-       int err;
-
-       inf = dev_get_drvdata(dev);
-       err = kstrtou8(buf, 10, &fsr);
-       if (err)
-               return -EINVAL;
-
-       if (fsr > 3)
-               return -EINVAL;
-
-       mutex_lock(&inf->mutex);
-       dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr);
-       if (fsr != inf->chip_config.gyro_fsr) {
-               inf->chip_config.gyro_fsr = fsr;
-               if (inf->chip_config.gyro_enable) {
-                       fsr = (fsr << 3) | inf->chip_config.lpf;
-                       err = inv_i2c_single_write(inf, inf->reg->lpf, fsr);
-                       mpu3050_fifo_reset(inf);
-               }
-       }
-       mutex_unlock(&inf->mutex);
-       if (err < 0) {
-               dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", __func__, fsr, err);
-               return err;
-       }
-
-       return count;
-}
-
-static ssize_t mpu3050_enable_store(struct device *dev,
-                                   struct device_attribute *attr,
-                                   const char *buf, size_t count)
-{
-       struct inv_gyro_state_s *inf;
-       unsigned char enable;
-       int err;
-
-       inf = dev_get_drvdata(dev);
-       err = kstrtou8(buf, 10, &enable);
-       if (err)
-               return -EINVAL;
-
-       if (enable)
-               enable = 1;
-       inf->chip_config.enable = enable;
-       return count;
-}
-
-static DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR,
-                  nvi_gyro_enable_show, mpu3050_gyro_enable_store);
-static DEVICE_ATTR(gyro_fifo_enable, S_IRUGO | S_IWUSR,
-                  nvi_gyro_fifo_enable_show, mpu3050_gyro_fifo_enable_store);
-static DEVICE_ATTR(gyro_max_range, S_IRUGO | S_IWUSR | S_IWOTH,
-                  nvi_gyro_max_range_show, mpu3050_gyro_max_range_store);
-static DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR,
-                  nvi_accl_enable_show, mpu3050_accl_enable_store);
-static DEVICE_ATTR(accl_fifo_enable, S_IRUGO | S_IWUSR,
-                  nvi_accl_fifo_enable_show, mpu3050_accl_fifo_enable_store);
-static DEVICE_ATTR(accl_delay, S_IRUGO | S_IWUSR | S_IWOTH,
-                  nvi_accl_delay_show, mpu3050_accl_delay_store);
-static DEVICE_ATTR(accl_max_range, S_IRUGO | S_IWUSR,
-                  nvi_accl_max_range_show, mpu3050_accl_max_range_store);
-static DEVICE_ATTR(accl_orientation, S_IRUGO,
-                  inv_accl_matrix_show, NULL);
-static DEVICE_ATTR(raw_accl, S_IRUGO,
-                  mpu3050_raw_accl_show, NULL);
-static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWOTH,
-                  nvi_enable_show, mpu3050_enable_store);
-
-static struct device_attribute *inv_mpu3050_attributes[] = {
-       &dev_attr_gyro_enable,
-       &dev_attr_gyro_fifo_enable,
-       &dev_attr_gyro_max_range,
-       &dev_attr_accl_enable,
-       &dev_attr_accl_fifo_enable,
-       &dev_attr_accl_delay,
-       &dev_attr_accl_max_range,
-       &dev_attr_accl_orientation,
-       &dev_attr_raw_accl,
-       &dev_attr_enable,
-       NULL
-};
-
-int inv_mpu3050_create_sysfs(struct inv_gyro_state_s *st)
-{
-       int result;
-       result = create_device_attributes(st->inv_dev, inv_mpu3050_attributes);
-       if (result)
-               return result;
-       return result;
-}
-int inv_mpu3050_remove_sysfs(struct inv_gyro_state_s *st)
-{
-       remove_device_attributes(st->inv_dev, inv_mpu3050_attributes);
-       return 0;
-}
-
-static void mpu3050_report(struct inv_gyro_state_s *st, s64 t,
-                          int counter, unsigned char *data)
-{
-       short x = 0, y = 0, z = 0;
-       int ind;
-       short out[3];
-       struct inv_chip_config_s *conf;
-       conf = &st->chip_config;
-       ind = 0;
-       if (counter)
-               ind += 2;
-       if (conf->gyro_fifo_enable | conf->dmp_on) {
-               x = (data[ind] << 8)     | data[ind + 1];
-               y = (data[ind + 2] << 8) | data[ind + 3];
-               z = (data[ind + 4] << 8) | data[ind + 5];
-               if (conf->gyro_fifo_enable) {
-                       /*it is possible that gyro disabled when dmp is on*/
-                       input_report_rel(st->idev, REL_X,  x);
-                       input_report_rel(st->idev, REL_Y,  y);
-                       input_report_rel(st->idev, REL_Z,  z);
-               }
-               ind += 6;
-       }
-
-       if (conf->accl_fifo_enable | conf->dmp_on) {
-               if (st->mpu_slave != NULL) {
-                       st->mpu_slave->combine_data(&data[ind], out);
-                       x = out[0];
-                       y = out[1];
-                       z = out[2];
-               }
-               if (conf->accl_fifo_enable) {
-                       /*it is possible that accl disabled when dmp is on*/
-                       input_report_rel(st->idev, REL_RX,  x);
-                       input_report_rel(st->idev, REL_RY,  y);
-                       input_report_rel(st->idev, REL_RZ,  z);
-               }
-               ind += 6;
-       }
-       if (conf->dmp_on) {
-               /* report tap information */
-               if (data[ind + 1] & 1) {
-                       input_report_rel(st->idev_dmp, REL_RX, data[ind+3]);
-                       input_sync(st->idev_dmp);
-               }
-               /* report orientation information */
-               if (data[ind + 1] & 2) {
-                       input_report_rel(st->idev_dmp, REL_RY, data[ind+2]);
-                       input_sync(st->idev_dmp);
-               }
-       }
-       /* always report time */
-       {
-               input_report_rel(st->idev, REL_MISC, (unsigned int)(t >> 32));
-               input_report_rel(st->idev, REL_WHEEL,
-                       (unsigned int)(t & 0xffffffff));
-               input_sync(st->idev);
-       }
-}
-
-/**
- *  inv_read_fifo() - Transfer data from FIFO to ring buffer.
- */
-irqreturn_t inv_read_fifo_mpu3050(int irq, void *dev_id)
-{
-       struct inv_gyro_state_s *st;
-       unsigned char bytes_per_datum;
-       const unsigned short fifo_thresh = 500;
-       int result;
-       unsigned char data[16];
-       short fifo_count, byte_read;
-       unsigned int copied;
-       s64 timestamp;
-       struct inv_reg_map_s *reg;
-
-       st = (struct inv_gyro_state_s *)dev_id;
-       reg = st->reg;
-
-       if (!(st->chip_config.accl_fifo_enable |
-               st->chip_config.gyro_fifo_enable |
-               st->chip_config.dmp_on))
-               goto end_session;
-
-       result = inv_i2c_read(st, reg->temperature, 2, data);
-       if (!result) {
-               mutex_lock(&st->mutex_temp);
-               st->temp_val = (data[0] << 8) | data[1];
-               st->temp_ts = timestamp;
-               mutex_unlock(&st->mutex_temp);
-       }
-
-       if (st->chip_config.dmp_on)
-               bytes_per_datum = BYTES_FOR_DMP;
-       else
-               bytes_per_datum = (st->chip_config.accl_fifo_enable +
-                                       st->chip_config.gyro_fifo_enable)*
-                                       BYTES_PER_SENSOR;
-       if (st->fifo_counter == 0)
-               byte_read = bytes_per_datum;
-       else
-               byte_read = bytes_per_datum + 2;
-
-       fifo_count = 0;
-       if (byte_read != 0) {
-               result = inv_i2c_read(st, reg->fifo_count_h, 2, data);
-               if (result)
-                       goto end_session;
-               fifo_count = (data[0] << 8) + data[1];
-               if (fifo_count < byte_read)
-                       goto end_session;
-               if (fifo_count%2)
-                       goto flush_fifo;
-               if (fifo_count > fifo_thresh)
-                       goto flush_fifo;
-               /* Timestamp mismatch. */
-               if (kfifo_len(&st->trigger.timestamps) <
-                       fifo_count / byte_read)
-                       goto flush_fifo;
-               if (kfifo_len(&st->trigger.timestamps) >
-                       fifo_count / byte_read + TIME_STAMP_TOR) {
-                       if (st->chip_config.dmp_on) {
-                               result = kfifo_to_user(&st->trigger.timestamps,
-                               &timestamp, sizeof(timestamp), &copied);
-                               if (result)
-                                       goto flush_fifo;
-                       } else
-                               goto flush_fifo;
-               }
-       }
-
-       while ((bytes_per_datum != 0) && (fifo_count >= byte_read)) {
-               result = inv_i2c_read(st, reg->fifo_r_w, byte_read, data);
-               if (result)
-                       goto flush_fifo;
-
-               result = kfifo_to_user(&st->trigger.timestamps,
-                       &timestamp, sizeof(timestamp), &copied);
-               if (result)
-                       goto flush_fifo;
-               mpu3050_report(st, timestamp, st->fifo_counter, data);
-               fifo_count -= byte_read;
-               if (st->fifo_counter == 0) {
-                       st->fifo_counter = 1;
-                       byte_read = bytes_per_datum + 2;
-               }
-       }
-end_session:
-       return IRQ_HANDLED;
-flush_fifo:
-       /* Flush HW and SW FIFOs. */
-       mpu3050_fifo_reset(st);
-       return IRQ_HANDLED;
-}
 void inv_setup_reg_mpu3050(struct inv_reg_map_s *reg)
 {
-       reg->fifo_en         = 0x12;
-       reg->sample_rate_div = 0x15;
-       reg->lpf             = 0x16;
-       reg->fifo_count_h    = 0x3a;
-       reg->fifo_r_w        = 0x3c;
-       reg->user_ctrl       = 0x3d;
-       reg->pwr_mgmt_1      = 0x3e;
-       reg->raw_gyro        = 0x1d;
-       reg->raw_accl        = 0x23;
-       reg->temperature     = 0x1b;
-       reg->int_enable      = 0x17;
-       reg->int_status      = 0x1a;
+       reg->who_am_i           = 0x00;
+       reg->fifo_en            = 0x12;
+       reg->sample_rate_div    = 0x15;
+       reg->lpf                = 0x16;
+       reg->fifo_count_h       = 0x3A;
+       reg->fifo_r_w           = 0x3C;
+       reg->user_ctrl          = 0x3D;
+       reg->pwr_mgmt_1         = 0x3E;
+       reg->raw_gyro           = 0x1D;
+       reg->raw_accl           = 0x23;
+       reg->temperature        = 0x1B;
+       reg->int_enable         = 0x17;
+       reg->int_status         = 0x1A;
+
+       reg->accl_fifo_en       = BITS_3050_ACCL_OUT;
+       reg->fifo_reset         = BIT_3050_FIFO_RST;
+       reg->i2c_mst_reset      = BIT_3050_AUX_IF_RST;
+       reg->cycle              = 0;
 }
 
 /**
  *  inv_init_config_mpu3050() - Initialize hardware, disable FIFO.
  *  @st:       Device driver instance.
- *  Initial configuration:
- *  FSR: +/- 2000DPS
- *  DLPF: 42Hz
- *  FIFO rate: 50Hz
- *  Clock source: Gyro PLL
  */
 int inv_init_config_mpu3050(struct inv_gyro_state_s *st)
 {
-       struct inv_reg_map_s *reg;
+       u8 data;
        int result;
-       unsigned char data;
+
+       st->chip_config.fifo_thr = FIFO_THRESHOLD;
 
        /*reading AUX VDDIO register */
        result = inv_i2c_read(st, REG_3050_AUX_VDDIO, 1, &data);
        if (result)
                return result;
+
        data &= ~BIT_3050_VDDIO;
        data |= (st->plat_data.level_shifter << 2);
        result = inv_i2c_single_write(st, REG_3050_AUX_VDDIO, data);
        if (result)
                return result;
 
-       reg = st->reg;
-       result = set_inv_enable(st, 0);
-       if (result)
-               return result;
-       /*2000dps full scale range*/
-       result = inv_i2c_single_write(st, reg->lpf, (INV_FSR_2000DPS << 3)
-                                       | INV_FILTER_42HZ);
-       if (result)
-               return result;
-       st->chip_config.gyro_fsr = INV_FSR_2000DPS;
-       st->chip_config.lpf = INV_FILTER_42HZ;
-       result = inv_i2c_single_write(st, reg->sample_rate_div, 19);
-       if (result)
-               return result;
-       st->chip_config.fifo_rate = 50;
-       st->irq_dur_us            = 20*1000;
-       st->chip_config.enable = 0;
-       st->chip_config.dmp_on = 0;
-       st->chip_config.firmware_loaded = 0;
-       st->chip_config.prog_start_addr = DMP_START_ADDR;
-       st->chip_config.gyro_enable = 1;
-       st->chip_config.gyro_fifo_enable = 1;
        if (SECONDARY_SLAVE_TYPE_ACCEL == st->plat_data.sec_slave_type) {
+               if (st->plat_data.sec_slave_id == ACCEL_ID_KXTF9)
+                       inv_register_kxtf9_slave(st);
                if (st->mpu_slave != NULL) {
                        result = st->mpu_slave->setup(st);
                        if (result)
                                return result;
-
-                       result = st->mpu_slave->set_fs(st, 0);
-                       if (result)
-                               return result;
-
-                       result = st->mpu_slave->set_lpf(st, 50);
-                       if (result)
-                               return result;
                }
-
-               st->chip_config.accl_enable = 1;
-               st->chip_config.accl_fifo_enable = 1;
-       } else {
-               st->chip_config.accl_enable = 0;
-               st->chip_config.accl_fifo_enable = 0;
        }
        return 0;
 }
 
-int set_3050_bypass(struct inv_gyro_state_s *st, int enable)
+int set_3050_bypass(struct inv_gyro_state_s *inf, int enable)
 {
        struct inv_reg_map_s *reg;
-       int result;
-       unsigned char b;
+       u8 user_ctrl;
+       int err;
+       int err_t = 0;
 
-       reg = st->reg;
-       result = inv_i2c_read(st, reg->user_ctrl, 1, &b);
-       if (result)
-               return result;
-       if (((b & BIT_3050_AUX_IF_EN) == 0) && enable)
+       reg = inf->reg;
+       if (((inf->hw.user_ctrl & BIT_3050_AUX_IF_EN) == 0) && enable)
                return 0;
-       if ((b & BIT_3050_AUX_IF_EN) && (enable == 0))
+
+       if ((inf->hw.user_ctrl & BIT_3050_AUX_IF_EN) && (enable == 0))
                return 0;
-       b &= ~BIT_3050_AUX_IF_EN;
+
+       user_ctrl = inf->hw.user_ctrl;
+       user_ctrl &= ~BIT_3050_AUX_IF_EN;
        if (!enable) {
-               b |= BIT_3050_AUX_IF_EN;
-               result = inv_i2c_single_write(st, reg->user_ctrl, b);
-               return result;
+               user_ctrl |= BIT_3050_AUX_IF_EN;
+               err_t = inv_i2c_single_write(inf, reg->user_ctrl, user_ctrl);
+               if (!err_t) {
+                       inf->hw.user_ctrl = user_ctrl;
+                       inf->aux.en3050 = true;
+               }
        } else {
-               /* Coming out of I2C is tricky due to several erratta.  Do not
-               * modify this algorithm
-               */
-               /*
+               inf->aux.en3050 = false;
+               /* Coming out of I2C is tricky due to several erratta.
+               * Do not modify this algorithm
                * 1) wait for the right time and send the command to change
                * the aux i2c slave address to an invalid address that will
                * get nack'ed
                *
                * 0x00 is broadcast.  0x7F is unlikely to be used by any aux.
                */
-               result = inv_i2c_single_write(st, REG_3050_SLAVE_ADDR,
-                                               0x7F);
-               if (result)
-                       return result;
+               err_t = inv_i2c_single_write(inf, REG_3050_SLAVE_ADDR, 0x7F);
                /*
                * 2) wait enough time for a nack to occur, then go into
                *    bypass mode:
                */
                mdelay(2);
-               result = inv_i2c_single_write(st, reg->user_ctrl, b);
-               if (result)
-                       return result;
+               err = inv_i2c_single_write(inf, reg->user_ctrl, user_ctrl);
+               if (!err)
+                       inf->hw.user_ctrl = user_ctrl;
+               else
+                       err_t |= err;
                /*
                * 3) wait for up to one MPU cycle then restore the slave
                *    address
                */
                mdelay(20);
-               result = inv_i2c_single_write(st, REG_3050_SLAVE_REG,
-                                    st->plat_data.secondary_read_reg);
-               if (result)
-                       return result;
-
-               result = inv_i2c_single_write(st, REG_3050_SLAVE_ADDR,
-                       st->plat_data.secondary_i2c_addr);
-               if (result)
-                       return result;
-
-               result = inv_i2c_single_write(st, reg->user_ctrl,
-                                               (b | BIT_3050_AUX_IF_RST));
-               if (result)
-                       return result;
-
+               err_t |= inv_i2c_single_write(inf, REG_3050_SLAVE_REG,
+                                           inf->plat_data.secondary_read_reg);
+               err_t |= inv_i2c_single_write(inf, REG_3050_SLAVE_ADDR,
+                                           inf->plat_data.secondary_i2c_addr);
+               err = inv_i2c_single_write(inf, reg->user_ctrl,
+                                          (user_ctrl | BIT_3050_AUX_IF_RST));
+               if (!err)
+                       inf->hw.user_ctrl = user_ctrl;
+               else
+                       err_t |= err;
                mdelay(2);
        }
-       return 0;
+       return err_t;
 }
 /**
  *  @}
index 25306d1..5c40f8d 100644 (file)
 #include <linux/delay.h>
 #include <linux/workqueue.h>
 #include <linux/regulator/consumer.h>
+#include <asm/div64.h>
 #include <linux/mpu.h>
 
 
+#define BMP180_RANGE_DFLT              (0)
+/* until OSS is supported in the pressure calculation, this defaults to 5 */
+#define BMP280_RANGE_DFLT              (5)
+
+#define BMPX80_NAME                    "bmpX80"
 #define BMP180_NAME                    "bmp180"
-#define BMP180_I2C_ADDR                        (0x77)
-#define BMP180_HW_DELAY_MS             (10)
+#define BMP280_NAME                    "bmp280"
+#define BMP180_I2C_ADDR0               (0x76)
+#define BMP180_I2C_ADDR1               (0x77)
+#define BMPX80_HW_DELAY_POR_MS         (10)
 #define BMP180_POLL_DELAY_MS_DFLT      (200)
 #define BMP180_MPU_RETRY_COUNT         (20)
-#define BMP180_MPU_RETRY_DELAY_MS      (20)
+#define BMP180_MPU_RETRY_DELAY_MS      (100)
 #define BMP180_ERR_CNT_MAX             (20)
 /* sampling delays */
-#define BMP180_DELAY_ULP               (5)
-#define        BMP180_DELAY_ST                 (8)
-#define        BMP180_DELAY_HIGH_RES           (14)
-#define        BMP180_DELAY_UHIGH_RES          (26)
+#define BMP180_DELAY_MS_ULP            (5)
+#define BMP180_DELAY_MS_ST             (8)
+#define BMP180_DELAY_MS_HIGH_RES       (14)
+#define BMP180_DELAY_MS_UHIGH_RES      (26)
+#define BMP280_DELAY_MS_ULP            (9)
+#define BMP280_DELAY_MS_LP             (12)
+#define BMP280_DELAY_MS_ST             (18)
+#define BMP280_DELAY_MS_HIGH_RES       (30)
+#define BMP280_DELAY_MS_UHIGH_RES      (57)
 /* input poll values*/
 #define BMP180_INPUT_RESOLUTION                (1)
 #define BMP180_INPUT_DIVISOR           (100)
-#define BMP180_INPUT_DELAY_MS_MIN      (BMP180_DELAY_UHIGH_RES)
+#define BMP280_INPUT_DIVISOR           (256)
+#define BMP180_INPUT_DELAY_MS_MIN      (BMP180_DELAY_MS_UHIGH_RES)
 #define BMP180_INPUT_POWER_UA          (12)
 #define BMP180_PRESSURE_MIN            (30000)
 #define BMP180_PRESSURE_MAX            (110000)
 #define BMP180_PRESSURE_FUZZ           (5)
 #define BMP180_PRESSURE_FLAT           (5)
-/* BMP180 registers */
-#define BMP180_REG_ID                  (0xD0)
-#define BMP180_REG_ID_VAL              (0x55)
-#define BMP180_REG_RESET               (0xE0)
-#define BMP180_REG_RESET_VAL           (0xB6)
-#define BMP180_REG_CTRL                        (0xF4)
+/* BMPX80 registers */
+#define BMPX80_REG_ID                  (0xD0)
+#define BMPX80_REG_ID_BMP180           (0x55)
+#define BMPX80_REG_ID_BMP280           (0x56)
+#define BMPX80_REG_RESET               (0xE0)
+#define BMPX80_REG_RESET_VAL           (0xB6)
+#define BMPX80_REG_CTRL                        (0xF4)
 #define BMP180_REG_CTRL_MODE_MASK      (0x1F)
 #define BMP180_REG_CTRL_MODE_PRES      (0x34)
 #define BMP180_REG_CTRL_MODE_TEMP      (0x2E)
-#define BMP180_REG_CTRL_OSS            (6)
 #define BMP180_REG_CTRL_SCO            (5)
+#define BMP180_REG_CTRL_OSS            (6)
+#define BMP280_REG_CTRL_MODE_MASK      (0x03)
+#define BMP280_REG_CTRL_MODE_SLEEP     (0)
+#define BMP280_REG_CTRL_MODE_FORCED1   (1)
+#define BMP280_REG_CTRL_MODE_FORCED2   (2)
+#define BMP280_REG_CTRL_MODE_NORMAL    (3)
+#define BMP280_REG_CTRL_OSRS_P         (2)
+#define BMP280_REG_CTRL_OSRS_P_MASK    (0x1C)
+#define BMP280_REG_CTRL_OSRS_T         (5)
+#define BMP280_REG_CTRL_OSRS_T_MASK    (0xE0)
 #define BMP180_REG_OUT_MSB             (0xF6)
 #define BMP180_REG_OUT_LSB             (0xF7)
 #define BMP180_REG_OUT_XLSB            (0xF8)
+#define BMP280_REG_STATUS              (0xF3)
+#define BMP280_REG_STATUS_MEASURING    (3)
+#define BMP280_REG_STATUS_IM_UPDATE    (0)
+#define BMP280_REG_CONFIG              (0xF5)
+#define BMP280_REG_PRESS_MSB           (0xF7)
+#define BMP280_REG_PRESS_LSB           (0xF8)
+#define BMP280_REG_PRESS_XLSB          (0xF9)
+#define BMP280_REG_TEMP_MSB            (0xFA)
+#define BMP280_REG_TEMP_LSB            (0xFB)
+#define BMP280_REG_TEMP_XLSB           (0xFC)
+
 /* ROM registers */
 #define BMP180_REG_AC1                 (0xAA)
 #define BMP180_REG_AC2                 (0xAC)
 #define BMP180_REG_MB                  (0xBA)
 #define BMP180_REG_MC                  (0xBC)
 #define BMP180_REG_MD                  (0xBE)
+#define BMP280_REG_CWORD00             (0x88)
+#define BMP280_REG_CWORD01             (0x8A)
+#define BMP280_REG_CWORD02             (0x8C)
+#define BMP280_REG_CWORD03             (0x8E)
+#define BMP280_REG_CWORD04             (0x90)
+#define BMP280_REG_CWORD05             (0x92)
+#define BMP280_REG_CWORD06             (0x94)
+#define BMP280_REG_CWORD07             (0x96)
+#define BMP280_REG_CWORD08             (0x98)
+#define BMP280_REG_CWORD09             (0x9A)
+#define BMP280_REG_CWORD10             (0x9C)
+#define BMP280_REG_CWORD11             (0x9E)
+#define BMP280_REG_CWORD12             (0xA0)
 
 #define WR                             (0)
 #define RD                             (1)
 
+enum BMP_DATA_INFO {
+       BMP_DATA_INFO_PRESSURE = 0,
+       BMP_DATA_INFO_TEMPERATURE,
+       BMP_DATA_INFO_PRESSURE_RAW,
+       BMP_DATA_INFO_TEMPERATURE_RAW,
+       BMP_DATA_INFO_TEMPERATURE_FINE,
+       BMP_DATA_INFO_CALIBRATION,
+       BMP_DATA_INFO_RESET,
+       BMP_DATA_INFO_REGISTERS,
+       BMP_DATA_INFO_LIMIT_MAX,
+};
+
+
+static u8 bmp_ids[] = {
+       BMPX80_REG_ID_BMP180,
+       BMPX80_REG_ID_BMP280,
+};
 
 /* regulator names in order of powering on */
-static char *bmp180_vregs[] = {
+static char *bmp_vregs[] = {
        "vdd",
        "vddio",
 };
 
-static unsigned long bmp180_delay_ms_tbl[] = {
-       BMP180_DELAY_ULP,
-       BMP180_DELAY_ST,
-       BMP180_DELAY_HIGH_RES,
-       BMP180_DELAY_UHIGH_RES,
+static char *bmp_configs[] = {
+       "auto",
+       "mpu",
+       "host",
 };
 
+static unsigned long bmp180_delay_ms_tbl[] = {
+       BMP180_DELAY_MS_ULP,
+       BMP180_DELAY_MS_ST,
+       BMP180_DELAY_MS_HIGH_RES,
+       BMP180_DELAY_MS_UHIGH_RES,
+};
 
-struct bmp180_rom {
-       s16 ac1;
-       s16 ac2;
-       s16 ac3;
-       u16 ac4;
-       u16 ac5;
-       u16 ac6;
-       s16 b1;
-       s16 b2;
-       s16 mb;
-       s16 mc;
-       s16 md;
+static unsigned long bmp280_delay_ms_tbl[] = {
+       BMP280_DELAY_MS_ULP,
+       BMP280_DELAY_MS_LP,
+       BMP280_DELAY_MS_ST,
+       BMP280_DELAY_MS_HIGH_RES,
+       BMP280_DELAY_MS_UHIGH_RES,
 };
 
-struct bmp180_inf {
+union bmp_rom {
+       struct bmp180_rom {
+               s16 ac1;
+               s16 ac2;
+               s16 ac3;
+               u16 ac4;
+               u16 ac5;
+               u16 ac6;
+               s16 b1;
+               s16 b2;
+               s16 mb;
+               s16 mc;
+               s16 md;
+       } bmp180;
+       struct bmp280_rom {
+               u16 dig_T1;
+               s16 dig_T2;
+               s16 dig_T3;
+               u16 dig_P1;
+               s16 dig_P2;
+               s16 dig_P3;
+               s16 dig_P4;
+               s16 dig_P5;
+               s16 dig_P6;
+               s16 dig_P7;
+               s16 dig_P8;
+               s16 dig_P9;
+               s16 reserved;
+       } bmp280;
+} rom;
+
+struct bmp_inf {
        struct i2c_client *i2c;
        struct input_dev *idev;
        struct workqueue_struct *wq;
        struct delayed_work dw;
-       struct regulator_bulk_data vreg[ARRAY_SIZE(bmp180_vregs)];
+       struct regulator_bulk_data vreg[ARRAY_SIZE(bmp_vregs)];
        struct mpu_platform_data pdata;
-       struct bmp180_rom rom;          /* data for calibration */
-       unsigned long poll_delay_us;    /* requested sampling delay (us) */
+       struct bmp_hal *hal;            /* Hardware Abstaction Layer */
+       union bmp_rom rom;              /* calibration data */
+       unsigned int poll_delay_us;     /* requested sampling delay (us) */
+       unsigned int range_user;        /* user oversampling value */
+       unsigned int range_i;           /* oversampling value */
        unsigned int resolution;        /* report when new data outside this */
+       unsigned int data_info;         /* data info to return */
+       unsigned int dev_id;            /* device ID */
        bool use_mpu;                   /* if device behind MPU */
        bool initd;                     /* set if initialized */
        bool enable;                    /* requested enable value */
+       bool fifo_enable;               /* MPU FIFO enable */
        bool report;                    /* used to report first valid sample */
        bool port_en[2];                /* enable status of MPU write port */
        int port_id[2];                 /* MPU port ID */
-       u8 data_out;                    /* write value to trigger a sample */
-       u8 range_index;                 /* oversampling value */
+       u8 data_out;                    /* write value to mode register */
        long UT;                        /* uncompensated temperature */
        long UP;                        /* uncompensated pressure */
+       s32 t_fine;                     /* temperature used in pressure calc */
        long temperature;               /* true temperature */
        int pressure;                   /* true pressure hPa/100 Pa/1 mBar */
 };
 
+struct bmp_hal {
+       u8 rom_addr_start;
+       u8 rom_size;
+       unsigned long *bmp_delay_ms_tbl;
+       unsigned int divisor;
+       unsigned int range_limit;
+       unsigned int range_dflt;
+       int (*bmp_read)(struct bmp_inf *inf);
+       int (*bmp_mode_wr_mpu)(struct bmp_inf *inf, u8 mode);
+};
 
-static int bmp180_i2c_rd(struct bmp180_inf *inf, u8 reg, u16 len, u8 *val)
+
+static int bmp_i2c_rd(struct bmp_inf *inf, u8 reg, u16 len, u8 *val)
 {
        struct i2c_msg msg[2];
 
-       msg[0].addr = BMP180_I2C_ADDR;
+       msg[0].addr = inf->i2c->addr;
        msg[0].flags = 0;
        msg[0].len = 1;
        msg[0].buf = &reg;
-       msg[1].addr = BMP180_I2C_ADDR;
+       msg[1].addr = inf->i2c->addr;
        msg[1].flags = I2C_M_RD;
        msg[1].len = len;
        msg[1].buf = val;
@@ -144,14 +256,14 @@ static int bmp180_i2c_rd(struct bmp180_inf *inf, u8 reg, u16 len, u8 *val)
        return 0;
 }
 
-static int bmp180_i2c_wr(struct bmp180_inf *inf, u8 reg, u8 val)
+static int bmp_i2c_wr(struct bmp_inf *inf, u8 reg, u8 val)
 {
        struct i2c_msg msg;
        u8 buf[2];
 
        buf[0] = reg;
        buf[1] = val;
-       msg.addr = BMP180_I2C_ADDR;
+       msg.addr = inf->i2c->addr;
        msg.flags = 0;
        msg.len = 2;
        msg.buf = buf;
@@ -161,7 +273,7 @@ static int bmp180_i2c_wr(struct bmp180_inf *inf, u8 reg, u8 val)
        return 0;
 }
 
-static int bmp180_vreg_dis(struct bmp180_inf *inf, int i)
+static int bmp_vreg_dis(struct bmp_inf *inf, int i)
 {
        int err = 0;
 
@@ -178,17 +290,17 @@ static int bmp180_vreg_dis(struct bmp180_inf *inf, int i)
        return err;
 }
 
-static int bmp180_vreg_dis_all(struct bmp180_inf *inf)
+static int bmp_vreg_dis_all(struct bmp_inf *inf)
 {
        unsigned int i;
        int err = 0;
 
-       for (i = ARRAY_SIZE(bmp180_vregs); i > 0; i--)
-               err |= bmp180_vreg_dis(inf, (i - 1));
+       for (i = ARRAY_SIZE(bmp_vregs); i > 0; i--)
+               err |= bmp_vreg_dis(inf, (i - 1));
        return err;
 }
 
-static int bmp180_vreg_en(struct bmp180_inf *inf, int i)
+static int bmp_vreg_en(struct bmp_inf *inf, int i)
 {
        int err = 0;
 
@@ -207,21 +319,21 @@ static int bmp180_vreg_en(struct bmp180_inf *inf, int i)
        return err;
 }
 
-static int bmp180_vreg_en_all(struct bmp180_inf *inf)
+static int bmp_vreg_en_all(struct bmp_inf *inf)
 {
        int i;
        int err = 0;
 
-       for (i = 0; i < ARRAY_SIZE(bmp180_vregs); i++)
-               err |= bmp180_vreg_en(inf, i);
+       for (i = 0; i < ARRAY_SIZE(bmp_vregs); i++)
+               err |= bmp_vreg_en(inf, i);
        return err;
 }
 
-static void bmp180_vreg_exit(struct bmp180_inf *inf)
+static void bmp_vreg_exit(struct bmp_inf *inf)
 {
        int i;
 
-       for (i = 0; i < ARRAY_SIZE(bmp180_vregs); i++) {
+       for (i = 0; i < ARRAY_SIZE(bmp_vregs); i++) {
                if (inf->vreg[i].consumer != NULL) {
                        devm_regulator_put(inf->vreg[i].consumer);
                        inf->vreg[i].consumer = NULL;
@@ -231,13 +343,13 @@ static void bmp180_vreg_exit(struct bmp180_inf *inf)
        }
 }
 
-static int bmp180_vreg_init(struct bmp180_inf *inf)
+static int bmp_vreg_init(struct bmp_inf *inf)
 {
        unsigned int i;
        int err = 0;
 
-       for (i = 0; i < ARRAY_SIZE(bmp180_vregs); i++) {
-               inf->vreg[i].supply = bmp180_vregs[i];
+       for (i = 0; i < ARRAY_SIZE(bmp_vregs); i++) {
+               inf->vreg[i].supply = bmp_vregs[i];
                inf->vreg[i].ret = 0;
                inf->vreg[i].consumer = devm_regulator_get(&inf->i2c->dev,
                                                          inf->vreg[i].supply);
@@ -254,16 +366,16 @@ static int bmp180_vreg_init(struct bmp180_inf *inf)
        return err;
 }
 
-static int bmp180_pm(struct bmp180_inf *inf, bool enable)
+static int bmp_pm(struct bmp_inf *inf, bool enable)
 {
        int err;
 
        if (enable) {
-               err = bmp180_vreg_en_all(inf);
+               err = bmp_vreg_en_all(inf);
                if (err)
-                       mdelay(BMP180_HW_DELAY_MS);
+                       mdelay(BMPX80_HW_DELAY_POR_MS);
        } else {
-               err = bmp180_vreg_dis_all(inf);
+               err = bmp_vreg_dis_all(inf);
        }
        if (err > 0)
                err = 0;
@@ -276,7 +388,7 @@ static int bmp180_pm(struct bmp180_inf *inf, bool enable)
        return err;
 }
 
-static int bmp180_port_free(struct bmp180_inf *inf, int port)
+static int bmp_port_free(struct bmp_inf *inf, int port)
 {
        int err = 0;
 
@@ -288,41 +400,41 @@ static int bmp180_port_free(struct bmp180_inf *inf, int port)
        return err;
 }
 
-static int bmp180_ports_free(struct bmp180_inf *inf)
+static int bmp_ports_free(struct bmp_inf *inf)
 {
        int err;
 
-       err = bmp180_port_free(inf, WR);
-       err |= bmp180_port_free(inf, RD);
+       err = bmp_port_free(inf, WR);
+       err |= bmp_port_free(inf, RD);
        return err;
 }
 
-static void bmp180_pm_exit(struct bmp180_inf *inf)
+static void bmp_pm_exit(struct bmp_inf *inf)
 {
-       bmp180_ports_free(inf);
-       bmp180_pm(inf, false);
-       bmp180_vreg_exit(inf);
+       bmp_ports_free(inf);
+       bmp_pm(inf, false);
+       bmp_vreg_exit(inf);
 }
 
-static int bmp180_pm_init(struct bmp180_inf *inf)
+static int bmp_pm_init(struct bmp_inf *inf)
 {
        int err;
 
        inf->initd = false;
        inf->enable = false;
+       inf->fifo_enable = false; /* DON'T ENABLE: MPU FIFO HW BROKEN */
        inf->port_en[WR] = false;
        inf->port_en[RD] = false;
        inf->port_id[WR] = -1;
        inf->port_id[RD] = -1;
        inf->resolution = 0;
-       inf->range_index = 0;
        inf->poll_delay_us = (BMP180_POLL_DELAY_MS_DFLT * 1000);
-       bmp180_vreg_init(inf);
-       err = bmp180_pm(inf, true);
+       bmp_vreg_init(inf);
+       err = bmp_pm(inf, true);
        return err;
 }
 
-static int bmp180_nvi_mpu_bypass_request(struct bmp180_inf *inf)
+static int bmp_nvi_mpu_bypass_request(struct bmp_inf *inf)
 {
        int i;
        int err = 0;
@@ -341,7 +453,7 @@ static int bmp180_nvi_mpu_bypass_request(struct bmp180_inf *inf)
        return err;
 }
 
-static int bmp180_nvi_mpu_bypass_release(struct bmp180_inf *inf)
+static int bmp_nvi_mpu_bypass_release(struct bmp_inf *inf)
 {
        int err = 0;
 
@@ -350,127 +462,220 @@ static int bmp180_nvi_mpu_bypass_release(struct bmp180_inf *inf)
        return err;
 }
 
-static int bmp180_wr(struct bmp180_inf *inf, u8 reg, u8 val)
+static int bmp_wr(struct bmp_inf *inf, u8 reg, u8 val)
 {
        int err = 0;
 
-       err = bmp180_nvi_mpu_bypass_request(inf);
+       err = bmp_nvi_mpu_bypass_request(inf);
        if (!err) {
-               err = bmp180_i2c_wr(inf, reg, val);
-               bmp180_nvi_mpu_bypass_release(inf);
+               err = bmp_i2c_wr(inf, reg, val);
+               bmp_nvi_mpu_bypass_release(inf);
        }
        return err;
 }
 
-static int bmp180_port_enable(struct bmp180_inf *inf, int port, bool enable)
+static int bmp_port_enable(struct bmp_inf *inf, int port, bool enable)
 {
        int err = 0;
 
        if (enable != inf->port_en[port]) {
-               err = nvi_mpu_enable(inf->port_id[port], enable, false);
+               err = nvi_mpu_enable(inf->port_id[port],
+                                    enable, inf->fifo_enable);
                if (!err)
                        inf->port_en[port] = enable;
        }
        return err;
 }
 
-static int bmp180_ports_enable(struct bmp180_inf *inf, bool enable)
+static int bmp_ports_enable(struct bmp_inf *inf, bool enable)
 {
        int err;
 
-       err = bmp180_port_enable(inf, WR, enable);
-       err |= bmp180_port_enable(inf, RD, enable);
+       err = bmp_port_enable(inf, WR, enable);
+       err |= bmp_port_enable(inf, RD, enable);
        return err;
 }
 
-static int bmp180_reset(struct bmp180_inf *inf)
+static int bmp180_mode_wr_mpu(struct bmp_inf *inf, u8 mode)
 {
        int err = 0;
 
-       if (inf->use_mpu)
-               err = bmp180_ports_enable(inf, false);
-       else
-               cancel_delayed_work_sync(&inf->dw);
-       if (err)
-               return err;
-
-       err = bmp180_wr(inf, BMP180_REG_RESET, BMP180_REG_RESET_VAL);
-       if (!err)
-               mdelay(BMP180_HW_DELAY_MS);
-       if (inf->use_mpu) {
-               err |= nvi_mpu_data_out(inf->port_id[WR],
-                                       BMP180_REG_CTRL_MODE_TEMP);
-               err |= bmp180_ports_enable(inf, true);
-       } else {
-               err = bmp180_wr(inf, BMP180_REG_CTRL,
-                               BMP180_REG_CTRL_MODE_TEMP);
-               queue_delayed_work(inf->wq, &inf->dw,
-                                  usecs_to_jiffies(inf->poll_delay_us));
+       if (mode) {
+               err = nvi_mpu_data_out(inf->port_id[WR], mode);
+               err |= bmp_ports_enable(inf, true);
        }
        return err;
 }
 
-static int bmp180_delay(struct bmp180_inf *inf, unsigned long delay_us)
+static int bmp280_mode_wr_mpu(struct bmp_inf *inf, u8 mode)
 {
+       u8 mode_old;
+       u8 mode_new;
        int err = 0;
 
-       if (inf->use_mpu)
-               err = nvi_mpu_delay_us(inf->port_id[RD], delay_us);
+       mode_old = inf->data_out & BMP280_REG_CTRL_MODE_MASK;
+       mode_new = mode & BMP280_REG_CTRL_MODE_MASK;
+       switch (mode_new) {
+       case BMP280_REG_CTRL_MODE_SLEEP:
+               if (mode_old == BMP280_REG_CTRL_MODE_NORMAL)
+                       err = bmp_wr(inf, BMPX80_REG_CTRL, mode_new);
+               break;
+
+       case BMP280_REG_CTRL_MODE_FORCED1:
+               if (mode_old == BMP280_REG_CTRL_MODE_NORMAL)
+                       err = bmp_wr(inf, BMPX80_REG_CTRL,
+                                    BMP280_REG_CTRL_MODE_SLEEP);
+               err |= nvi_mpu_data_out(inf->port_id[WR], mode);
+               err |= bmp_ports_enable(inf, true);
+               break;
+
+       case BMP280_REG_CTRL_MODE_FORCED2:
+               if (mode_old == BMP280_REG_CTRL_MODE_NORMAL)
+                       err = bmp_wr(inf, BMPX80_REG_CTRL,
+                                    BMP280_REG_CTRL_MODE_SLEEP);
+               err |= nvi_mpu_data_out(inf->port_id[WR], mode);
+               err |= bmp_ports_enable(inf, true);
+               break;
+
+       case BMP280_REG_CTRL_MODE_NORMAL:
+               err = bmp_wr(inf, BMPX80_REG_CTRL, mode);
+               err |= bmp_port_enable(inf, RD, true);
+               break;
+
+       default:
+               err = -EINVAL;
+               break;
+       }
+
        return err;
 }
 
-static int bmp180_init_hw(struct bmp180_inf *inf)
+static int bmp_mode_wr(struct bmp_inf *inf, bool reset, bool enable)
 {
-       u8 *p_rom1;
-       u8 *p_rom2;
-       u8 tmp;
-       int i;
+       u8 mode_new;
        int err = 0;
 
-       inf->UT = 0;
-       inf->UP = 0;
-       inf->temperature = 0;
-       inf->pressure = 0;
-       p_rom1 = (u8 *)&inf->rom.ac1;
-       err = bmp180_nvi_mpu_bypass_request(inf);
-       if (!err) {
-               err = bmp180_i2c_rd(inf, BMP180_REG_AC1, 22, p_rom1);
-               bmp180_nvi_mpu_bypass_release(inf);
+       if (inf->dev_id == BMPX80_REG_ID_BMP280) {
+               if (enable) {
+                       mode_new = inf->range_i + 1;
+                       mode_new = (mode_new << BMP280_REG_CTRL_OSRS_T) |
+                                  (mode_new << BMP280_REG_CTRL_OSRS_P);
+                       mode_new |= BMP280_REG_CTRL_MODE_FORCED1;
+               } else {
+                       mode_new = BMP280_REG_CTRL_MODE_SLEEP;
+               }
+       } else {
+               if (enable) {
+                       mode_new = inf->range_i << BMP180_REG_CTRL_OSS;
+                       mode_new |= BMP180_REG_CTRL_MODE_TEMP;
+               } else {
+                       mode_new = 0;
+               }
        }
+       if ((mode_new == inf->data_out) && (!reset))
+               return err;
+
+       if (inf->use_mpu)
+               err = bmp_ports_enable(inf, false);
+       else
+               cancel_delayed_work_sync(&inf->dw);
        if (err)
                return err;
 
-       for (i = 0; i < 11; i++) {
-               p_rom2 = p_rom1;
-               tmp = *p_rom1;
-               *p_rom1++ = *++p_rom2;
-               *p_rom2 = tmp;
-               p_rom1++;
+       if (reset) {
+               err = bmp_wr(inf, BMPX80_REG_RESET, BMPX80_REG_RESET_VAL);
+               if (!err)
+                       mdelay(BMPX80_HW_DELAY_POR_MS);
        }
-       inf->initd = true;
+       if (inf->use_mpu) {
+               inf->hal->bmp_mode_wr_mpu(inf, mode_new);
+       } else {
+               err = bmp_i2c_wr(inf, BMPX80_REG_CTRL, mode_new);
+               if (enable)
+                       queue_delayed_work(inf->wq, &inf->dw,
+                                        usecs_to_jiffies(inf->poll_delay_us));
+       }
+       if (!err)
+               inf->data_out = mode_new;
        return err;
 }
 
-static void bmp180_calc(struct bmp180_inf *inf)
+static int bmp_delay(struct bmp_inf *inf,
+                    unsigned int delay_us, unsigned int range_user)
+{
+       unsigned int i;
+       int err;
+       int err_t = 0;
+
+       if (!range_user) {
+               for (i = (inf->hal->range_limit - 1); i > 0; i--) {
+                       if (delay_us >= (inf->hal->bmp_delay_ms_tbl[i] * 1000))
+                               break;
+               }
+       } else {
+               i = (range_user - 1);
+       }
+       if (i != inf->range_i) {
+               err = 0;
+               if (inf->use_mpu)
+                       err = nvi_mpu_delay_ms(inf->port_id[WR],
+                                              inf->hal->bmp_delay_ms_tbl[i]);
+               if (err < 0)
+                       err_t |= err;
+               else
+                       inf->range_i = i;
+       }
+       if (delay_us < (inf->hal->bmp_delay_ms_tbl[inf->range_i] * 1000))
+               delay_us = (inf->hal->bmp_delay_ms_tbl[inf->range_i] * 1000);
+       if (delay_us != inf->poll_delay_us) {
+               err = 0;
+               if (inf->use_mpu)
+                       err = nvi_mpu_delay_us(inf->port_id[RD],
+                                              (unsigned long)delay_us);
+               if (err)
+                       err_t |= err;
+               else
+                       inf->poll_delay_us = delay_us;
+       }
+       return err_t;
+}
+
+static s64 bmp_timestamp_ns(void)
+{
+       struct timespec ts;
+       s64 ns;
+
+       ktime_get_ts(&ts);
+       ns = timespec_to_ns(&ts);
+       return ns;
+}
+
+static void bmp_report(struct bmp_inf *inf, s64 ts)
+{
+       input_report_abs(inf->idev, ABS_PRESSURE, inf->pressure);
+       input_sync(inf->idev);
+}
+
+static void bmp180_calc(struct bmp_inf *inf)
 {
        long X1, X2, X3, B3, B5, B6, p;
        unsigned long B4, B7;
        long pressure;
 
-       X1 = ((inf->UT - inf->rom.ac6) * inf->rom.ac5) >> 15;
-       X2 = inf->rom.mc * (1 << 11) / (X1 + inf->rom.md);
+       X1 = ((inf->UT - inf->rom.bmp180.ac6) * inf->rom.bmp180.ac5) >> 15;
+       X2 = inf->rom.bmp180.mc * (1 << 11) / (X1 + inf->rom.bmp180.md);
        B5 = X1 + X2;
        inf->temperature = (B5 + 8) >> 4;
        B6 = B5 - 4000;
-       X1 = (inf->rom.b2 * ((B6 * B6) >> 12)) >> 11;
-       X2 = (inf->rom.ac2 * B6) >> 11;
+       X1 = (inf->rom.bmp180.b2 * ((B6 * B6) >> 12)) >> 11;
+       X2 = (inf->rom.bmp180.ac2 * B6) >> 11;
        X3 = X1 + X2;
-       B3 = ((((inf->rom.ac1 << 2) + X3) << inf->range_index) + 2) >> 2;
-       X1 = (inf->rom.ac3 * B6) >> 13;
-       X2 = (inf->rom.b1 * ((B6 * B6) >> 12)) >> 16;
+       B3 = ((((inf->rom.bmp180.ac1 << 2) + X3) << inf->range_i) + 2) >> 2;
+       X1 = (inf->rom.bmp180.ac3 * B6) >> 13;
+       X2 = (inf->rom.bmp180.b1 * ((B6 * B6) >> 12)) >> 16;
        X3 = ((X1 + X2) + 2) >> 2;
-       B4 = (inf->rom.ac4 * (unsigned long)(X3 + 32768)) >> 15;
-       B7 =&nb