misc: mpu3050: Invensense MPU code update v3.3.3

Update Invensense Motion Library (MPL) to production version
v3.3.3. Previous version was engineering release and is not
suitable for mass production. This version of the MPL most
notably fixes problems with suspend/resume where touch screen
would quit working during a suspend/resume cycle.

Change-Id: Ia33433a677862b3d1e31d410aac7176d7a381a8f
Reviewed-on: http://git-master/r/22987
Reviewed-by: Varun Colbert <vcolbert@nvidia.com>
Tested-by: Varun Colbert <vcolbert@nvidia.com>

1 files changed, 623 insertions, 428 deletions

diff --git a/drivers/misc/mpu3050/mldl_cfg.c b/drivers/misc/mpu3050/mldl_cfg.c
index 668a4cf03ccc..66f447e7ba85 100644
--- a/drivers/misc/mpu3050/mldl_cfg.c
+++ b/drivers/misc/mpu3050/mldl_cfg.c
@@ -57,99 +57,85 @@
 #define STANDBY 1
 #endif
 
-/* --------------------- */
-/* -    Prototypes.    - */
-/* --------------------- */
-#ifdef M_HW
-static tMLError MLDLPowerMgmtMantis(struct mldl_cfg *pdata,
-				    void *mlsl_handle,
-				    unsigned char reset,
-				    unsigned char powerselection);
-static tMLError MLDLStandByGyros(struct mldl_cfg *pdata,
-				 void *mlsl_handle,
-				 unsigned char disable_gx,
-				 unsigned char disable_gy,
-				 unsigned char disable_gz);
-#else
-static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
-			    void *mlsl_handle,
-			    unsigned char reset,
-			    unsigned char sleep,
-			    unsigned char disable_gx,
-			    unsigned char disable_gy,
-			    unsigned char disable_gz);
-#endif
+/*---------------------*/
+/*-    Prototypes.    -*/
+/*---------------------*/
 
-/* ---------------------- */
-/* -  Static Functions. - */
-/* ---------------------- */
+/*----------------------*/
+/*-  Static Functions. -*/
+/*----------------------*/
 
-/**
- *  @internal
- * @brief   MLDLCfgDMP configures the Digital Motion Processor internal to
- *          the MPU. The DMP can be enabled or disabled and the start address
- *          can be set.
- *
- * @param   enableRun   Enables the DMP processing if set to TRUE.
- * @param   enableFIFO  Enables DMP output to the FIFO if set to TRUE.
- *
- * @return  Zero if the command is successful, an error code otherwise.
- */
-static int MLDLCtrlDmp(struct mldl_cfg *pdata, void *mlsl_handle,
-		       bool enableRun, bool enableFIFO)
+static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
 {
-	unsigned char b;
+	unsigned char userCtrlReg;
+	int result;
 
-	MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_USER_CTRL, 1, &b);
-	if (enableRun)
-		b |= BIT_DMP_EN;
-	else
-		b &= ~BIT_DMP_EN;
+	if (!mldl_cfg->dmp_is_running)
+		return ML_SUCCESS;
 
-	if (enableFIFO)
-		b |= BIT_FIFO_EN;
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_USER_CTRL, 1, &userCtrlReg);
+	ERROR_CHECK(result);
+	userCtrlReg = (userCtrlReg & (~BIT_FIFO_EN)) | BIT_FIFO_RST;
+	userCtrlReg = (userCtrlReg & (~BIT_DMP_EN)) | BIT_DMP_RST;
 
-	b |= BIT_DMP_RST;
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				       MPUREG_USER_CTRL, userCtrlReg);
+	ERROR_CHECK(result);
+	mldl_cfg->dmp_is_running = 0;
 
-	MLSLSerialWriteSingle(mlsl_handle, pdata->addr, MPUREG_USER_CTRL,
-			      b);
+	return result;
 
-	return ML_SUCCESS;
 }
-
 /**
  * @brief Starts the DMP running
  *
  * @return ML_SUCCESS or non-zero error code
  */
-static int MLDLDmpStart(struct mldl_cfg *pdata, void *mlsl_handle)
+static int dmp_start(struct mldl_cfg *pdata, void *mlsl_handle)
 {
-	unsigned char fifoBuf[2];
-	unsigned char tries = 0;
 	unsigned char userCtrlReg;
 	int result;
-	unsigned short len = !0;
+
+	if (pdata->dmp_is_running == pdata->dmp_enable)
+		return ML_SUCCESS;
 
 	result = MLSLSerialRead(mlsl_handle, pdata->addr,
 				MPUREG_USER_CTRL, 1, &userCtrlReg);
+	ERROR_CHECK(result);
 
-	while (len != 0 && tries < 6) {
-		result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
-					       MPUREG_USER_CTRL,
-					       ((userCtrlReg & (~BIT_FIFO_EN))
-							     |   BIT_FIFO_RST));
-		MLSLSerialRead(mlsl_handle, pdata->addr,
-			       MPUREG_FIFO_COUNTH, 2, fifoBuf);
-		len = (((unsigned short) fifoBuf[0] << 8)
-		       | (unsigned short) fifoBuf[1]);
-		tries++;
-	}
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				       MPUREG_USER_CTRL,
+				       ((userCtrlReg & (~BIT_FIFO_EN))
+						|   BIT_FIFO_RST));
+	ERROR_CHECK(result);
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				       MPUREG_USER_CTRL, userCtrlReg);
+	ERROR_CHECK(result);
+
+	result = MLSLSerialRead(mlsl_handle, pdata->addr,
+				MPUREG_USER_CTRL, 1, &userCtrlReg);
+	ERROR_CHECK(result);
+
+	if (pdata->dmp_enable)
+		userCtrlReg |= BIT_DMP_EN;
+	else
+		userCtrlReg &= ~BIT_DMP_EN;
+
+	if (pdata->fifo_enable)
+		userCtrlReg |= BIT_FIFO_EN;
+	else
+		userCtrlReg &= ~BIT_FIFO_EN;
 
-	MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
-			      MPUREG_USER_CTRL, userCtrlReg);
+	userCtrlReg |= BIT_DMP_RST;
 
-	return MLDLCtrlDmp(pdata, mlsl_handle,
-			   pdata->dmp_enable, pdata->fifo_enable);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				       MPUREG_USER_CTRL, userCtrlReg);
+	ERROR_CHECK(result);
+	pdata->dmp_is_running = pdata->dmp_enable;
+
+	return result;
 }
 
 /**
@@ -166,22 +152,15 @@ static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
 	unsigned char b;
 	int result;
 
-#ifdef ML_USE_DMP_SIM
-	/*  done this way so that pc demo  */
-	/*  w/arm board works with universal api  */
-	if (!MLGetGyroPresent())
+	if ((mldl_cfg->gyro_is_bypassed && enable) ||
+	    (!mldl_cfg->gyro_is_bypassed && !enable))
 		return ML_SUCCESS;
-#endif
 
 	/*---- get current 'USER_CTRL' into b ----*/
 	result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
 				MPUREG_USER_CTRL, 1, &b);
 	ERROR_CHECK(result);
 
-	/* No change */
-	if ((b & BIT_AUX_IF_EN) != (enable * BIT_AUX_IF_EN))
-		return ML_SUCCESS;
-
 	b &= ~BIT_AUX_IF_EN;
 
 	if (!enable) {
@@ -215,7 +194,7 @@ static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
 		 * 3) wait for up to one MPU cycle then restore the slave
 		 *    address
 		 */
-		MLOSSleep(5);
+		MLOSSleep(SAMPLING_PERIOD_US(mldl_cfg) / 1000);
 		result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
 					       MPUREG_AUX_SLV_ADDR,
 					       mldl_cfg->pdata->
@@ -238,6 +217,7 @@ static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
 		ERROR_CHECK(result);
 		MLOSSleep(2);
 	}
+	mldl_cfg->gyro_is_bypassed = enable;
 
 	return result;
 }
@@ -254,11 +234,14 @@ struct tsProdRevMap {
 #define OLDEST_PROD_REV_SUPPORTED 1
 static struct tsProdRevMap prodRevsMap[] = {
 	{0, 0},
-	{MPU_SILICON_REV_A1, 131},	/* 1 A1 */
-	{MPU_SILICON_REV_A1, 131},	/* 2 A1 */
-	{MPU_SILICON_REV_A1, 131},	/* 3 A1 */
-	{MPU_SILICON_REV_A1, 131},	/* 4 A1 */
-	{MPU_SILICON_REV_A1, 131},	/* 5 A1 */
+	{MPU_SILICON_REV_A1, 131},	/* 1 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 2 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 3 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 4 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 5 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 6 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 7 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 8 A1 (npp) */
 };
 
 #else				/* !M_HW */
@@ -411,11 +394,11 @@ static int MLDLSetLevelShifterBit(struct mldl_cfg *pdata,
 /**
  *  @internal
  * @param reset 1 to reset hardware
-**/
-static tMLError MLDLPowerMgmtMantis(struct mldl_cfg *pdata,
-				    void *mlsl_handle,
-				    unsigned char reset,
-				    unsigned char powerselection)
+ */
+static tMLError mpu60xx_pwr_mgmt(struct mldl_cfg *pdata,
+				 void *mlsl_handle,
+				 unsigned char reset,
+				 unsigned char powerselection)
 {
 	unsigned char b;
 	tMLError result;
@@ -434,7 +417,7 @@ static tMLError MLDLPowerMgmtMantis(struct mldl_cfg *pdata,
 		/* Current sillicon has an errata where the reset will get
 		 * nacked.  Ignore the error code for now. */
 		result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
-					       MPUREG_PWR_MGM, b | 0x80);
+					       MPUREG_PWR_MGM, b | BIT_H_RESET);
 #define M_HW_RESET_ERRATTA
 #ifndef M_HW_RESET_ERRATTA
 		ERROR_CHECK(result);
@@ -446,9 +429,9 @@ static tMLError MLDLPowerMgmtMantis(struct mldl_cfg *pdata,
 	b |= (powerselection << 4);
 
 	if (b & BITS_PWRSEL)
-		pdata->is_suspended = FALSE;
+		pdata->gyro_is_suspended = FALSE;
 	else
-		pdata->is_suspended = TRUE;
+		pdata->gyro_is_suspended = TRUE;
 
 	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
 				       MPUREG_PWR_MGM, b);
@@ -570,29 +553,34 @@ static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
 	if ((!(b & BIT_SLEEP)) && reset)
 		result = MLDLSetI2CBypass(pdata, mlsl_handle, 1);
 
+	/* If we are awake, we need stop the dmp sleeping */
+	if ((!(b & BIT_SLEEP)) && sleep)
+		dmp_stop(pdata, mlsl_handle);
+
 	/* Reset if requested */
 	if (reset) {
-		result =
-		    MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
-					  MPUREG_PWR_MGM, b | BIT_H_RESET);
+		MPL_LOGV("Reset MPU3050\n");
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+					MPUREG_PWR_MGM, b | BIT_H_RESET);
+		ERROR_CHECK(result);
 		MLOSSleep(5);
+		pdata->gyro_needs_reset = FALSE;
+		/* Some chips are awake after reset and some are asleep,
+		 * check the status */
+		result = MLSLSerialRead(mlsl_handle, pdata->addr,
+					MPUREG_PWR_MGM, 1, &b);
+		ERROR_CHECK(result);
 	}
 
-	/* Some chips are awake after reset and some are asleep, check the
-	 * status */
-	result =
-	    MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGM, 1,
-			   &b);
-	ERROR_CHECK(result);
-
 	/* Update the suspended state just in case we return early */
 	if (b & BIT_SLEEP)
-		pdata->is_suspended = TRUE;
+		pdata->gyro_is_suspended = TRUE;
 	else
-		pdata->is_suspended = FALSE;
+		pdata->gyro_is_suspended = FALSE;
 
-	if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG))
-	    == (((sleep != 0) * BIT_SLEEP) |
+	/* if power status match requested, nothing else's left to do */
+	if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
+		(((sleep != 0) * BIT_SLEEP) |
 		((disable_gx != 0) * BIT_STBY_XG) |
 		((disable_gy != 0) * BIT_STBY_YG) |
 		((disable_gz != 0) * BIT_STBY_ZG))) {
@@ -608,7 +596,7 @@ static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
 	 */
 	if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
 		 (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
-	    && ((!sleep) && disable_gx && disable_gy && disable_gz)) {
+		&& ((!sleep) && disable_gx && disable_gy && disable_gz)) {
 		result = MLDLPowerMgmtMPU(pdata, mlsl_handle, 0, 1, 0, 0, 0);
 		if (result)
 			return result;
@@ -616,7 +604,6 @@ static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
 		b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
 	}
 
-
 	if ((b & BIT_SLEEP) != ((sleep != 0) * BIT_SLEEP)) {
 		if (sleep) {
 			result = MLDLSetI2CBypass(pdata, mlsl_handle, 1);
@@ -626,26 +613,25 @@ static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
 			    MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
 						  MPUREG_PWR_MGM, b);
 			ERROR_CHECK(result);
-			pdata->is_suspended = TRUE;
+			pdata->gyro_is_suspended = TRUE;
 		} else {
 			b &= ~BIT_SLEEP;
-
 			result =
 			    MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
 						  MPUREG_PWR_MGM, b);
 			ERROR_CHECK(result);
-			pdata->is_suspended = FALSE;
+			pdata->gyro_is_suspended = FALSE;
 			MLOSSleep(5);
 		}
 	}
-    /*---
-      WORKAROUND FOR PUTTING GYRO AXIS in STAND-BY MODE
-      1) put one axis at a time in stand-by
-      ---*/
+	/*---
+	  WORKAROUND FOR PUTTING GYRO AXIS in STAND-BY MODE
+	  1) put one axis at a time in stand-by
+	  ---*/
 	if ((b & BIT_STBY_XG) != ((disable_gx != 0) * BIT_STBY_XG)) {
 		b ^= BIT_STBY_XG;
 		result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
-					       MPUREG_PWR_MGM, b);
+						MPUREG_PWR_MGM, b);
 		ERROR_CHECK(result);
 	}
 	if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) {
@@ -701,6 +687,8 @@ void mpu_print_cfg(struct mldl_cfg *mldl_cfg)
 	MPL_LOGD("mldl_cfg.product_id       = %02x\n",
 		 mldl_cfg->product_id);
 	MPL_LOGD("mldl_cfg.trim             = %02x\n", mldl_cfg->trim);
+	MPL_LOGD("mldl_cfg.requested_sensors= %04lx\n",
+		 mldl_cfg->requested_sensors);
 
 	if (mldl_cfg->accel) {
 		MPL_LOGD("slave_accel->suspend      = %02x\n",
@@ -839,6 +827,275 @@ void mpu_print_cfg(struct mldl_cfg *mldl_cfg)
 		 offsetof(struct mldl_cfg, ram));
 }
 
+int mpu_set_slave(struct mldl_cfg *mldl_cfg,
+		void *gyro_handle,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *slave_pdata)
+{
+	int result;
+	unsigned char reg;
+	unsigned char slave_reg;
+	unsigned char slave_len;
+	unsigned char slave_endian;
+	unsigned char slave_address;
+
+	result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
+
+	if (NULL == slave || NULL == slave_pdata) {
+		slave_reg = 0;
+		slave_len = 0;
+		slave_endian = 0;
+		slave_address = 0;
+	} else {
+		slave_reg = slave->reg;
+		slave_len = slave->len;
+		slave_endian = slave->endian;
+		slave_address = slave_pdata->address;
+	}
+
+	/* Address */
+	result = MLSLSerialWriteSingle(gyro_handle,
+				mldl_cfg->addr,
+				MPUREG_AUX_SLV_ADDR,
+				slave_address);
+	ERROR_CHECK(result);
+	/* Register */
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_ACCEL_BURST_ADDR, 1,
+				&reg);
+	ERROR_CHECK(result);
+	reg = ((reg & 0x80) | slave_reg);
+	result = MLSLSerialWriteSingle(gyro_handle,
+				mldl_cfg->addr,
+				MPUREG_ACCEL_BURST_ADDR,
+				reg);
+	ERROR_CHECK(result);
+
+#ifdef M_HW
+	/* Length, byte swapping, grouping & enable */
+	if (slave_len > BITS_SLV_LENG) {
+		MPL_LOGW("Limiting slave burst read length to "
+			"the allowed maximum (15B, req. %d)\n",
+			slave_len);
+		slave_len = BITS_SLV_LENG;
+	}
+	reg = slave_len;
+	if (slave_endian == EXT_SLAVE_LITTLE_ENDIAN)
+		reg |= BIT_SLV_BYTE_SW;
+	reg |= BIT_SLV_GRP;
+	reg |= BIT_SLV_ENABLE;
+
+	result = MLSLSerialWriteSingle(gyro_handle,
+				mldl_cfg->addr,
+				MPUREG_I2C_SLV0_CTRL,
+				reg);
+#else
+	/* Length */
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_USER_CTRL, 1, &reg);
+	ERROR_CHECK(result);
+	reg = (reg & ~BIT_AUX_RD_LENG);
+	result = MLSLSerialWriteSingle(gyro_handle,
+				mldl_cfg->addr,
+				MPUREG_USER_CTRL, reg);
+	ERROR_CHECK(result);
+#endif
+
+	if (slave_address) {
+		result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, FALSE);
+		ERROR_CHECK(result);
+	}
+	return result;
+}
+
+/**
+ * Check to see if the gyro was reset by testing a couple of registers known
+ * to change on reset.
+ *
+ * @param mldl_cfg mldl configuration structure
+ * @param gyro_handle handle used to communicate with the gyro
+ *
+ * @return ML_SUCCESS or non-zero error code
+ */
+static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	int result = ML_SUCCESS;
+	unsigned char reg;
+
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_DMP_CFG_2, 1, &reg);
+	ERROR_CHECK(result);
+
+	if (mldl_cfg->dmp_cfg2 != reg)
+		return TRUE;
+
+	if (0 != mldl_cfg->dmp_cfg1)
+		return FALSE;
+
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_SMPLRT_DIV, 1, &reg);
+	ERROR_CHECK(result);
+	if (reg != mldl_cfg->divider)
+		return TRUE;
+
+	if (0 != mldl_cfg->divider)
+		return FALSE;
+
+	/* Inconclusive assume it was reset */
+	return TRUE;
+}
+
+static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	int result;
+	int ii;
+	int jj;
+	unsigned char reg;
+	unsigned char regs[7];
+
+	/* Wake up the part */
+#ifdef M_HW
+	result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, RESET,
+				WAKE_UP);
+	ERROR_CHECK(result);
+
+	/* Configure the MPU */
+	result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, 1);
+	ERROR_CHECK(result);
+	/* setting int_config with the propert flag BIT_BYPASS_EN
+	   should be done by the setup functions */
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_INT_PIN_CFG,
+				(mldl_cfg->pdata->int_config |
+					BIT_BYPASS_EN));
+	ERROR_CHECK(result);
+	/* temporary: masking out higher bits to avoid switching
+	   intelligence */
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_INT_ENABLE,
+				(mldl_cfg->int_config));
+	ERROR_CHECK(result);
+#else
+	result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle, 0, 0,
+				mldl_cfg->gyro_power & BIT_STBY_XG,
+				mldl_cfg->gyro_power & BIT_STBY_YG,
+				mldl_cfg->gyro_power & BIT_STBY_ZG);
+
+	if (!mldl_cfg->gyro_needs_reset &&
+	    !mpu_was_reset(mldl_cfg, gyro_handle)) {
+		return ML_SUCCESS;
+	}
+
+	result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle, 1, 0,
+				mldl_cfg->gyro_power & BIT_STBY_XG,
+				mldl_cfg->gyro_power & BIT_STBY_YG,
+				mldl_cfg->gyro_power & BIT_STBY_ZG);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_INT_CFG,
+				(mldl_cfg->int_config |
+					mldl_cfg->pdata->int_config));
+	ERROR_CHECK(result);
+#endif
+
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_PWR_MGM, 1, &reg);
+	ERROR_CHECK(result);
+	reg &= ~BITS_CLKSEL;
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_PWR_MGM,
+				mldl_cfg->clk_src | reg);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_SMPLRT_DIV,
+				mldl_cfg->divider);
+	ERROR_CHECK(result);
+
+#ifdef M_HW
+	reg = DLPF_FS_SYNC_VALUE(0, mldl_cfg->full_scale, 0);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_GYRO_CONFIG, reg);
+	reg = DLPF_FS_SYNC_VALUE(mldl_cfg->ext_sync, 0, mldl_cfg->lpf);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_CONFIG, reg);
+#else
+	reg = DLPF_FS_SYNC_VALUE(mldl_cfg->ext_sync,
+				mldl_cfg->full_scale, mldl_cfg->lpf);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_DLPF_FS_SYNC, reg);
+#endif
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_DMP_CFG_1,
+				mldl_cfg->dmp_cfg1);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_DMP_CFG_2,
+				mldl_cfg->dmp_cfg2);
+	ERROR_CHECK(result);
+
+	/* Write and verify memory */
+	for (ii = 0; ii < MPU_MEM_NUM_RAM_BANKS; ii++) {
+		unsigned char read[MPU_MEM_BANK_SIZE];
+
+		result = MLSLSerialWriteMem(gyro_handle,
+					mldl_cfg->addr,
+					((ii << 8) | 0x00),
+					MPU_MEM_BANK_SIZE,
+					mldl_cfg->ram[ii]);
+		ERROR_CHECK(result);
+		result = MLSLSerialReadMem(gyro_handle, mldl_cfg->addr,
+					((ii << 8) | 0x00),
+					MPU_MEM_BANK_SIZE, read);
+		ERROR_CHECK(result);
+
+#ifdef M_HW
+#define ML_SKIP_CHECK 38
+#else
+#define ML_SKIP_CHECK 20
+#endif
+		for (jj = 0; jj < MPU_MEM_BANK_SIZE; jj++) {
+			/* skip the register memory locations */
+			if (ii == 0 && jj < ML_SKIP_CHECK)
+				continue;
+			if (mldl_cfg->ram[ii][jj] != read[jj]) {
+				result = ML_ERROR_SERIAL_WRITE;
+				break;
+			}
+		}
+		ERROR_CHECK(result);
+	}
+
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_XG_OFFS_TC,
+				mldl_cfg->offset_tc[0]);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_YG_OFFS_TC,
+				mldl_cfg->offset_tc[1]);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_ZG_OFFS_TC,
+				mldl_cfg->offset_tc[2]);
+	ERROR_CHECK(result);
+
+	regs[0] = MPUREG_X_OFFS_USRH;
+	for (ii = 0; ii < DIM(mldl_cfg->offset); ii++) {
+		regs[1 + ii * 2] =
+			(unsigned char)(mldl_cfg->offset[ii] >> 8)
+			& 0xff;
+		regs[1 + ii * 2 + 1] =
+			(unsigned char)(mldl_cfg->offset[ii] & 0xff);
+	}
+	result = MLSLSerialWrite(gyro_handle, mldl_cfg->addr, 7, regs);
+	ERROR_CHECK(result);
+
+	/* Configure slaves */
+	result = MLDLSetLevelShifterBit(mldl_cfg, gyro_handle,
+					mldl_cfg->pdata->level_shifter);
+	ERROR_CHECK(result);
+	return result;
+}
 /*******************************************************************************
  *******************************************************************************
  * Exported functions
@@ -877,6 +1134,13 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
 	mldl_cfg->dmp_cfg1 = 0;
 	mldl_cfg->dmp_cfg2 = 0;
 	mldl_cfg->gyro_power = 0;
+	mldl_cfg->gyro_is_bypassed = TRUE;
+	mldl_cfg->dmp_is_running = FALSE;
+	mldl_cfg->gyro_is_suspended = TRUE;
+	mldl_cfg->accel_is_suspended = TRUE;
+	mldl_cfg->compass_is_suspended = TRUE;
+	mldl_cfg->pressure_is_suspended = TRUE;
+	mldl_cfg->gyro_needs_reset = FALSE;
 	if (mldl_cfg->addr == 0) {
 #ifdef __KERNEL__
 		return ML_ERROR_INVALID_PARAMETER;
@@ -891,20 +1155,21 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
 	 * read the product_id, sillicon rev and whoami
 	 */
 #ifdef M_HW
-	result =
-	    MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, RESET, WAKE_UP);
+	result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
+				  RESET, WAKE_UP);
 #else
-	result =
-	    MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, RESET, 0, 0, 0, 0);
+	result = MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, RESET, 0, 0, 0, 0);
 #endif
 	ERROR_CHECK(result);
 
 	result = MLDLGetSiliconRev(mldl_cfg, mlsl_handle);
 	ERROR_CHECK(result);
+#ifndef M_HW
 	result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
 				MPUREG_PRODUCT_ID, 1,
 				&mldl_cfg->product_id);
 	ERROR_CHECK(result);
+#endif
 
 	/* Get the factory temperature compensation offsets */
 	result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
@@ -922,7 +1187,8 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
 
 	/* Configure the MPU */
 #ifdef M_HW
-	result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, 0, SLEEP);
+	result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
+				  FALSE, SLEEP);
 #else
 	result =
 	    MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, 0, SLEEP, 0, 0, 0);
@@ -931,26 +1197,25 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
 
 	if (mldl_cfg->accel && mldl_cfg->accel->init) {
 		result = mldl_cfg->accel->init(accel_handle,
-					mldl_cfg->accel,
-					&mldl_cfg->pdata->accel);
+					       mldl_cfg->accel,
+					       &mldl_cfg->pdata->accel);
 		ERROR_CHECK(result);
 	}
 
 	if (mldl_cfg->compass && mldl_cfg->compass->init) {
 		result = mldl_cfg->compass->init(compass_handle,
-						mldl_cfg->compass,
-						&mldl_cfg->pdata->compass);
+						 mldl_cfg->compass,
+						 &mldl_cfg->pdata->compass);
 		if (ML_SUCCESS != result) {
 			MPL_LOGE("mldl_cfg->compass->init returned %d\n",
 				result);
 			goto out_accel;
 		}
 	}
-
 	if (mldl_cfg->pressure && mldl_cfg->pressure->init) {
 		result = mldl_cfg->pressure->init(pressure_handle,
-						mldl_cfg->pressure,
-						&mldl_cfg->pdata->pressure);
+						  mldl_cfg->pressure,
+						  &mldl_cfg->pdata->pressure);
 		if (ML_SUCCESS != result) {
 			MPL_LOGE("mldl_cfg->pressure->init returned %d\n",
 				result);
@@ -958,6 +1223,16 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
 		}
 	}
 
+	mldl_cfg->requested_sensors = ML_THREE_AXIS_GYRO;
+	if (mldl_cfg->accel && mldl_cfg->accel->resume)
+		mldl_cfg->requested_sensors |= ML_THREE_AXIS_ACCEL;
+
+	if (mldl_cfg->compass && mldl_cfg->compass->resume)
+		mldl_cfg->requested_sensors |= ML_THREE_AXIS_COMPASS;
+
+	if (mldl_cfg->pressure && mldl_cfg->pressure->resume)
+		mldl_cfg->requested_sensors |= ML_THREE_AXIS_PRESSURE;
+
 	return result;
 
 out_compass:
@@ -1028,7 +1303,10 @@ int mpu3050_close(struct mldl_cfg *mldl_cfg,
 /**
  *  @brief  resume the MPU3050 device and all the other sensor
  *          devices from their low power state.
- *  @param  mlsl_handle
+ *
+ *  @param  mldl_cfg
+ *              pointer to the configuration structure
+ *  @param  gyro_handle
  *              the main file handle to the MPU3050 device.
  *  @param  accel_handle
  *              an handle to the accelerometer device, if sitting
@@ -1045,6 +1323,10 @@ int mpu3050_close(struct mldl_cfg *mldl_cfg,
  *              onto a separate bus. Can match mlsl_handle if
  *              the pressure sensor device operates on the same
  *              primary bus of MPU.
+ *  @param  resume_gyro
+ *              whether resuming the gyroscope device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
  *  @param  resume_accel
  *              whether resuming the accelerometer device is
  *              actually needed (if the device supports low power
@@ -1060,318 +1342,131 @@ int mpu3050_close(struct mldl_cfg *mldl_cfg,
  *  @return  ML_SUCCESS or a non-zero error code.
  */
 int mpu3050_resume(struct mldl_cfg *mldl_cfg,
-		   void *mlsl_handle,
+		   void *gyro_handle,
 		   void *accel_handle,
 		   void *compass_handle,
 		   void *pressure_handle,
-		   bool resume_accel, bool resume_compass, bool resume_pressure)
+		   bool resume_gyro,
+		   bool resume_accel,
+		   bool resume_compass,
+		   bool resume_pressure)
 {
-	int result;
-	int ii;
-	int jj;
-	unsigned char reg;
-	unsigned char regs[7];
-
+	int result = ML_SUCCESS;
 
-#ifdef CONFIG_SENSORS_MPU_DEBUG
+#ifdef CONFIG_MPU_SENSORS_DEBUG
 	mpu_print_cfg(mldl_cfg);
 #endif
 
-	/* Wake up the part */
-#ifdef M_HW
-	result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, 0, WAKE_UP);
-#else
-	result = MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, 1, 0,
-				  mldl_cfg->gyro_power & BIT_STBY_XG,
-				  mldl_cfg->gyro_power & BIT_STBY_YG,
-				  mldl_cfg->gyro_power & BIT_STBY_ZG);
-#endif
-	ERROR_CHECK(result);
-
-	/* Configure the MPU */
-#ifdef M_HW
-	result = MLDLSetI2CBypass(mldl_cfg, mlsl_handle, 1);
-	ERROR_CHECK(result);
-	result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, RESET, SLEEP);
-	ERROR_CHECK(result);
-	result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, 0, WAKE_UP);
-	ERROR_CHECK(result);
-	/* setting int_config with the propert flag BIT_BYPASS_EN should be
-	   done by the setup functions */
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_INT_PIN_CFG,
-				       (mldl_cfg->pdata->int_config |
-					       BIT_BYPASS_EN));
-	ERROR_CHECK(result);
-	/* temporary: masking out higher bits to avoid switching intelligence */
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_INT_ENABLE,
-				       (mldl_cfg->int_config & 0x0f));
-	ERROR_CHECK(result);
-#else
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_INT_CFG,
-				       (mldl_cfg->
-					int_config | mldl_cfg->pdata->
-					int_config));
-	ERROR_CHECK(result);
-#endif
-
-	result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
-				MPUREG_PWR_MGM, 1, &reg);
-	ERROR_CHECK(result);
-	reg &= ~BITS_CLKSEL;
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_PWR_MGM,
-				       mldl_cfg->clk_src | reg);
-	ERROR_CHECK(result);
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_SMPLRT_DIV,
-				       mldl_cfg->divider);
-	ERROR_CHECK(result);
-
-#ifdef M_HW
-	reg = DLPF_FS_SYNC_VALUE(0, mldl_cfg->full_scale, 0);
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_GYRO_CONFIG, reg);
-	reg = DLPF_FS_SYNC_VALUE(0, 0, mldl_cfg->lpf);
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_CONFIG, reg);
-#else
-	reg = DLPF_FS_SYNC_VALUE(mldl_cfg->ext_sync,
-				 mldl_cfg->full_scale, mldl_cfg->lpf);
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_DLPF_FS_SYNC, reg);
-#endif
-	ERROR_CHECK(result);
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_DMP_CFG_1,
-				       mldl_cfg->dmp_cfg1);
-	ERROR_CHECK(result);
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_DMP_CFG_2,
-				       mldl_cfg->dmp_cfg2);
-	ERROR_CHECK(result);
-
-	/* Write and verify memory */
-	for (ii = 0; ii < MPU_MEM_NUM_RAM_BANKS; ii++) {
-		unsigned char read[MPU_MEM_BANK_SIZE];
+	if (resume_accel &&
+	    ((!mldl_cfg->accel) || (!mldl_cfg->accel->resume)))
+		return ML_ERROR_INVALID_PARAMETER;
+	if (resume_compass &&
+	    ((!mldl_cfg->compass) || (!mldl_cfg->compass->resume)))
+		return ML_ERROR_INVALID_PARAMETER;
+	if (resume_pressure &&
+	    ((!mldl_cfg->pressure) || (!mldl_cfg->pressure->resume)))
+		return ML_ERROR_INVALID_PARAMETER;
 
-		result = MLSLSerialWriteMem(mlsl_handle, mldl_cfg->addr,
-					    ((ii << 8) | 0x00),
-					    MPU_MEM_BANK_SIZE,
-					    mldl_cfg->ram[ii]);
-		ERROR_CHECK(result);
-		result = MLSLSerialReadMem(mlsl_handle, mldl_cfg->addr,
-					   ((ii << 8) | 0x00),
-					   MPU_MEM_BANK_SIZE, read);
+	if (resume_gyro && mldl_cfg->gyro_is_suspended) {
+		result = gyro_resume(mldl_cfg, gyro_handle);
 		ERROR_CHECK(result);
-
-#ifdef M_HW
-#define ML_SKIP_CHECK 38
-#else
-#define ML_SKIP_CHECK 20
-#endif
-		for (jj = 0; jj < MPU_MEM_BANK_SIZE; jj++) {
-			/* skip the register memory locations */
-			if (ii == 0 && jj < ML_SKIP_CHECK)
-				continue;
-			if (mldl_cfg->ram[ii][jj] != read[jj]) {
-				result = ML_ERROR_SERIAL_WRITE;
-				break;
-			}
-		}
-		ERROR_CHECK(result);
-	}
-
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_XG_OFFS_TC,
-				       mldl_cfg->offset_tc[0]);
-	ERROR_CHECK(result);
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_YG_OFFS_TC,
-				       mldl_cfg->offset_tc[1]);
-	ERROR_CHECK(result);
-	result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
-				       MPUREG_ZG_OFFS_TC,
-				       mldl_cfg->offset_tc[2]);
-	ERROR_CHECK(result);
-
-	regs[0] = MPUREG_X_OFFS_USRH;
-	for (ii = 0; ii < DIM(mldl_cfg->offset); ii++) {
-		regs[1 + ii * 2] =
-		    (unsigned char)(mldl_cfg->offset[ii] >> 8) & 0xff;
-		regs[1 + ii * 2 + 1] =
-		    (unsigned char)(mldl_cfg->offset[ii] & 0xff);
 	}
-	result = MLSLSerialWrite(mlsl_handle, mldl_cfg->addr, 7, regs);
-	ERROR_CHECK(result);
-
-	/* Configure slaves */
-	result = MLDLSetLevelShifterBit(mldl_cfg, mlsl_handle,
-					mldl_cfg->pdata->level_shifter);
-	ERROR_CHECK(result);
-
-	if (resume_accel) {
-		if ((!mldl_cfg->accel) || (!mldl_cfg->accel->resume))
-			return ML_ERROR_INVALID_PARAMETER;
 
+	if (resume_accel && mldl_cfg->accel_is_suspended) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
+			result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
+			ERROR_CHECK(result);
+		}
 		result = mldl_cfg->accel->resume(accel_handle,
 						 mldl_cfg->accel,
 						 &mldl_cfg->pdata->accel);
 		ERROR_CHECK(result);
-		if (EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
-			/* Address */
-			result =
-			    MLSLSerialWriteSingle(accel_handle,
-						  mldl_cfg->addr,
-						  MPUREG_AUX_SLV_ADDR,
-						  mldl_cfg->pdata->
-						  accel.address);
-			ERROR_CHECK(result);
-			/* Register */
-			result =
-			    MLSLSerialRead(accel_handle, mldl_cfg->addr,
-					   MPUREG_ACCEL_BURST_ADDR, 1,
-					   &reg);
-			ERROR_CHECK(result);
-			reg = ((reg & 0x80) | mldl_cfg->accel->reg);
-			/* Set VDDIO bit for ST accel */
-			if ((ACCEL_ID_LIS331 == mldl_cfg->accel->id)
-			    || (ACCEL_ID_LSM303 == mldl_cfg->accel->id)) {
-				reg |= 0x80;
-			}
-			result =
-			    MLSLSerialWriteSingle(accel_handle,
-						  mldl_cfg->addr,
-						  MPUREG_ACCEL_BURST_ADDR,
-						  reg);
-			ERROR_CHECK(result);
-			/* Length */
-			result =
-			    MLSLSerialRead(accel_handle, mldl_cfg->addr,
-					   MPUREG_USER_CTRL, 1, &reg);
-			ERROR_CHECK(result);
-			reg = (reg & ~BIT_AUX_RD_LENG);
-			result =
-			    MLSLSerialWriteSingle(accel_handle,
-						  mldl_cfg->addr,
-						  MPUREG_USER_CTRL, reg);
-			ERROR_CHECK(result);
-			result =
-			    MLDLSetI2CBypass(mldl_cfg, accel_handle, 0);
-			ERROR_CHECK(result);
-		}
+		mldl_cfg->accel_is_suspended = FALSE;
 	}
 
-	if (resume_compass) {
-		if ((mldl_cfg->compass) && (mldl_cfg->compass->resume)) {
-			result = mldl_cfg->compass->resume(compass_handle,
-							   mldl_cfg->compass,
-							   &mldl_cfg->pdata->
-							   compass);
+	if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->accel_is_suspended &&
+		EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
+		result = mpu_set_slave(mldl_cfg,
+				gyro_handle,
+				mldl_cfg->accel,
+				&mldl_cfg->pdata->accel);
+		ERROR_CHECK(result);
+	}
+
+	if (resume_compass && mldl_cfg->compass_is_suspended) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) {
+			result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
 			ERROR_CHECK(result);
 		}
+		result = mldl_cfg->compass->resume(compass_handle,
+						   mldl_cfg->compass,
+						   &mldl_cfg->pdata->
+						   compass);
 		ERROR_CHECK(result);
-		if (EXT_SLAVE_BUS_SECONDARY ==
-		    mldl_cfg->pdata->compass.bus) {
-			/* Address */
-			result =
-			    MLSLSerialWriteSingle(mlsl_handle,
-						  mldl_cfg->addr,
-						  MPUREG_AUX_SLV_ADDR,
-						  mldl_cfg->pdata->
-						  compass.address);
-			ERROR_CHECK(result);
-			/* Register */
-			result =
-			    MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
-					   MPUREG_ACCEL_BURST_ADDR, 1,
-					   &reg);
-			ERROR_CHECK(result);
-			reg = ((reg & 0x80) | mldl_cfg->compass->reg);
-			result =
-			    MLSLSerialWriteSingle(mlsl_handle,
-						  mldl_cfg->addr,
-						  MPUREG_ACCEL_BURST_ADDR,
-						  reg);
-			ERROR_CHECK(result);
+		mldl_cfg->compass_is_suspended = FALSE;
+	}
 
-#ifdef M_HW
-			/* Length, byte swapping, grouping & enable */
-			if (mldl_cfg->compass->len > BITS_SLV_LENG) {
-				MPL_LOGW("Limiting slave burst read length to "
-					 "the allowed maximum (15B, req. %d)\n",
-					 mldl_cfg->compass->len);
-				mldl_cfg->compass->len = BITS_SLV_LENG;
-			}
-			reg = mldl_cfg->compass->len;
-			if (mldl_cfg->compass->endian ==
-			    EXT_SLAVE_LITTLE_ENDIAN)
-				reg |= BIT_SLV_BYTE_SW;
-			reg |= BIT_SLV_GRP;
-			reg |= BIT_SLV_ENABLE;
+	if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->compass_is_suspended &&
+		EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) {
+		result = mpu_set_slave(mldl_cfg,
+				gyro_handle,
+				mldl_cfg->compass,
+				&mldl_cfg->pdata->compass);
+		ERROR_CHECK(result);
+	}
 
-			result =
-			    MLSLSerialWriteSingle(mlsl_handle,
-						  mldl_cfg->addr,
-						  MPUREG_I2C_SLV0_CTRL,
-						  reg);
-#else
-			/* Length */
-			result =
-			    MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
-					   MPUREG_USER_CTRL, 1, &reg);
-			ERROR_CHECK(result);
-			reg = (reg & ~BIT_AUX_RD_LENG);
-			result =
-			    MLSLSerialWriteSingle(mlsl_handle,
-						  mldl_cfg->addr,
-						  MPUREG_USER_CTRL, reg);
-			ERROR_CHECK(result);
-#endif
-			result =
-			    MLDLSetI2CBypass(mldl_cfg, mlsl_handle, 0);
+	if (resume_pressure && mldl_cfg->pressure_is_suspended) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) {
+			result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
 			ERROR_CHECK(result);
 		}
+		result = mldl_cfg->pressure->resume(pressure_handle,
+						    mldl_cfg->pressure,
+						    &mldl_cfg->pdata->
+						    pressure);
+		ERROR_CHECK(result);
+		mldl_cfg->pressure_is_suspended = FALSE;
 	}
 
-	if (resume_pressure) {
-		if ((mldl_cfg->pressure) && (mldl_cfg->pressure->resume)) {
-			result = mldl_cfg->pressure->resume(pressure_handle,
-							   mldl_cfg->pressure,
-							   &mldl_cfg->pdata->
-							   pressure);
-			ERROR_CHECK(result);
-		}
+	if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->pressure_is_suspended &&
+		EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) {
+		result = mpu_set_slave(mldl_cfg,
+				gyro_handle,
+				mldl_cfg->pressure,
+				&mldl_cfg->pdata->pressure);
 		ERROR_CHECK(result);
 	}
 
 	/* Now start */
-	result = MLDLDmpStart(mldl_cfg, mlsl_handle);
-	ERROR_CHECK(result);
+	if (resume_gyro) {
+		result = dmp_start(mldl_cfg, gyro_handle);
+		ERROR_CHECK(result);
+	}
+
 	return result;
 }
 
-
 /**
  *  @brief  suspend the MPU3050 device and all the other sensor
  *          devices into their low power state.
- *  @param  mlsl_handle
+ *  @param  gyro_handle
  *              the main file handle to the MPU3050 device.
  *  @param  accel_handle
  *              an handle to the accelerometer device, if sitting
- *              onto a separate bus. Can match mlsl_handle if
+ *              onto a separate bus. Can match gyro_handle if
  *              the accelerometer device operates on the same
  *              primary bus of MPU.
  *  @param  compass_handle
  *              an handle to the compass device, if sitting
- *              onto a separate bus. Can match mlsl_handle if
+ *              onto a separate bus. Can match gyro_handle if
  *              the compass device operates on the same
  *              primary bus of MPU.
  *  @param  pressure_handle
  *              an handle to the pressure sensor device, if sitting
- *              onto a separate bus. Can match mlsl_handle if
+ *              onto a separate bus. Can match gyro_handle if
  *              the pressure sensor device operates on the same
  *              primary bus of MPU.
  *  @param  accel
@@ -1388,44 +1483,77 @@ int mpu3050_resume(struct mldl_cfg *mldl_cfg,
  *              mode of some sort).
  *  @return  ML_SUCCESS or a non-zero error code.
  */
-int mpu3050_suspend(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+int mpu3050_suspend(struct mldl_cfg *mldl_cfg,
+		    void *gyro_handle,
 		    void *accel_handle,
 		    void *compass_handle,
 		    void *pressure_handle,
-		    bool accel, bool compass, bool pressure)
+		    bool suspend_gyro,
+		    bool suspend_accel,
+		    bool suspend_compass,
+		    bool suspend_pressure)
 {
-	int result;
-	/* This puts the bus into bypass mode */
+	int result = ML_SUCCESS;
 
+	if (suspend_gyro && !mldl_cfg->gyro_is_suspended) {
 #ifdef M_HW
-	result = MLDLSetI2CBypass(mldl_cfg, mlsl_handle, 1);
-	ERROR_CHECK(result);
-	result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, 0, SLEEP);
+		return ML_SUCCESS;
+		/* This puts the bus into bypass mode */
+		result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, 1);
+		ERROR_CHECK(result);
+		result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, 0, SLEEP);
 #else
-	result =
-	    MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, 0, SLEEP, 0, 0, 0);
+		result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle,
+					0, SLEEP, 0, 0, 0);
 #endif
-	if (ML_SUCCESS == result &&
-	    accel && mldl_cfg->accel && mldl_cfg->accel->suspend) {
+		ERROR_CHECK(result);
+	}
+
+	if (!mldl_cfg->accel_is_suspended && suspend_accel &&
+	    mldl_cfg->accel && mldl_cfg->accel->suspend) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
+			result = mpu_set_slave(mldl_cfg, gyro_handle,
+					       NULL, NULL);
+			ERROR_CHECK(result);
+		}
 		result = mldl_cfg->accel->suspend(accel_handle,
 						  mldl_cfg->accel,
 						  &mldl_cfg->pdata->accel);
+		ERROR_CHECK(result);
+		mldl_cfg->accel_is_suspended = TRUE;
 	}
 
-	if (ML_SUCCESS == result && compass &&
+	if (!mldl_cfg->compass_is_suspended && suspend_compass &&
 	    mldl_cfg->compass && mldl_cfg->compass->suspend) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) {
+			result = mpu_set_slave(mldl_cfg, gyro_handle,
+					       NULL, NULL);
+			ERROR_CHECK(result);
+		}
 		result = mldl_cfg->compass->suspend(compass_handle,
 						    mldl_cfg->compass,
 						    &mldl_cfg->
 						    pdata->compass);
+		ERROR_CHECK(result);
+		mldl_cfg->compass_is_suspended = TRUE;
 	}
 
-	if (ML_SUCCESS == result && pressure &&
+	if (!mldl_cfg->pressure_is_suspended && suspend_pressure &&
 	    mldl_cfg->pressure && mldl_cfg->pressure->suspend) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) {
+			result = mpu_set_slave(mldl_cfg, gyro_handle,
+					       NULL, NULL);
+			ERROR_CHECK(result);
+		}
 		result = mldl_cfg->pressure->suspend(pressure_handle,
 						    mldl_cfg->pressure,
 						    &mldl_cfg->
 						    pdata->pressure);
+		ERROR_CHECK(result);
+		mldl_cfg->pressure_is_suspended = TRUE;
 	}
 	return result;
 }
@@ -1434,18 +1562,26 @@ int mpu3050_suspend(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
 /**
  *  @brief  read raw sensor data from the accelerometer device
  *          in use.
+ *  @param  mldl_cfg
+ *              A pointer to the struct mldl_cfg data structure.
+ *  @param  accel_handle
+ *              The handle to the device the accelerometer is connected to.
  *  @param  data
  *              a buffer to store the raw sensor data.
  *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
  */
 int mpu3050_read_accel(struct mldl_cfg *mldl_cfg,
-		       void *mlsl_handle, unsigned char *data)
+		       void *accel_handle, unsigned char *data)
 {
 	if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->read)
-		return mldl_cfg->accel->read(mlsl_handle,
-					     mldl_cfg->accel,
-					     &mldl_cfg->pdata->accel,
-					     data);
+		if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus)
+			&& (!mldl_cfg->gyro_is_bypassed))
+			return ML_ERROR_FEATURE_NOT_ENABLED;
+		else
+			return mldl_cfg->accel->read(accel_handle,
+						     mldl_cfg->accel,
+						     &mldl_cfg->pdata->accel,
+						     data);
 	else
 		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
 }
@@ -1453,18 +1589,26 @@ int mpu3050_read_accel(struct mldl_cfg *mldl_cfg,
 /**
  *  @brief  read raw sensor data from the compass device
  *          in use.
+ *  @param  mldl_cfg
+ *              A pointer to the struct mldl_cfg data structure.
+ *  @param  compass_handle
+ *              The handle to the device the compass is connected to.
  *  @param  data
  *              a buffer to store the raw sensor data.
  *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
  */
 int mpu3050_read_compass(struct mldl_cfg *mldl_cfg,
-			 void *mlsl_handle, unsigned char *data)
+			 void *compass_handle, unsigned char *data)
 {
 	if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->read)
-		return mldl_cfg->compass->read(mlsl_handle,
-					       mldl_cfg->compass,
-					       &mldl_cfg->pdata->compass,
-					       data);
+		if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus)
+			&& (!mldl_cfg->gyro_is_bypassed))
+			return ML_ERROR_FEATURE_NOT_ENABLED;
+		else
+			return mldl_cfg->compass->read(compass_handle,
+						mldl_cfg->compass,
+						&mldl_cfg->pdata->compass,
+						data);
 	else
 		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
 }
@@ -1472,18 +1616,27 @@ int mpu3050_read_compass(struct mldl_cfg *mldl_cfg,
 /**
  *  @brief  read raw sensor data from the pressure device
  *          in use.
+ *  @param  mldl_cfg
+ *              A pointer to the struct mldl_cfg data structure.
+ *  @param  pressure_handle
+ *              The handle to the device the pressure sensor is connected to.
  *  @param  data
  *              a buffer to store the raw sensor data.
  *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
  */
 int mpu3050_read_pressure(struct mldl_cfg *mldl_cfg,
-			 void *mlsl_handle, unsigned char *data)
+			 void *pressure_handle, unsigned char *data)
 {
 	if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->read)
-		return mldl_cfg->pressure->read(mlsl_handle,
-					       mldl_cfg->pressure,
-					       &mldl_cfg->pdata->pressure,
-					       data);
+		if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus)
+			&& (!mldl_cfg->gyro_is_bypassed))
+			return ML_ERROR_FEATURE_NOT_ENABLED;
+		else
+			return mldl_cfg->pressure->read(
+				pressure_handle,
+				mldl_cfg->pressure,
+				&mldl_cfg->pdata->pressure,
+				data);
 	else
 		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
 }
@@ -1494,9 +1647,9 @@ int mpu3050_config_accel(struct mldl_cfg *mldl_cfg,
 {
 	if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->config)
 		return mldl_cfg->accel->config(accel_handle,
-					mldl_cfg->accel,
-					&mldl_cfg->pdata->accel,
-					data);
+					       mldl_cfg->accel,
+					       &mldl_cfg->pdata->accel,
+					       data);
 	else
 		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
 
@@ -1508,9 +1661,9 @@ int mpu3050_config_compass(struct mldl_cfg *mldl_cfg,
 {
 	if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->config)
 		return mldl_cfg->compass->config(compass_handle,
-						mldl_cfg->compass,
-						&mldl_cfg->pdata->compass,
-						data);
+						 mldl_cfg->compass,
+						 &mldl_cfg->pdata->compass,
+						 data);
 	else
 		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
 
@@ -1522,6 +1675,48 @@ int mpu3050_config_pressure(struct mldl_cfg *mldl_cfg,
 {
 	if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->config)
 		return mldl_cfg->pressure->config(pressure_handle,
+						  mldl_cfg->pressure,
+						  &mldl_cfg->pdata->pressure,
+						  data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+int mpu3050_get_config_accel(struct mldl_cfg *mldl_cfg,
+			void *accel_handle,
+			struct ext_slave_config *data)
+{
+	if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->get_config)
+		return mldl_cfg->accel->get_config(accel_handle,
+						mldl_cfg->accel,
+						&mldl_cfg->pdata->accel,
+						data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+}
+
+int mpu3050_get_config_compass(struct mldl_cfg *mldl_cfg,
+			void *compass_handle,
+			struct ext_slave_config *data)
+{
+	if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->get_config)
+		return mldl_cfg->compass->get_config(compass_handle,
+						mldl_cfg->compass,
+						&mldl_cfg->pdata->compass,
+						data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+}
+
+int mpu3050_get_config_pressure(struct mldl_cfg *mldl_cfg,
+				void *pressure_handle,
+				struct ext_slave_config *data)
+{
+	if (NULL != mldl_cfg->pressure &&
+	    NULL != mldl_cfg->pressure->get_config)
+		return mldl_cfg->pressure->get_config(pressure_handle,
 						mldl_cfg->pressure,
 						&mldl_cfg->pdata->pressure,
 						data);