input: misc: fix sysfs permissions

- 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/236327
Reviewed-by: Automatic_Commit_Validation_User
GVS: Gerrit_Virtual_Submit
Reviewed-by: Robert Collins <rcollins@nvidia.com>
Tested-by: Robert Collins <rcollins@nvidia.com>
Reviewed-by: Jon Mayo <jmayo@nvidia.com>

6 files changed, 3515 insertions, 2556 deletions

diff --git a/drivers/input/misc/compass/ak8975_input.c b/drivers/input/misc/compass/ak8975_input.c
index cfa6115caa1c..b882f66514b0 100644
--- a/drivers/input/misc/compass/ak8975_input.c
+++ b/drivers/input/misc/compass/ak8975_input.c
@@ -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)
@@ -77,30 +78,47 @@
 #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,
diff --git a/drivers/input/misc/mpu/inv_gyro.c b/drivers/input/misc/mpu/inv_gyro.c
index cfa76ab46865..65e375f9d2e6 100644
--- a/drivers/input/misc/mpu/inv_gyro.c
+++ b/drivers/input/misc/mpu/inv_gyro.c
@@ -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");
diff --git a/drivers/input/misc/mpu/inv_gyro.h b/drivers/input/misc/mpu/inv_gyro.h
index 9852064f93d6..d4b333d45764 100644
--- a/drivers/input/misc/mpu/inv_gyro.h
+++ b/drivers/input/misc/mpu/inv_gyro.h
@@ -37,7 +37,31 @@
 
 #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.
@@ -64,37 +88,51 @@
  *  @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);
diff --git a/drivers/input/misc/mpu/inv_gyro_misc.c b/drivers/input/misc/mpu/inv_gyro_misc.c
index 0a058dea9b3a..491df7dfd217 100644
--- a/drivers/input/misc/mpu/inv_gyro_misc.c
+++ b/drivers/input/misc/mpu/inv_gyro_misc.c
@@ -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;
 }
 
 /**
diff --git a/drivers/input/misc/mpu/inv_mpu3050.c b/drivers/input/misc/mpu/inv_mpu3050.c
index 205bf456f974..6718e8668eca 100644
--- a/drivers/input/misc/mpu/inv_mpu3050.c
+++ b/drivers/input/misc/mpu/inv_mpu3050.c
@@ -40,910 +40,125 @@
 
 #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;
 }
 /**
  *  @}
diff --git a/drivers/input/misc/pressure/bmp180.c b/drivers/input/misc/pressure/bmp180.c
index 25306d157698..5c40f8d2575d 100644
--- a/drivers/input/misc/pressure/bmp180.c
+++ b/drivers/input/misc/pressure/bmp180.c
@@ -18,44 +18,79 @@
 #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)
@@ -68,73 +103,150 @@
 #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 = ((unsigned long)inf->UP - B3) * (50000 >> inf->range_index);
+	B4 = (inf->rom.bmp180.ac4 * (unsigned long)(X3 + 32768)) >> 15;
+	B7 = ((unsigned long)inf->UP - B3) * (50000 >> inf->range_i);
 	if (B7 < 0x80000000)
 		p = (B7 << 1) / B4;
 	else
@@ -482,13 +687,7 @@ static void bmp180_calc(struct bmp180_inf *inf)
 	inf->pressure = (int)pressure;
 }
 
-static void bmp180_report(struct bmp180_inf *inf, u8 *data, s64 ts)
-{
-	input_report_abs(inf->idev, ABS_PRESSURE, inf->pressure);
-	input_sync(inf->idev);
-}
-
-static int bmp180_read_sts(struct bmp180_inf *inf, u8 *data)
+static int bmp180_read_sts(struct bmp_inf *inf, u8 *data)
 {
 	long val;
 	int limit_lo;
@@ -504,10 +703,10 @@ static int bmp180_read_sts(struct bmp180_inf *inf, u8 *data)
 		if (data[0] == 0x0A) { /* temperature */
 			inf->UT = ((data[2] << 8) + data[3]);
 			inf->data_out = BMP180_REG_CTRL_MODE_PRES |
-				     (inf->range_index << BMP180_REG_CTRL_OSS);
+					(inf->range_i << BMP180_REG_CTRL_OSS);
 		} else { /* pressure */
-			val = ((data[2] << 16) + (data[3] << 8) + data[4]) >>
-							(8 - inf->range_index);
+			val = ((data[2] << 16) + (data[3] << 8) +
+						data[4]) >> (8 - inf->range_i);
 			inf->data_out = BMP180_REG_CTRL_MODE_TEMP;
 			if (inf->resolution && (!inf->report)) {
 				if (inf->UP == val)
@@ -540,26 +739,16 @@ static int bmp180_read_sts(struct bmp180_inf *inf, u8 *data)
 	return err;
 }
 
-static s64 bmp180_timestamp_ns(void)
-{
-	struct timespec ts;
-	s64 ns;
-
-	ktime_get_ts(&ts);
-	ns = timespec_to_ns(&ts);
-	return ns;
-}
-
-static int bmp180_read(struct bmp180_inf *inf)
+static int bmp180_read(struct bmp_inf *inf)
 {
 	long long timestamp1;
 	long long timestamp2;
 	u8 data[5];
 	int err;
 
-	timestamp1 = bmp180_timestamp_ns();
-	err = bmp180_i2c_rd(inf, BMP180_REG_CTRL, 5, data);
-	timestamp2 = bmp180_timestamp_ns();
+	timestamp1 = bmp_timestamp_ns();
+	err = bmp_i2c_rd(inf, BMPX80_REG_CTRL, 5, data);
+	timestamp2 = bmp_timestamp_ns();
 	if (err)
 		return err;
 
@@ -567,24 +756,24 @@ static int bmp180_read(struct bmp180_inf *inf)
 	if (err > 0) {
 		timestamp2 = (timestamp2 - timestamp1) / 2;
 		timestamp1 += timestamp2;
-		bmp180_report(inf, data, timestamp1);
-		bmp180_i2c_wr(inf, BMP180_REG_CTRL, inf->data_out);
+		bmp_report(inf, timestamp1);
+		bmp_i2c_wr(inf, BMPX80_REG_CTRL, inf->data_out);
 	} else if (err < 0) {
-		bmp180_i2c_wr(inf, BMP180_REG_CTRL, inf->data_out);
+		bmp_i2c_wr(inf, BMPX80_REG_CTRL, inf->data_out);
 	}
-	return err;
+	return 0;
 }
 
 static void bmp180_mpu_handler(u8 *data, unsigned int len, s64 ts, void *p_val)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
 	int err;
 
-	inf = (struct bmp180_inf *)p_val;
+	inf = (struct bmp_inf *)p_val;
 	if (inf->enable) {
 		err = bmp180_read_sts(inf, data);
 		if (err > 0) {
-			bmp180_report(inf, data, ts);
+			bmp_report(inf, ts);
 			nvi_mpu_data_out(inf->port_id[WR], inf->data_out);
 		} else if (err < 0) {
 			nvi_mpu_data_out(inf->port_id[WR], inf->data_out);
@@ -592,111 +781,184 @@ static void bmp180_mpu_handler(u8 *data, unsigned int len, s64 ts, void *p_val)
 	}
 }
 
-static int bmp180_id(struct bmp180_inf *inf)
+static void bmp280_calc_temp(struct bmp_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 = BMP180_I2C_ADDR | 0x80;
-		nmp.reg = BMP180_REG_ID;
-		nmp.ctrl = 1;
-		err = nvi_mpu_dev_valid(&nmp, &val);
-		/* see mpu.h for possible return values */
-		if ((err == -EAGAIN) || (err == -EBUSY))
-			return -EAGAIN;
+	s32 adc_T;
+	s32 var1;
+	s32 var2;
+
+	adc_T = (s32)inf->UT;
+	adc_T <<= inf->range_i;
+	adc_T >>= 4;
+	var1 = ((((adc_T >> 3) - ((s32)inf->rom.bmp280.dig_T1 << 1))) *
+					  ((s32)inf->rom.bmp280.dig_T2)) >> 11;
+	var2 = (((((adc_T >> 4) - ((s32)inf->rom.bmp280.dig_T1)) *
+		  ((adc_T >> 4) - ((s32)inf->rom.bmp280.dig_T1))) >> 12) *
+		((s32)inf->rom.bmp280.dig_T3)) >> 14;
+	inf->t_fine = var1 + var2;
+	inf->temperature = (inf->t_fine * 5 + 128) >> 8;
+}
 
-		if (((!err) && (val == BMP180_REG_ID_VAL)) || (err == -EIO))
-			config_boot = NVI_CONFIG_BOOT_MPU;
+static void bmp280_calc_pres(struct bmp_inf *inf)
+{
+	s32 adc_P;
+	s64 var1;
+	s64 var2;
+	s64 p;
+
+	adc_P = (s32)inf->UP;
+	adc_P <<= inf->range_i;
+	adc_P >>= 4;
+	var1 = ((s64)inf->t_fine) - 128000;
+	var2 = var1 * var1 * (s64)inf->rom.bmp280.dig_P6;
+	var2 += ((var1 * (s64)inf->rom.bmp280.dig_P5) << 17);
+	var2 += (((s64)inf->rom.bmp280.dig_P4) << 35);
+	var1 = ((var1 * var1 * (s64)inf->rom.bmp280.dig_P3) >> 8) +
+		((var1 * (s64)inf->rom.bmp280.dig_P2) << 12);
+	var1 = (((((s64)1) << 47) + var1) *
+		((s64)inf->rom.bmp280.dig_P1)) >> 33;
+	if (var1) { /* avoid division by zero */
+		p = 1048576 - adc_P;
+		p = ((p << 31) - var2) * 3125;
+		p = do_div(p, var1);
+		var1 = (((s64)inf->rom.bmp280.dig_P9) *
+			(p >> 13) * (p >> 13)) >> 25;
+		var2 = (((s64)inf->rom.bmp280.dig_P8) * p) >> 19;
+		p = ((p + var1 + var2) >> 8) +
+			(((s64)inf->rom.bmp280.dig_P7) << 4);
+	} else {
+		p = 0;
 	}
-	if (config_boot == NVI_CONFIG_BOOT_MPU) {
-		inf->use_mpu = true;
-		nmp.addr = BMP180_I2C_ADDR | 0x80;
-		nmp.reg = BMP180_REG_CTRL;
-		nmp.ctrl = 5;
-		nmp.data_out = 0;
-		nmp.delay_ms = 0;
-		nmp.delay_us = inf->poll_delay_us;
-		nmp.shutdown_bypass = false;
-		nmp.handler = &bmp180_mpu_handler;
-		nmp.ext_driver = (void *)inf;
-		err = nvi_mpu_port_alloc(&nmp);
-		if (err < 0)
-			return err;
+	inf->pressure = (int)p;
+}
 
-		inf->port_id[RD] = err;
-		nmp.addr = BMP180_I2C_ADDR;
-		nmp.reg = BMP180_REG_CTRL;
-		nmp.ctrl = 1;
-		nmp.data_out = BMP180_REG_CTRL_MODE_TEMP;
-		nmp.delay_ms = bmp180_delay_ms_tbl[inf->range_index];
-		nmp.delay_us = 0;
-		nmp.shutdown_bypass = false;
-		nmp.handler = NULL;
-		nmp.ext_driver = NULL;
-		err = nvi_mpu_port_alloc(&nmp);
-		if (err < 0) {
-			bmp180_ports_free(inf);
-			dev_err(&inf->i2c->dev, "%s ERR %d", __func__, err);
-		} else {
-			inf->port_id[WR] = err;
-			err = 0;
-		}
+static int bmp280_read_sts(struct bmp_inf *inf, u8 *data)
+{
+	u8 sts;
+	int val;
+
+	sts = data[1] & BMP280_REG_CTRL_MODE_MASK;
+	if ((sts == BMP280_REG_CTRL_MODE_FORCED1) ||
+					 (sts == BMP280_REG_CTRL_MODE_FORCED2))
+		return 0;
+
+	val = (data[4] << 12) | (data[5] << 4) | (data[6] >> 4);
+	inf->UP = val;
+	val = (data[7] << 12) | (data[8] << 4) | (data[9] >> 4);
+	inf->UT = val;
+	bmp280_calc_temp(inf);
+	bmp280_calc_pres(inf);
+	return 1;
+}
+
+static int bmp280_read(struct bmp_inf *inf)
+{
+	long long timestamp1;
+	long long timestamp2;
+	u8 data[10];
+	int err;
+
+	timestamp1 = bmp_timestamp_ns();
+	err = bmp_i2c_rd(inf, BMP280_REG_STATUS, 10, data);
+	timestamp2 = bmp_timestamp_ns();
+	if (err)
 		return err;
+
+	err = bmp180_read_sts(inf, data);
+	if (err > 0) {
+		timestamp2 = (timestamp2 - timestamp1) >> 1;
+		timestamp1 += timestamp2;
+		bmp_report(inf, timestamp1);
+		bmp_i2c_wr(inf, BMPX80_REG_CTRL, inf->data_out);
 	}
+	return 0;
+}
 
-	/* NVI_CONFIG_BOOT_EXTERNAL */
-	inf->use_mpu = false;
-	err = bmp180_i2c_rd(inf, BMP180_REG_ID, 1, &val);
-	if ((!err) && (val == BMP180_REG_ID_VAL))
-		return 0;
+static void bmp280_mpu_handler(u8 *data, unsigned int len, s64 ts, void *p_val)
+{
+	struct bmp_inf *inf;
+	int err;
 
-	return -ENODEV;
+	inf = (struct bmp_inf *)p_val;
+	if (inf->enable) {
+		err = bmp280_read_sts(inf, data);
+		if (!err)
+			bmp_report(inf, ts);
+	}
 }
 
-static void bmp180_work(struct work_struct *ws)
+static void bmp_work(struct work_struct *ws)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
 
-	inf = container_of(ws, struct bmp180_inf, dw.work);
-	bmp180_read(inf);
+	inf = container_of(ws, struct bmp_inf, dw.work);
+	inf->hal->bmp_read(inf);
 	queue_delayed_work(inf->wq, &inf->dw,
 			   usecs_to_jiffies(inf->poll_delay_us));
 }
 
-static int bmp180_enable(struct bmp180_inf *inf, bool enable)
+static int bmp_init_hw(struct bmp_inf *inf)
 {
+	u8 *p_rom8;
+	u16 *p_rom16;
+	int i;
 	int err = 0;
 
-	bmp180_pm(inf, true);
+	inf->UT = 0;
+	inf->UP = 0;
+	inf->temperature = 0;
+	inf->pressure = 0;
+	p_rom8 = (u8 *)&inf->rom;
+	err = bmp_nvi_mpu_bypass_request(inf);
+	if (!err) {
+		err = bmp_i2c_rd(inf, inf->hal->rom_addr_start,
+				 inf->hal->rom_size, p_rom8);
+		bmp_nvi_mpu_bypass_release(inf);
+	}
+	if (err)
+		return err;
+
+	p_rom16 = (u16 *)&inf->rom;
+	for (i = 0; i < (inf->hal->rom_size / 2); i++) {
+		*p_rom16 = be16_to_cpup(p_rom16);
+		p_rom16++;
+	}
+	inf->initd = true;
+	return err;
+}
+
+static int bmp_enable(struct bmp_inf *inf, bool enable)
+{
+	int err = 0;
+
+	bmp_pm(inf, true);
 	if (!inf->initd)
-		err = bmp180_init_hw(inf);
-	if (enable) {
-		inf->report = true;
-		err |= bmp180_delay(inf, inf->poll_delay_us);
-		err |= bmp180_reset(inf);
-		if (!err)
-			inf->enable = true;
+		err = bmp_init_hw(inf);
+	if (!err) {
+		if (enable) {
+			inf->report = true;
+			err |= bmp_delay(inf, inf->poll_delay_us,
+					 inf->range_user);
+			err |= bmp_mode_wr(inf, true, true);
+			if (!err)
+				inf->enable = true;
+		} else {
+			inf->enable = false;
+			err = bmp_mode_wr(inf, false, false);
+			if (!err)
+				bmp_pm(inf, false);
+		}
 	} else {
-		inf->enable = false;
-		if (inf->use_mpu)
-			err = bmp180_ports_enable(inf, false);
-		else
-			cancel_delayed_work_sync(&inf->dw);
-		if (!err)
-			bmp180_pm(inf, false);
+		bmp_pm(inf, false);
 	}
 	return err;
 }
 
-static ssize_t bmp180_enable_store(struct device *dev,
-				   struct device_attribute *attr,
-				   const char *buf, size_t count)
+static ssize_t bmp_enable_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
 	unsigned int enable;
 	bool en;
 	int err;
@@ -710,54 +972,52 @@ static ssize_t bmp180_enable_store(struct device *dev,
 		en = true;
 	else
 		en = false;
-	dev_dbg(&inf->i2c->dev, "%s: %x", __func__, en);
-	err = bmp180_enable(inf, en);
+	dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, en);
+	err = bmp_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;
 	}
 
 	return count;
 }
 
-static ssize_t bmp180_enable_show(struct device *dev,
-				  struct device_attribute *attr, char *buf)
+static ssize_t bmp_enable_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
 {
-	struct bmp180_inf *inf;
-	unsigned int enable = 0;
+	struct bmp_inf *inf;
+	unsigned int enable;
 
 	inf = dev_get_drvdata(dev);
 	if (inf->enable)
 		enable = 1;
+	else
+		enable = 0;
 	return sprintf(buf, "%u\n", enable);
 }
 
-static ssize_t bmp180_delay_store(struct device *dev,
-				  struct device_attribute *attr,
-				  const char *buf, size_t count)
+static ssize_t bmp_delay_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
 {
-	struct bmp180_inf *inf;
-	unsigned long delay_us;
+	struct bmp_inf *inf;
+	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);
-	/* since we rotate between acquiring data for pressure and temperature
-	 * we need to go twice as fast.
-	 */
-	delay_us >>= 1;
-	if (delay_us < (bmp180_delay_ms_tbl[inf->range_index] * 1000))
-		delay_us = (bmp180_delay_ms_tbl[inf->range_index] * 1000);
-	if (inf->enable && (delay_us != inf->poll_delay_us))
-		err = bmp180_delay(inf, delay_us);
-	if (!err) {
-		inf->poll_delay_us = delay_us;
-	} else {
-		dev_err(&inf->i2c->dev, "%s: %lu ERR=%d\n",
+	dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, delay_us);
+	if (inf->dev_id == BMPX80_REG_ID_BMP180)
+		/* since we rotate between acquiring data for pressure and
+		 * temperature on the BMP180 we need to go twice as fast.
+		 */
+		delay_us >>= 1;
+	err = bmp_delay(inf, delay_us, inf->range_user);
+	if (err) {
+		dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n",
 			__func__, delay_us, err);
 		return err;
 	}
@@ -765,25 +1025,25 @@ static ssize_t bmp180_delay_store(struct device *dev,
 	return count;
 }
 
-static ssize_t bmp180_delay_show(struct device *dev,
-				 struct device_attribute *attr, char *buf)
+static ssize_t bmp_delay_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
 {
-	struct bmp180_inf *inf;
-	unsigned long delay_us;
+	struct bmp_inf *inf;
+	unsigned int delay_us;
 
 	inf = dev_get_drvdata(dev);
 	if (inf->enable)
 		delay_us = inf->poll_delay_us;
 	else
-		delay_us = bmp180_delay_ms_tbl[inf->range_index] * 1000;
-	return sprintf(buf, "%lu\n", delay_us);
+		delay_us = inf->hal->bmp_delay_ms_tbl[inf->range_i] * 1000;
+	return sprintf(buf, "%u\n", delay_us);
 }
 
-static ssize_t bmp180_resolution_store(struct device *dev,
-				       struct device_attribute *attr,
-				       const char *buf, size_t count)
+static ssize_t bmp_resolution_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
 	unsigned int resolution;
 
 	inf = dev_get_drvdata(dev);
@@ -795,10 +1055,10 @@ static ssize_t bmp180_resolution_store(struct device *dev,
 	return count;
 }
 
-static ssize_t bmp180_resolution_show(struct device *dev,
-				      struct device_attribute *attr, char *buf)
+static ssize_t bmp_resolution_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
 	unsigned int resolution;
 
 	inf = dev_get_drvdata(dev);
@@ -809,137 +1069,276 @@ static ssize_t bmp180_resolution_show(struct device *dev,
 	return sprintf(buf, "%u\n", resolution);
 }
 
-static ssize_t bmp180_max_range_store(struct device *dev,
-				      struct device_attribute *attr,
-				      const char *buf, size_t count)
+static ssize_t bmp_max_range_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count)
 {
-	struct bmp180_inf *inf;
-	u8 range_index;
+	struct bmp_inf *inf;
+	unsigned int range_user;
 	int err;
 
 	inf = dev_get_drvdata(dev);
-	err = kstrtou8(buf, 10, &range_index);
+	err = kstrtouint(buf, 10, &range_user);
 	if (err)
 		return -EINVAL;
 
-	if (range_index > 3)
+	if (range_user > inf->hal->range_limit)
 		return -EINVAL;
 
-	dev_dbg(&inf->i2c->dev, "%s %u", __func__, range_index);
-	inf->range_index = range_index;
-	nvi_mpu_delay_ms(inf->port_id[WR], bmp180_delay_ms_tbl[range_index]);
+	if (range_user != inf->range_user) {
+		dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, range_user);
+		err = bmp_delay(inf, inf->poll_delay_us, range_user);
+		if (!err) {
+			inf->range_user = range_user;
+		} else {
+			dev_err(&inf->i2c->dev, "%s ERR %u\n",
+				__func__, range_user);
+			return err;
+		}
+	}
+
 	return count;
 }
 
-static ssize_t bmp180_max_range_show(struct device *dev,
-				     struct device_attribute *attr, char *buf)
+static ssize_t bmp_max_range_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
 	unsigned int range;
 
 	inf = dev_get_drvdata(dev);
 	if (inf->enable)
-		range = inf->range_index;
+		range = inf->range_i;
 	else
-		range = (BMP180_PRESSURE_MAX * BMP180_INPUT_DIVISOR);
+		range = (BMP180_PRESSURE_MAX * inf->hal->divisor);
 	return sprintf(buf, "%u\n", range);
 }
 
-static ssize_t bmp180_divisor_show(struct device *dev,
-				   struct device_attribute *attr, char *buf)
+static ssize_t bmp_data_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
+	unsigned int data_info;
+	int err;
 
 	inf = dev_get_drvdata(dev);
-	return sprintf(buf, "%d\n", BMP180_INPUT_DIVISOR);
+	err = kstrtouint(buf, 10, &data_info);
+	if (err)
+		return -EINVAL;
+
+	if (data_info >= BMP_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 bmp180_microamp_show(struct device *dev,
-				    struct device_attribute *attr, char *buf)
+static ssize_t bmp_data_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
+	u8 data[11];
+	long val = 0;
+	enum BMP_DATA_INFO data_info;
+	bool enable;
+	char const *info_str;
+	int err = 0;
 
 	inf = dev_get_drvdata(dev);
-	return sprintf(buf, "%d\n", BMP180_INPUT_POWER_UA);
+	data_info = inf->data_info;
+	inf->data_info = BMP_DATA_INFO_PRESSURE;
+	enable = inf->enable;
+	info_str = "ERR: DISABLED";
+	switch (data_info) {
+	case BMP_DATA_INFO_PRESSURE:
+		val = (long)inf->pressure;
+		if (inf->enable)
+			info_str = "pressure";
+		break;
+
+	case BMP_DATA_INFO_TEMPERATURE:
+		val = inf->temperature;
+		if (inf->enable)
+			info_str = "temperature";
+		break;
+
+	case BMP_DATA_INFO_PRESSURE_RAW:
+		val = inf->UP;
+		if (inf->enable)
+			info_str = "pressure_raw";
+		break;
+
+	case BMP_DATA_INFO_TEMPERATURE_RAW:
+		val = inf->UT;
+		if (inf->enable)
+			info_str = "temperature_raw";
+		break;
+
+	case BMP_DATA_INFO_TEMPERATURE_FINE:
+		val = (long)inf->t_fine;
+		if (inf->enable)
+			info_str = "temperature_fine";
+		break;
+
+	case BMP_DATA_INFO_CALIBRATION:
+		err = 0;
+		if (!inf->initd)
+			err = bmp_enable(inf, enable);
+		if (err)
+			return sprintf(buf, "calibration ERR\n");
+
+		return sprintf(buf, "%x %x %x %x %x %x %x %x %x %x %x %x\n",
+			       inf->rom.bmp280.dig_T1,
+			       inf->rom.bmp280.dig_T2,
+			       inf->rom.bmp280.dig_T3,
+			       inf->rom.bmp280.dig_P1,
+			       inf->rom.bmp280.dig_P2,
+			       inf->rom.bmp280.dig_P3,
+			       inf->rom.bmp280.dig_P4,
+			       inf->rom.bmp280.dig_P5,
+			       inf->rom.bmp280.dig_P6,
+			       inf->rom.bmp280.dig_P7,
+			       inf->rom.bmp280.dig_P8,
+			       inf->rom.bmp280.dig_P9);
+
+	case BMP_DATA_INFO_RESET:
+		bmp_pm(inf, true);
+		err = bmp_mode_wr(inf, true, enable);
+		bmp_pm(inf, enable);
+		if (err)
+			return sprintf(buf, "reset ERR\n");
+		else
+			return sprintf(buf, "reset done\n");
+
+	case BMP_DATA_INFO_REGISTERS:
+		err = bmp_nvi_mpu_bypass_request(inf);
+		if (!err) {
+			err = bmp_i2c_rd(inf, BMPX80_REG_ID, 1, data);
+			err |= bmp_i2c_rd(inf, BMP280_REG_STATUS, 10, &data[1]);
+			bmp_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 %x\n",
+			       data[0], data[1], data[2], data[3], data[4],
+			       data[5], data[6], data[7], data[8], data[9],
+			       data[10]);
+
+	default:
+		return -EINVAL;
+	}
+
+	return sprintf(buf, "%s: %ld\n", info_str, val);
 }
 
-static ssize_t bmp180_pressure_show(struct device *dev,
-				    struct device_attribute *attr, char *buf)
+static ssize_t bmp_mpu_fifo_enable_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
+	unsigned int fifo_enable;
+	int err;
 
 	inf = dev_get_drvdata(dev);
-	if (inf->enable)
-		return sprintf(buf, "%d\n", inf->pressure);
+	err = kstrtouint(buf, 10, &fifo_enable);
+	if (err)
+		return -EINVAL;
 
-	return -EPERM;
+	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 bmp180_temperature_show(struct device *dev,
-				       struct device_attribute *attr,
-				       char *buf)
+static ssize_t bmp_mpu_fifo_enable_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%x\n", inf->fifo_enable & inf->use_mpu);
+}
+
+static ssize_t bmp_divisor_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct bmp_inf *inf;
 
 	inf = dev_get_drvdata(dev);
-	if (inf->enable)
-		return sprintf(buf, "%ld\n", inf->temperature);
+	return sprintf(buf, "%u\n", inf->hal->divisor);
+}
+
+static ssize_t bmp_microamp_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	struct bmp_inf *inf;
 
-	return -EPERM;
+	inf = dev_get_drvdata(dev);
+	return sprintf(buf, "%u\n", BMP180_INPUT_POWER_UA);
 }
 
-static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWOTH,
-		   bmp180_enable_show, bmp180_enable_store);
-static DEVICE_ATTR(delay, S_IRUGO | S_IWUSR | S_IWOTH,
-		   bmp180_delay_show, bmp180_delay_store);
-static DEVICE_ATTR(resolution, S_IRUGO | S_IWUSR | S_IWOTH,
-		   bmp180_resolution_show, bmp180_resolution_store);
-static DEVICE_ATTR(max_range, S_IRUGO | S_IWUSR | S_IWOTH,
-		   bmp180_max_range_show, bmp180_max_range_store);
+
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP,
+		   bmp_enable_show, bmp_enable_store);
+static DEVICE_ATTR(delay, S_IRUGO | S_IWUSR | S_IWGRP,
+		   bmp_delay_show, bmp_delay_store);
+static DEVICE_ATTR(resolution, S_IRUGO | S_IWUSR | S_IWGRP,
+		   bmp_resolution_show, bmp_resolution_store);
+static DEVICE_ATTR(max_range, S_IRUGO | S_IWUSR | S_IWGRP,
+		   bmp_max_range_show, bmp_max_range_store);
+static DEVICE_ATTR(mpu_fifo_en, S_IRUGO | S_IWUSR | S_IWGRP,
+		   bmp_mpu_fifo_enable_show, bmp_mpu_fifo_enable_store);
+static DEVICE_ATTR(data, S_IRUGO | S_IWUSR | S_IWGRP,
+		   bmp_data_show, bmp_data_store);
 static DEVICE_ATTR(divisor, S_IRUGO,
-		   bmp180_divisor_show, NULL);
+		   bmp_divisor_show, NULL);
 static DEVICE_ATTR(microamp, S_IRUGO,
-		   bmp180_microamp_show, NULL);
-static DEVICE_ATTR(pressure, S_IRUGO,
-		   bmp180_pressure_show, NULL);
-static DEVICE_ATTR(temperature, S_IRUGO,
-		   bmp180_temperature_show, NULL);
+		   bmp_microamp_show, NULL);
 
-static struct attribute *bmp180_attrs[] = {
+static struct attribute *bmp_attrs[] = {
 	&dev_attr_enable.attr,
 	&dev_attr_delay.attr,
 	&dev_attr_resolution.attr,
 	&dev_attr_max_range.attr,
 	&dev_attr_divisor.attr,
 	&dev_attr_microamp.attr,
-	&dev_attr_pressure.attr,
-	&dev_attr_temperature.attr,
+	&dev_attr_data.attr,
+	&dev_attr_mpu_fifo_en.attr,
 	NULL
 };
 
-static struct attribute_group bmp180_attr_group = {
-	.name = BMP180_NAME,
-	.attrs = bmp180_attrs
+static struct attribute_group bmp_attr_group = {
+	.name = BMPX80_NAME,
+	.attrs = bmp_attrs
 };
 
-static int bmp180_sysfs_create(struct bmp180_inf *inf)
+static int bmp_sysfs_create(struct bmp_inf *inf)
 {
 	int err;
 
-	err = sysfs_create_group(&inf->idev->dev.kobj, &bmp180_attr_group);
+	err = sysfs_create_group(&inf->idev->dev.kobj, &bmp_attr_group);
 	return err;
 }
 
-static void bmp180_input_close(struct input_dev *idev)
+static void bmp_input_close(struct input_dev *idev)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
 
 	inf = input_get_drvdata(idev);
 	if (inf != NULL)
-		bmp180_enable(inf, false);
+		bmp_enable(inf, false);
 }
 
-static int bmp180_input_create(struct bmp180_inf *inf)
+static int bmp_input_create(struct bmp_inf *inf)
 {
 	int err;
 
@@ -950,9 +1349,9 @@ static int bmp180_input_create(struct bmp180_inf *inf)
 		return err;
 	}
 
-	inf->idev->name = BMP180_NAME;
+	inf->idev->name = BMPX80_NAME;
 	inf->idev->dev.parent = &inf->i2c->dev;
-	inf->idev->close = bmp180_input_close;
+	inf->idev->close = bmp_input_close;
 	input_set_drvdata(inf->idev, inf);
 	input_set_capability(inf->idev, EV_ABS, ABS_PRESSURE);
 	input_set_abs_params(inf->idev, ABS_PRESSURE,
@@ -966,9 +1365,177 @@ static int bmp180_input_create(struct bmp180_inf *inf)
 	return err;
 }
 
-static int bmp180_remove(struct i2c_client *client)
+static int bmp_id_compare(struct bmp_inf *inf, u8 val,
+			  const struct i2c_device_id *id)
+{
+	unsigned int i;
+	int err = 0;
+
+	for (i = 0; i < ARRAY_SIZE(bmp_ids); i++) {
+		if (val == bmp_ids[i]) {
+			if (inf->dev_id && (inf->dev_id != val))
+				dev_err(&inf->i2c->dev, "%s ERR: %x != %s\n",
+					__func__, inf->dev_id, id->name);
+			inf->dev_id = val;
+			break;
+		}
+	}
+	if (!inf->dev_id) {
+		err = -ENODEV;
+		dev_err(&inf->i2c->dev, "%s ERR: ID %x != %s\n",
+			__func__, val, id->name);
+	} else {
+		dev_dbg(&inf->i2c->dev, "%s using ID %x for %s\n",
+			__func__, inf->dev_id, id->name);
+	}
+	return err;
+}
+
+static struct bmp_hal bmp180_hal = {
+	.rom_addr_start		= BMP180_REG_AC1,
+	.rom_size		= 22,
+	.bmp_delay_ms_tbl	= bmp180_delay_ms_tbl,
+	.divisor		= BMP180_INPUT_DIVISOR,
+	.range_limit		= ARRAY_SIZE(bmp180_delay_ms_tbl),
+	.range_dflt		= BMP180_RANGE_DFLT,
+	.bmp_read		= &bmp180_read,
+	.bmp_mode_wr_mpu	= &bmp180_mode_wr_mpu
+};
+
+static struct bmp_hal bmp280_hal = {
+	.rom_addr_start		= BMP280_REG_CWORD00,
+	.rom_size		= 26,
+	.bmp_delay_ms_tbl	= bmp280_delay_ms_tbl,
+	.divisor		= BMP280_INPUT_DIVISOR,
+	.range_limit		= ARRAY_SIZE(bmp280_delay_ms_tbl),
+	.range_dflt		= BMP280_RANGE_DFLT,
+	.bmp_read		= &bmp280_read,
+	.bmp_mode_wr_mpu	= &bmp280_mode_wr_mpu
+};
+
+static int bmp_hal(struct bmp_inf *inf)
+{
+	int err = 0;
+
+	switch (inf->dev_id) {
+	case BMPX80_REG_ID_BMP280:
+		inf->hal = &bmp280_hal;
+		break;
+
+	case BMPX80_REG_ID_BMP180:
+		inf->hal = &bmp180_hal;
+		break;
+
+	default:
+		dev_err(&inf->i2c->dev, "%s ERR: Unknown device\n", __func__);
+		inf->hal = &bmp180_hal; /* to prevent NULL pointers */
+		err = -ENODEV;
+		break;
+	}
+
+	inf->range_user = inf->hal->range_dflt;
+	return err;
+}
+
+static int bmp_id(struct bmp_inf *inf, const struct i2c_device_id *id)
+{
+	struct nvi_mpu_port nmp;
+	u8 config_boot;
+	u8 val = 0;
+	int err;
+
+	config_boot = inf->pdata.config & NVI_CONFIG_BOOT_MASK;
+	if ((inf->i2c->addr != BMP180_I2C_ADDR0) ||
+					  (inf->i2c->addr != BMP180_I2C_ADDR1))
+		inf->i2c->addr = BMP180_I2C_ADDR1;
+	if (!strcmp(id->name, BMP180_NAME))
+		inf->dev_id = BMPX80_REG_ID_BMP180;
+	else if (!strcmp(id->name, BMP280_NAME))
+		inf->dev_id = BMPX80_REG_ID_BMP280;
+	if ((config_boot == NVI_CONFIG_BOOT_MPU) && (!inf->dev_id)) {
+		dev_err(&inf->i2c->dev, "%s ERR: NVI_CONFIG_BOOT_MPU && %s\n",
+			__func__, id->name);
+		config_boot = NVI_CONFIG_BOOT_AUTO;
+	}
+	if (config_boot == NVI_CONFIG_BOOT_AUTO) {
+		nmp.addr = inf->i2c->addr | 0x80;
+		nmp.reg = BMPX80_REG_ID;
+		nmp.ctrl = 1;
+		err = nvi_mpu_dev_valid(&nmp, &val);
+		dev_dbg(&inf->i2c->dev, "%s AUTO ID=%x err=%d\n",
+			__func__, val, err);
+		/* see mpu.h for possible return values */
+		if ((err == -EAGAIN) || (err == -EBUSY))
+			return -EAGAIN;
+
+		if (!err)
+			err = bmp_id_compare(inf, val, id);
+		if ((!err) || ((err == -EIO) && (inf->dev_id)))
+			config_boot = NVI_CONFIG_BOOT_MPU;
+	}
+	if (config_boot == NVI_CONFIG_BOOT_MPU) {
+		bmp_hal(inf);
+		inf->use_mpu = true;
+		nmp.addr = inf->i2c->addr | 0x80;
+		nmp.data_out = 0;
+		nmp.delay_ms = 0;
+		nmp.delay_us = inf->poll_delay_us;
+		nmp.shutdown_bypass = true;
+		nmp.ext_driver = (void *)inf;
+		if (inf->dev_id == BMPX80_REG_ID_BMP280) {
+			nmp.reg = BMP280_REG_STATUS;
+			nmp.ctrl = 10; /* MPU FIFO can't handle odd size */
+			nmp.handler = &bmp280_mpu_handler;
+		} else {
+			nmp.reg = BMPX80_REG_CTRL;
+			nmp.ctrl = 6; /* MPU FIFO can't handle odd size */
+			nmp.handler = &bmp180_mpu_handler;
+		}
+		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 = BMPX80_REG_CTRL;
+		nmp.ctrl = 1;
+		nmp.data_out = inf->data_out;
+		nmp.delay_ms = inf->hal->bmp_delay_ms_tbl[inf->range_i];
+		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) {
+			bmp_ports_free(inf);
+			dev_err(&inf->i2c->dev, "%s ERR %d\n", __func__, err);
+		} else {
+			inf->port_id[WR] = err;
+			err = 0;
+		}
+		return err;
+	}
+
+	/* NVI_CONFIG_BOOT_HOST */
+	inf->use_mpu = false;
+	err = bmp_i2c_rd(inf, BMPX80_REG_ID, 1, &val);
+	dev_dbg(&inf->i2c->dev, "%s Host read ID=%x err=%d\n",
+		__func__, val, err);
+	if (!err) {
+		err = bmp_id_compare(inf, val, id);
+		if (!err)
+			err = bmp_hal(inf);
+	}
+	return err;
+}
+
+static int bmp_remove(struct i2c_client *client)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
 
 	inf = i2c_get_clientdata(client);
 	if (inf != NULL) {
@@ -978,103 +1545,174 @@ static int bmp180_remove(struct i2c_client *client)
 		}
 		if (inf->wq)
 			destroy_workqueue(inf->wq);
-		bmp180_pm_exit(inf);
-		kfree(inf);
+		bmp_pm_exit(inf);
 	}
 	dev_info(&client->dev, "%s\n", __func__);
 	return 0;
 }
 
-static void bmp180_shutdown(struct i2c_client *client)
+static void bmp_shutdown(struct i2c_client *client)
 {
-	bmp180_remove(client);
+	bmp_remove(client);
+}
+
+static struct mpu_platform_data *bmp_parse_dt(struct i2c_client *client)
+{
+	struct mpu_platform_data *pdata;
+	struct device_node *np = client->dev.of_node;
+	char const *pchar;
+	u8 config;
+
+	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(&client->dev, "Can't allocate platform data\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	if (of_property_read_string(np, "config", &pchar)) {
+		dev_err(&client->dev, "Cannot read config property\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	for (config = 0; config < ARRAY_SIZE(bmp_configs); config++) {
+		if (!strcasecmp(pchar, bmp_configs[config])) {
+			pdata->config = config;
+			break;
+		}
+	}
+
+	if (config == ARRAY_SIZE(bmp_configs)) {
+		dev_err(&client->dev, "Invalid config value\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	return pdata;
 }
 
-static int bmp180_probe(struct i2c_client *client,
-			const struct i2c_device_id *id)
+static int bmp_probe(struct i2c_client *client,
+		     const struct i2c_device_id *id)
 {
-	struct bmp180_inf *inf;
+	struct bmp_inf *inf;
 	struct mpu_platform_data *pd;
 	int err;
 
 	dev_info(&client->dev, "%s\n", __func__);
-	inf = kzalloc(sizeof(*inf), GFP_KERNEL);
-	if (IS_ERR_OR_NULL(inf)) {
+	inf = devm_kzalloc(&client->dev, sizeof(*inf), GFP_KERNEL);
+	if (!inf) {
 		dev_err(&client->dev, "%s kzalloc ERR\n", __func__);
 		return -ENOMEM;
 	}
 
 	inf->i2c = client;
 	i2c_set_clientdata(client, inf);
-	pd = (struct mpu_platform_data *)dev_get_platdata(&client->dev);
-	if (pd != NULL)
-		inf->pdata = *pd;
-	bmp180_pm_init(inf);
-	err = bmp180_id(inf);
-	bmp180_pm(inf, false);
+	if (client->dev.of_node) {
+		pd = bmp_parse_dt(client);
+		if (IS_ERR(pd))
+			return -EINVAL;
+		else
+			inf->pdata = *pd;
+	} else {
+		pd = (struct mpu_platform_data *)dev_get_platdata(&client->dev);
+		if (pd != NULL)
+			inf->pdata = *pd;
+	}
+
+	bmp_pm_init(inf);
+	err = bmp_id(inf, id);
+	bmp_pm(inf, false);
 	if (err == -EAGAIN)
-		goto bmp180_probe_again;
+		goto bmp_probe_again;
 	else if (err)
-		goto bmp180_probe_err;
+		goto bmp_probe_err;
 
-	err = bmp180_input_create(inf);
+	err = bmp_input_create(inf);
 	if (err)
-		goto bmp180_probe_err;
+		goto bmp_probe_err;
 
-	inf->wq = create_singlethread_workqueue(BMP180_NAME);
+	inf->wq = create_singlethread_workqueue(BMPX80_NAME);
 	if (!inf->wq) {
 		dev_err(&client->dev, "%s workqueue ERR\n", __func__);
 		err = -ENOMEM;
-		goto bmp180_probe_err;
+		goto bmp_probe_err;
 	}
 
-	INIT_DELAYED_WORK(&inf->dw, bmp180_work);
-	err = bmp180_sysfs_create(inf);
+	INIT_DELAYED_WORK(&inf->dw, bmp_work);
+	err = bmp_sysfs_create(inf);
 	if (err)
-		goto bmp180_probe_err;
+		goto bmp_probe_err;
 
 	return 0;
 
-bmp180_probe_err:
+bmp_probe_err:
 	dev_err(&client->dev, "%s ERR %d\n", __func__, err);
-bmp180_probe_again:
-	bmp180_remove(client);
+bmp_probe_again:
+	bmp_remove(client);
 	return err;
 }
 
-static const struct i2c_device_id bmp180_i2c_device_id[] = {
+static int bmp_suspend(struct device *dev)
+{
+	struct bmp_inf *inf;
+	int err;
+
+	inf = dev_get_drvdata(dev);
+	err = bmp_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 bmp_pm_ops = {
+	.suspend	= bmp_suspend,
+};
+
+static const struct i2c_device_id bmp_i2c_device_id[] = {
+	{BMPX80_NAME, 0},
 	{BMP180_NAME, 0},
+	{BMP280_NAME, 0},
 	{}
 };
 
-MODULE_DEVICE_TABLE(i2c, bmp180_i2c_device_id);
+MODULE_DEVICE_TABLE(i2c, bmp_i2c_device_id);
+
+static const struct of_device_id bmp_of_match[] = {
+	{ .compatible = "bmp,bmpX80", },
+	{ .compatible = "bmp,bmp180", },
+	{ .compatible = "bmp,bmp280", },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, bmp_of_match);
 
-static struct i2c_driver bmp180_driver = {
+static struct i2c_driver bmp_driver = {
 	.class		= I2C_CLASS_HWMON,
-	.probe		= bmp180_probe,
-	.remove		= bmp180_remove,
+	.probe		= bmp_probe,
+	.remove		= bmp_remove,
 	.driver = {
-		.name	= BMP180_NAME,
+		.name	= BMPX80_NAME,
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(bmp_of_match),
+		.pm		= &bmp_pm_ops,
 	},
-	.id_table	= bmp180_i2c_device_id,
-	.shutdown	= bmp180_shutdown,
+	.id_table	= bmp_i2c_device_id,
+	.shutdown	= bmp_shutdown,
 };
 
-static int __init bmp180_init(void)
+static int __init bmp_init(void)
 {
-	return i2c_add_driver(&bmp180_driver);
+	return i2c_add_driver(&bmp_driver);
 }
 
-static void __exit bmp180_exit(void)
+static void __exit bmp_exit(void)
 {
-	i2c_del_driver(&bmp180_driver);
+	i2c_del_driver(&bmp_driver);
 }
 
-module_init(bmp180_init);
-module_exit(bmp180_exit);
+module_init(bmp_init);
+module_exit(bmp_exit);
 
 MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("BMP180 driver");
-MODULE_AUTHOR("NVIDIA Corp");
+MODULE_DESCRIPTION("BMPX80 driver");
+MODULE_AUTHOR("NVIDIA Corporation");