diff --git a/drivers/misc/inv_mpu/mldl_cfg.c b/drivers/misc/inv_mpu/mldl_cfg.c
new file mode 100644
index 0000000..55a772d
--- /dev/null
+++ b/drivers/misc/inv_mpu/mldl_cfg.c
@@ -0,0 +1,1737 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+
+	This program is free software; you can redistribute it and/or modify
+	it under the terms of the GNU General Public License as published by
+	the Free Software Foundation; either version 2 of the License, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program.  If not, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_cfg.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+/* -------------------------------------------------------------------------- */
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include <stddef.h>
+
+#include "mldl_cfg.h"
+#include <linux/mpu.h>
+#  include "mpu3050.h"
+
+#include "mlsl.h"
+#include "mldl_print_cfg.h"
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_cfg:"
+
+/* -------------------------------------------------------------------------- */
+
+#define SLEEP   1
+#define WAKE_UP 0
+#define RESET   1
+#define STANDBY 1
+
+/* -------------------------------------------------------------------------- */
+
+/**
+ * @brief Stop the DMP running
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	unsigned char user_ctrl_reg;
+	int result;
+
+	if (mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)
+		return INV_SUCCESS;
+
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	user_ctrl_reg = (user_ctrl_reg & (~BIT_FIFO_EN)) | BIT_FIFO_RST;
+	user_ctrl_reg = (user_ctrl_reg & (~BIT_DMP_EN)) | BIT_DMP_RST;
+
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 MPUREG_USER_CTRL, user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	mldl_cfg->inv_mpu_state->status |= MPU_DMP_IS_SUSPENDED;
+
+	return result;
+}
+
+/**
+ * @brief Starts the DMP running
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+static int dmp_start(struct mldl_cfg *mldl_cfg, void *mlsl_handle)
+{
+	unsigned char user_ctrl_reg;
+	int result;
+
+	if ((!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) &&
+	    mldl_cfg->mpu_gyro_cfg->dmp_enable)
+		||
+	   ((mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) &&
+		   !mldl_cfg->mpu_gyro_cfg->dmp_enable))
+		return INV_SUCCESS;
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL,
+		((user_ctrl_reg & (~BIT_FIFO_EN))
+			| BIT_FIFO_RST));
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	user_ctrl_reg |= BIT_DMP_EN;
+
+	if (mldl_cfg->mpu_gyro_cfg->fifo_enable)
+		user_ctrl_reg |= BIT_FIFO_EN;
+	else
+		user_ctrl_reg &= ~BIT_FIFO_EN;
+
+	user_ctrl_reg |= BIT_DMP_RST;
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	mldl_cfg->inv_mpu_state->status &= ~MPU_DMP_IS_SUSPENDED;
+
+	return result;
+}
+
+
+
+static int mpu3050_set_i2c_bypass(struct mldl_cfg *mldl_cfg,
+				  void *mlsl_handle, unsigned char enable)
+{
+	unsigned char b;
+	int result;
+	unsigned char status = mldl_cfg->inv_mpu_state->status;
+
+	if ((status & MPU_GYRO_IS_BYPASSED && enable) ||
+	    (!(status & MPU_GYRO_IS_BYPASSED) && !enable))
+		return INV_SUCCESS;
+
+	/*---- get current 'USER_CTRL' into b ----*/
+	result = inv_serial_read(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, 1, &b);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	b &= ~BIT_AUX_IF_EN;
+
+	if (!enable) {
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_USER_CTRL,
+			(b | BIT_AUX_IF_EN));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	} else {
+		/* 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_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_AUX_SLV_ADDR, 0x7F);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		/*
+		 * 2) wait enough time for a nack to occur, then go into
+		 *    bypass mode:
+		 */
+		msleep(2);
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_USER_CTRL, (b));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		/*
+		 * 3) wait for up to one MPU cycle then restore the slave
+		 *    address
+		 */
+		msleep(inv_mpu_get_sampling_period_us(mldl_cfg->mpu_gyro_cfg)
+			/ 1000);
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_AUX_SLV_ADDR,
+			mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]
+				->address);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		/*
+		 * 4) reset the ime interface
+		 */
+
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_USER_CTRL,
+			(b | BIT_AUX_IF_RST));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		msleep(2);
+	}
+	if (enable)
+		mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED;
+	else
+		mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
+
+	return result;
+}
+
+
+/**
+ *  @brief  enables/disables the I2C bypass to an external device
+ *          connected to MPU's secondary I2C bus.
+ *  @param  enable
+ *              Non-zero to enable pass through.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+static int mpu_set_i2c_bypass(struct mldl_cfg *mldl_cfg,
+			      void *mlsl_handle, unsigned char enable)
+{
+	return mpu3050_set_i2c_bypass(mldl_cfg, mlsl_handle, enable);
+}
+
+
+#define NUM_OF_PROD_REVS (ARRAY_SIZE(prod_rev_map))
+
+/* NOTE : when not indicated, product revision
+	  is considered an 'npp'; non production part */
+
+struct prod_rev_map_t {
+	unsigned char silicon_rev;
+	unsigned short gyro_trim;
+};
+
+#define OLDEST_PROD_REV_SUPPORTED	11
+static struct prod_rev_map_t prod_rev_map[] = {
+	{0, 0},
+	{MPU_SILICON_REV_A4, 131},	/* 1  A? OBSOLETED */
+	{MPU_SILICON_REV_A4, 131},	/* 2  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 3  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 4  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 5  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 6  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 7  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 8  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 9  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 10 V  */
+	{MPU_SILICON_REV_B1, 131},	/* 11 B1 */
+	{MPU_SILICON_REV_B1, 131},	/* 12 |  */
+	{MPU_SILICON_REV_B1, 131},	/* 13 |  */
+	{MPU_SILICON_REV_B1, 131},	/* 14 V  */
+	{MPU_SILICON_REV_B4, 131},	/* 15 B4 */
+	{MPU_SILICON_REV_B4, 131},	/* 16 |  */
+	{MPU_SILICON_REV_B4, 131},	/* 17 |  */
+	{MPU_SILICON_REV_B4, 131},	/* 18 |  */
+	{MPU_SILICON_REV_B4, 115},	/* 19 |  */
+	{MPU_SILICON_REV_B4, 115},	/* 20 V  */
+	{MPU_SILICON_REV_B6, 131},	/* 21 B6 (B6/A9)  */
+	{MPU_SILICON_REV_B4, 115},	/* 22 B4 (B7/A10) */
+	{MPU_SILICON_REV_B6, 0},	/* 23 B6 */
+	{MPU_SILICON_REV_B6, 0},	/* 24 |  */
+	{MPU_SILICON_REV_B6, 0},	/* 25 |  */
+	{MPU_SILICON_REV_B6, 131},	/* 26 V  (B6/A11) */
+};
+
+/**
+ *  @internal
+ *  @brief  Get the silicon revision ID from OTP for MPU3050.
+ *          The silicon revision number is in read from OTP bank 0,
+ *          ADDR6[7:2].  The corresponding ID is retrieved by lookup
+ *          in a map.
+ *
+ *  @param  mldl_cfg
+ *              a pointer to the mldl config data structure.
+ *  @param  mlsl_handle
+ *              an file handle to the serial communication device the
+ *              device is connected to.
+ *
+ *  @return 0 on success, a non-zero error code otherwise.
+ */
+static int inv_get_silicon_rev_mpu3050(
+		struct mldl_cfg *mldl_cfg, void *mlsl_handle)
+{
+	int result;
+	unsigned char index = 0x00;
+	unsigned char bank =
+	    (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
+	unsigned short mem_addr = ((bank << 8) | 0x06);
+	struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_PRODUCT_ID, 1,
+				 &mpu_chip_info->product_id);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_read_mem(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		mem_addr, 1, &index);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	index >>= 2;
+
+	/* clean the prefetch and cfg user bank bits */
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_BANK_SEL, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (index < OLDEST_PROD_REV_SUPPORTED || index >= NUM_OF_PROD_REVS) {
+		mpu_chip_info->silicon_revision = 0;
+		mpu_chip_info->gyro_sens_trim = 0;
+		MPL_LOGE("Unsupported Product Revision Detected : %d\n", index);
+		return INV_ERROR_INVALID_MODULE;
+	}
+
+	mpu_chip_info->product_revision = index;
+	mpu_chip_info->silicon_revision = prod_rev_map[index].silicon_rev;
+	mpu_chip_info->gyro_sens_trim = prod_rev_map[index].gyro_trim;
+	if (mpu_chip_info->gyro_sens_trim == 0) {
+		MPL_LOGE("gyro sensitivity trim is 0"
+			 " - unsupported non production part.\n");
+		return INV_ERROR_INVALID_MODULE;
+	}
+
+	return result;
+}
+#define inv_get_silicon_rev inv_get_silicon_rev_mpu3050
+
+
+/**
+ *  @brief  Enable / Disable the use MPU's secondary I2C interface level
+ *          shifters.
+ *          When enabled the secondary I2C interface to which the external
+ *          device is connected runs at VDD voltage (main supply).
+ *          When disabled the 2nd interface runs at VDDIO voltage.
+ *          See the device specification for more details.
+ *
+ *  @note   using this API may produce unpredictable results, depending on how
+ *          the MPU and slave device are setup on the target platform.
+ *          Use of this API should entirely be restricted to system
+ *          integrators. Once the correct value is found, there should be no
+ *          need to change the level shifter at runtime.
+ *
+ *  @pre    Must be called after inv_serial_start().
+ *  @note   Typically called before inv_dmp_open().
+ *
+ *  @param[in]  enable:
+ *                  0 to run at VDDIO (default),
+ *                  1 to run at VDD.
+ *
+ *  @return INV_SUCCESS if successfull, a non-zero error code otherwise.
+ */
+static int inv_mpu_set_level_shifter_bit(struct mldl_cfg *mldl_cfg,
+				  void *mlsl_handle, unsigned char enable)
+{
+	int result;
+	unsigned char reg;
+	unsigned char mask;
+	unsigned char regval;
+
+	if (0 == mldl_cfg->mpu_chip_info->silicon_revision)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	/*-- on parts before B6 the VDDIO bit is bit 7 of ACCEL_BURST_ADDR --
+	NOTE: this is incompatible with ST accelerometers where the VDDIO
+	bit MUST be set to enable ST's internal logic to autoincrement
+	the register address on burst reads --*/
+	if ((mldl_cfg->mpu_chip_info->silicon_revision & 0xf)
+		< MPU_SILICON_REV_B6) {
+		reg = MPUREG_ACCEL_BURST_ADDR;
+		mask = 0x80;
+	} else {
+		/*-- on B6 parts the VDDIO bit was moved to FIFO_EN2 =>
+		  the mask is always 0x04 --*/
+		reg = MPUREG_FIFO_EN2;
+		mask = 0x04;
+	}
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 reg, 1, &regval);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (enable)
+		regval |= mask;
+	else
+		regval &= ~mask;
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr, reg, regval);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+	return INV_SUCCESS;
+}
+
+
+/**
+ * @internal
+ * @brief   This function controls the power management on the MPU device.
+ *          The entire chip can be put to low power sleep mode, or individual
+ *          gyros can be turned on/off.
+ *
+ *          Putting the device into sleep mode depending upon the changing needs
+ *          of the associated applications is a recommended method for reducing
+ *          power consuption.  It is a safe opearation in that sleep/wake up of
+ *          gyros while running will not result in any interruption of data.
+ *
+ *          Although it is entirely allowed to put the device into full sleep
+ *          while running the DMP, it is not recomended because it will disrupt
+ *          the ongoing calculations carried on inside the DMP and consequently
+ *          the sensor fusion algorithm. Furthermore, while in sleep mode
+ *          read & write operation from the app processor on both registers and
+ *          memory are disabled and can only regained by restoring the MPU in
+ *          normal power mode.
+ *          Disabling any of the gyro axis will reduce the associated power
+ *          consuption from the PLL but will not stop the DMP from running
+ *          state.
+ *
+ * @param   reset
+ *              Non-zero to reset the device. Note that this setting
+ *              is volatile and the corresponding register bit will
+ *              clear itself right after being applied.
+ * @param   sleep
+ *              Non-zero to put device into full sleep.
+ * @param   disable_gx
+ *              Non-zero to disable gyro X.
+ * @param   disable_gy
+ *              Non-zero to disable gyro Y.
+ * @param   disable_gz
+ *              Non-zero to disable gyro Z.
+ *
+ * @return  INV_SUCCESS if successfull; a non-zero error code otherwise.
+ */
+static int mpu3050_pwr_mgmt(struct mldl_cfg *mldl_cfg,
+			    void *mlsl_handle,
+			    unsigned char reset,
+			    unsigned char sleep,
+			    unsigned char disable_gx,
+			    unsigned char disable_gy,
+			    unsigned char disable_gz)
+{
+	unsigned char b;
+	int result;
+
+	result =
+	    inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			    MPUREG_PWR_MGM, 1, &b);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* If we are awake, we need to put it in bypass before resetting */
+	if ((!(b & BIT_SLEEP)) && reset)
+		result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, 1);
+
+	/* Reset if requested */
+	if (reset) {
+		MPL_LOGV("Reset MPU3050\n");
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, b | BIT_H_RESET);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		msleep(5);
+		mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_RESET;
+		/* Some chips are awake after reset and some are asleep,
+		 * check the status */
+		result = inv_serial_read(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, 1, &b);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	/* Update the suspended state just in case we return early */
+	if (b & BIT_SLEEP)
+		mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_SUSPENDED;
+	else
+		mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED;
+
+	/* 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))) {
+		return INV_SUCCESS;
+	}
+
+	/*
+	 * This specific transition between states needs to be reinterpreted:
+	 *    (1,1,1,1) -> (0,1,1,1) has to become
+	 *    (1,1,1,1) -> (1,0,0,0) -> (0,1,1,1)
+	 * where
+	 *    (1,1,1,1) is (sleep=1,disable_gx=1,disable_gy=1,disable_gz=1)
+	 */
+	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)) {
+		result = mpu3050_pwr_mgmt(mldl_cfg, mlsl_handle, 0, 1, 0, 0, 0);
+		if (result)
+			return result;
+		b |= BIT_SLEEP;
+		b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
+	}
+
+	if ((b & BIT_SLEEP) != ((sleep != 0) * BIT_SLEEP)) {
+		if (sleep) {
+			result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, 1);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			b |= BIT_SLEEP;
+			result =
+			    inv_serial_single_write(
+				    mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				    MPUREG_PWR_MGM, b);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			mldl_cfg->inv_mpu_state->status |=
+				MPU_GYRO_IS_SUSPENDED;
+		} else {
+			b &= ~BIT_SLEEP;
+			result =
+			    inv_serial_single_write(
+				    mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				    MPUREG_PWR_MGM, b);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			mldl_cfg->inv_mpu_state->status &=
+				~MPU_GYRO_IS_SUSPENDED;
+			msleep(5);
+		}
+	}
+	/*---
+	  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 = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, b);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) {
+		b ^= BIT_STBY_YG;
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, b);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	if ((b & BIT_STBY_ZG) != ((disable_gz != 0) * BIT_STBY_ZG)) {
+		b ^= BIT_STBY_ZG;
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, b);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  sets the clock source for the gyros.
+ *  @param  mldl_cfg
+ *              a pointer to the struct mldl_cfg data structure.
+ *  @param  gyro_handle
+ *              an handle to the serial device the gyro is assigned to.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+static int mpu_set_clock_source(void *gyro_handle, struct mldl_cfg *mldl_cfg)
+{
+	int result;
+	unsigned char cur_clk_src;
+	unsigned char reg;
+
+	/* clock source selection */
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_PWR_MGM, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	cur_clk_src = reg & BITS_CLKSEL;
+	reg &= ~BITS_CLKSEL;
+
+
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_PWR_MGM, mldl_cfg->mpu_gyro_cfg->clk_src | reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* TODO : workarounds to be determined and implemented */
+
+	return result;
+}
+
+/**
+ * Configures the MPU I2C Master
+ *
+ * @mldl_cfg Handle to the configuration data
+ * @gyro_handle handle to the gyro communictation interface
+ * @slave Can be Null if turning off the slave
+ * @slave_pdata Can be null if turning off the slave
+ * @slave_id enum ext_slave_type to determine which index to use
+ *
+ *
+ * This fucntion configures the slaves by:
+ * 1) Setting up the read
+ *    a) Read Register
+ *    b) Read Length
+ * 2) Set up the data trigger (MPU6050 only)
+ *    a) Set trigger write register
+ *    b) Set Trigger write value
+ * 3) Set up the divider (MPU6050 only)
+ * 4) Set the slave bypass mode depending on slave
+ *
+ * returns INV_SUCCESS or non-zero error code
+ */
+static int mpu_set_slave_mpu3050(struct mldl_cfg *mldl_cfg,
+				 void *gyro_handle,
+				 struct ext_slave_descr *slave,
+				 struct ext_slave_platform_data *slave_pdata,
+				 int slave_id)
+{
+	int result;
+	unsigned char reg;
+	unsigned char slave_reg;
+	unsigned char slave_len;
+	unsigned char slave_endian;
+	unsigned char slave_address;
+
+	if (slave_id != EXT_SLAVE_TYPE_ACCEL)
+		return 0;
+
+	result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
+
+	if (NULL == slave || NULL == slave_pdata) {
+		slave_reg = 0;
+		slave_len = 0;
+		slave_endian = 0;
+		slave_address = 0;
+		mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
+	} else {
+		slave_reg = slave->read_reg;
+		slave_len = slave->read_len;
+		slave_endian = slave->endian;
+		slave_address = slave_pdata->address;
+		mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 1;
+	}
+
+	/* Address */
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 MPUREG_AUX_SLV_ADDR, slave_address);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Register */
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_ACCEL_BURST_ADDR, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	reg = ((reg & 0x80) | slave_reg);
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 MPUREG_ACCEL_BURST_ADDR, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Length */
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	reg = (reg & ~BIT_AUX_RD_LENG);
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+
+static 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 slave_id)
+{
+	return mpu_set_slave_mpu3050(mldl_cfg, gyro_handle, slave,
+				     slave_pdata, slave_id);
+}
+/**
+ * Check to see if the gyro was reset by testing a couple of registers known
+ * to change on reset.
+ *
+ * @mldl_cfg mldl configuration structure
+ * @gyro_handle handle used to communicate with the gyro
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg;
+
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_DMP_CFG_2, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (mldl_cfg->mpu_gyro_cfg->dmp_cfg2 != reg)
+		return true;
+
+	if (0 != mldl_cfg->mpu_gyro_cfg->dmp_cfg1)
+		return false;
+
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_SMPLRT_DIV, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	if (reg != mldl_cfg->mpu_gyro_cfg->divider)
+		return true;
+
+	if (0 != mldl_cfg->mpu_gyro_cfg->divider)
+		return false;
+
+	/* Inconclusive assume it was reset */
+	return true;
+}
+
+static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle,
+		       unsigned long sensors)
+{
+	int result;
+	int ii;
+	int jj;
+	unsigned char reg;
+	unsigned char regs[7];
+
+	/* Wake up the part */
+	result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, false, false,
+				  !(sensors & INV_X_GYRO),
+				  !(sensors & INV_Y_GYRO),
+				  !(sensors & INV_Z_GYRO));
+
+	if (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_RESET) &&
+	    !mpu_was_reset(mldl_cfg, gyro_handle)) {
+		return INV_SUCCESS;
+	}
+
+	result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, true, false,
+				  !(sensors & INV_X_GYRO),
+				  !(sensors & INV_Y_GYRO),
+				  !(sensors & INV_Z_GYRO));
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_INT_CFG,
+		(mldl_cfg->mpu_gyro_cfg->int_config |
+			mldl_cfg->pdata->int_config));
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu_set_clock_source(gyro_handle, mldl_cfg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_SMPLRT_DIV, mldl_cfg->mpu_gyro_cfg->divider);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	reg = DLPF_FS_SYNC_VALUE(mldl_cfg->mpu_gyro_cfg->ext_sync,
+				 mldl_cfg->mpu_gyro_cfg->full_scale,
+				 mldl_cfg->mpu_gyro_cfg->lpf);
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_DLPF_FS_SYNC, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_DMP_CFG_1, mldl_cfg->mpu_gyro_cfg->dmp_cfg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_DMP_CFG_2, mldl_cfg->mpu_gyro_cfg->dmp_cfg2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Write and verify memory */
+	for (ii = 0; ii < MPU_MEM_NUM_RAM_BANKS; ii++) {
+		unsigned char read[MPU_MEM_BANK_SIZE];
+
+		result = inv_serial_write_mem(
+			gyro_handle, mldl_cfg->mpu_chip_info->addr,
+			((ii << 8) | 0x00), MPU_MEM_BANK_SIZE,
+			&mldl_cfg->mpu_ram->ram[ii * MPU_MEM_BANK_SIZE]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_read_mem(
+			gyro_handle, mldl_cfg->mpu_chip_info->addr,
+			((ii << 8) | 0x00), MPU_MEM_BANK_SIZE, read);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+#define ML_SKIP_CHECK 20
+		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->mpu_ram->ram[ii * MPU_MEM_BANK_SIZE + jj]
+				!= read[jj]) {
+				result = INV_ERROR_SERIAL_WRITE;
+				break;
+			}
+		}
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_XG_OFFS_TC, mldl_cfg->mpu_offsets->tc[0]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_YG_OFFS_TC, mldl_cfg->mpu_offsets->tc[1]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_ZG_OFFS_TC, mldl_cfg->mpu_offsets->tc[2]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	regs[0] = MPUREG_X_OFFS_USRH;
+	for (ii = 0; ii < ARRAY_SIZE(mldl_cfg->mpu_offsets->gyro); ii++) {
+		regs[1 + ii * 2] =
+			(unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] >> 8)
+			& 0xff;
+		regs[1 + ii * 2 + 1] =
+			(unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] & 0xff);
+	}
+	result = inv_serial_write(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				  7, regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Configure slaves */
+	result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
+					       mldl_cfg->pdata->level_shifter);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+int gyro_config(void *mlsl_handle,
+		struct mldl_cfg *mldl_cfg,
+		struct ext_slave_config *data)
+{
+	struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
+	struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
+	struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
+	int ii;
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_INT_CONFIG:
+		mpu_gyro_cfg->int_config = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_EXT_SYNC:
+		mpu_gyro_cfg->ext_sync = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_FULL_SCALE:
+		mpu_gyro_cfg->full_scale = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_LPF:
+		mpu_gyro_cfg->lpf = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_CLK_SRC:
+		mpu_gyro_cfg->clk_src = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DIVIDER:
+		mpu_gyro_cfg->divider = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DMP_ENABLE:
+		mpu_gyro_cfg->dmp_enable = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_FIFO_ENABLE:
+		mpu_gyro_cfg->fifo_enable = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DMP_CFG1:
+		mpu_gyro_cfg->dmp_cfg1 = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DMP_CFG2:
+		mpu_gyro_cfg->dmp_cfg2 = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_TC:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			mpu_offsets->tc[ii] = ((__u8 *)data->data)[ii];
+		break;
+	case MPU_SLAVE_GYRO:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			mpu_offsets->gyro[ii] = ((__u16 *)data->data)[ii];
+		break;
+	case MPU_SLAVE_ADDR:
+		mpu_chip_info->addr = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_PRODUCT_REVISION:
+		mpu_chip_info->product_revision = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_SILICON_REVISION:
+		mpu_chip_info->silicon_revision = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_PRODUCT_ID:
+		mpu_chip_info->product_id = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_GYRO_SENS_TRIM:
+		mpu_chip_info->gyro_sens_trim = *((__u16 *)data->data);
+		break;
+	case MPU_SLAVE_ACCEL_SENS_TRIM:
+		mpu_chip_info->accel_sens_trim = *((__u16 *)data->data);
+		break;
+	case MPU_SLAVE_RAM:
+		if (data->len != mldl_cfg->mpu_ram->length)
+			return INV_ERROR_INVALID_PARAMETER;
+
+		memcpy(mldl_cfg->mpu_ram->ram, data->data, data->len);
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+	mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_RESET;
+	return INV_SUCCESS;
+}
+
+int gyro_get_config(void *mlsl_handle,
+		struct mldl_cfg *mldl_cfg,
+		struct ext_slave_config *data)
+{
+	struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
+	struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
+	struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
+	int ii;
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_INT_CONFIG:
+		*((__u8 *)data->data) = mpu_gyro_cfg->int_config;
+		break;
+	case MPU_SLAVE_EXT_SYNC:
+		*((__u8 *)data->data) = mpu_gyro_cfg->ext_sync;
+		break;
+	case MPU_SLAVE_FULL_SCALE:
+		*((__u8 *)data->data) = mpu_gyro_cfg->full_scale;
+		break;
+	case MPU_SLAVE_LPF:
+		*((__u8 *)data->data) = mpu_gyro_cfg->lpf;
+		break;
+	case MPU_SLAVE_CLK_SRC:
+		*((__u8 *)data->data) = mpu_gyro_cfg->clk_src;
+		break;
+	case MPU_SLAVE_DIVIDER:
+		*((__u8 *)data->data) = mpu_gyro_cfg->divider;
+		break;
+	case MPU_SLAVE_DMP_ENABLE:
+		*((__u8 *)data->data) = mpu_gyro_cfg->dmp_enable;
+		break;
+	case MPU_SLAVE_FIFO_ENABLE:
+		*((__u8 *)data->data) = mpu_gyro_cfg->fifo_enable;
+		break;
+	case MPU_SLAVE_DMP_CFG1:
+		*((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg1;
+		break;
+	case MPU_SLAVE_DMP_CFG2:
+		*((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg2;
+		break;
+	case MPU_SLAVE_TC:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			((__u8 *)data->data)[ii] = mpu_offsets->tc[ii];
+		break;
+	case MPU_SLAVE_GYRO:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			((__u16 *)data->data)[ii] = mpu_offsets->gyro[ii];
+		break;
+	case MPU_SLAVE_ADDR:
+		*((__u8 *)data->data) = mpu_chip_info->addr;
+		break;
+	case MPU_SLAVE_PRODUCT_REVISION:
+		*((__u8 *)data->data) = mpu_chip_info->product_revision;
+		break;
+	case MPU_SLAVE_SILICON_REVISION:
+		*((__u8 *)data->data) = mpu_chip_info->silicon_revision;
+		break;
+	case MPU_SLAVE_PRODUCT_ID:
+		*((__u8 *)data->data) = mpu_chip_info->product_id;
+		break;
+	case MPU_SLAVE_GYRO_SENS_TRIM:
+		*((__u16 *)data->data) = mpu_chip_info->gyro_sens_trim;
+		break;
+	case MPU_SLAVE_ACCEL_SENS_TRIM:
+		*((__u16 *)data->data) = mpu_chip_info->accel_sens_trim;
+		break;
+	case MPU_SLAVE_RAM:
+		if (data->len != mldl_cfg->mpu_ram->length)
+			return INV_ERROR_INVALID_PARAMETER;
+
+		memcpy(data->data, mldl_cfg->mpu_ram->ram, data->len);
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+
+/*******************************************************************************
+ *******************************************************************************
+ * Exported functions
+ *******************************************************************************
+ ******************************************************************************/
+
+/**
+ * Initializes the pdata structure to defaults.
+ *
+ * Opens the device to read silicon revision, product id and whoami.
+ *
+ * @mldl_cfg
+ *          The internal device configuration data structure.
+ * @mlsl_handle
+ *          The serial communication handle.
+ *
+ * @return INV_SUCCESS if silicon revision, product id and woami are supported
+ *         by this software.
+ */
+int inv_mpu_open(struct mldl_cfg *mldl_cfg,
+		 void *mlsl_handle,
+		 void *accel_handle,
+		 void *compass_handle, void *pressure_handle)
+{
+	int result;
+	int ii;
+
+	/* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
+	mldl_cfg->inv_mpu_cfg->ignore_system_suspend = false;
+	mldl_cfg->mpu_gyro_cfg->int_config = BIT_DMP_INT_EN;
+	mldl_cfg->mpu_gyro_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ;
+	mldl_cfg->mpu_gyro_cfg->lpf = MPU_FILTER_42HZ;
+	mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_2000DPS;
+	mldl_cfg->mpu_gyro_cfg->divider = 4;
+	mldl_cfg->mpu_gyro_cfg->dmp_enable = 1;
+	mldl_cfg->mpu_gyro_cfg->fifo_enable = 1;
+	mldl_cfg->mpu_gyro_cfg->ext_sync = 0;
+	mldl_cfg->mpu_gyro_cfg->dmp_cfg1 = 0;
+	mldl_cfg->mpu_gyro_cfg->dmp_cfg2 = 0;
+	mldl_cfg->inv_mpu_state->status =
+		MPU_DMP_IS_SUSPENDED |
+		MPU_GYRO_IS_SUSPENDED |
+		MPU_ACCEL_IS_SUSPENDED |
+		MPU_COMPASS_IS_SUSPENDED |
+		MPU_PRESSURE_IS_SUSPENDED;
+	mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
+
+	if (mldl_cfg->mpu_chip_info->addr == 0) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	/*
+	 * Reset,
+	 * Take the DMP out of sleep, and
+	 * read the product_id, sillicon rev and whoami
+	 */
+	mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED;
+	result = mpu3050_pwr_mgmt(mldl_cfg, mlsl_handle, RESET, 0, 0, 0, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_get_silicon_rev(mldl_cfg, mlsl_handle);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Get the factory temperature compensation offsets */
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_XG_OFFS_TC, 1,
+				 &mldl_cfg->mpu_offsets->tc[0]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_YG_OFFS_TC, 1,
+				 &mldl_cfg->mpu_offsets->tc[1]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_ZG_OFFS_TC, 1,
+				 &mldl_cfg->mpu_offsets->tc[2]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Into bypass mode before sleeping and calling the slaves init */
+	result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, true);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = mpu3050_pwr_mgmt(mldl_cfg, mlsl_handle, 0, SLEEP, 0, 0, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+
+		if (!mldl_cfg->slave[ii] || !mldl_cfg->slave[ii]->init)
+			continue;
+
+		result = mldl_cfg->slave[ii]->init(accel_handle,
+						mldl_cfg->slave[ii],
+						mldl_cfg->pdata_slave[ii]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			goto out_slave_failure;
+		}
+
+	}
+
+
+	return result;
+
+out_slave_failure:
+	for (ii = ii - 1; ii >= 0; ii--)
+		if (mldl_cfg->slave[ii] && mldl_cfg->slave[ii]->exit)
+			mldl_cfg->slave[ii]->exit(compass_handle,
+						mldl_cfg->slave[ii],
+						mldl_cfg->pdata_slave[ii]);
+	return result;
+}
+
+/**
+ * Close the mpu interface
+ *
+ * @mldl_cfg pointer to the configuration structure
+ * @mlsl_handle pointer to the serial layer handle
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+int inv_mpu_close(struct mldl_cfg *mldl_cfg,
+		  void *mlsl_handle,
+		  void *accel_handle,
+		  void *compass_handle,
+		  void *pressure_handle)
+{
+	int result = INV_SUCCESS;
+	int ret_result = INV_SUCCESS;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!mldl_cfg->slave[ii] || !mldl_cfg->slave[ii]->exit)
+			continue;
+		result = mldl_cfg->slave[ii]->exit(accel_handle,
+						mldl_cfg->slave[ii],
+						mldl_cfg->pdata_slave[ii]);
+		if (INV_SUCCESS != result)
+			MPL_LOGE("Slave %d exit failed %d\n", ii, result);
+
+		if (INV_SUCCESS == ret_result)
+			ret_result = result;
+	}
+
+	return ret_result;
+}
+
+/**
+ *  @brief  resume the MPU device and all the other sensor
+ *          devices from their low power state.
+ *
+ *  @mldl_cfg
+ *              pointer to the configuration structure
+ *  @gyro_handle
+ *              the main file handle to the MPU device.
+ *  @accel_handle
+ *              an handle to the accelerometer device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the accelerometer device operates on the same
+ *              primary bus of MPU.
+ *  @compass_handle
+ *              an handle to the compass device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the compass device operates on the same
+ *              primary bus of MPU.
+ *  @pressure_handle
+ *              an handle to the pressure sensor device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the pressure sensor device operates on the same
+ *              primary bus of MPU.
+ *  @resume_gyro
+ *              whether resuming the gyroscope device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @resume_accel
+ *              whether resuming the accelerometer device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @resume_compass
+ *              whether resuming the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @resume_pressure
+ *              whether resuming the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  INV_SUCCESS or a non-zero error code.
+ */
+int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
+		   void *gyro_handle,
+		   void *accel_handle,
+		   void *compass_handle,
+		   void *pressure_handle,
+		   unsigned long sensors)
+{
+	int result = INV_SUCCESS;
+	int ii;
+	bool resume_slave[EXT_SLAVE_NUM_TYPES];
+	bool resume_dmp = sensors & INV_DMP_PROCESSOR;
+	void *slave_handle[EXT_SLAVE_NUM_TYPES];
+	resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
+		(sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
+	resume_slave[EXT_SLAVE_TYPE_ACCEL] =
+		sensors & INV_THREE_AXIS_ACCEL;
+	resume_slave[EXT_SLAVE_TYPE_COMPASS] =
+		sensors & INV_THREE_AXIS_COMPASS;
+	resume_slave[EXT_SLAVE_TYPE_PRESSURE] =
+		sensors & INV_THREE_AXIS_PRESSURE;
+
+	slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
+	slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
+	slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
+	slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
+
+
+	mldl_print_cfg(mldl_cfg);
+
+	/* Skip the Gyro since slave[EXT_SLAVE_TYPE_GYROSCOPE] is NULL */
+	for (ii = EXT_SLAVE_TYPE_ACCEL; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (resume_slave[ii] &&
+		    ((!mldl_cfg->slave[ii]) ||
+			(!mldl_cfg->slave[ii]->resume))) {
+			LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+			return INV_ERROR_INVALID_PARAMETER;
+		}
+	}
+
+	if ((resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] || resume_dmp)
+		&& (mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED)) {
+		result = gyro_resume(mldl_cfg, gyro_handle, sensors);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!mldl_cfg->slave[ii] ||
+		    !mldl_cfg->pdata_slave[ii] ||
+		    !resume_slave[ii] ||
+		    !(mldl_cfg->inv_mpu_state->status & (1 << ii)))
+			continue;
+
+		if (EXT_SLAVE_BUS_SECONDARY ==
+		    mldl_cfg->pdata_slave[ii]->bus) {
+			result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle,
+						    true);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		result = mldl_cfg->slave[ii]->resume(slave_handle[ii],
+						mldl_cfg->slave[ii],
+						mldl_cfg->pdata_slave[ii]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		mldl_cfg->inv_mpu_state->status &= ~(1 << ii);
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (resume_dmp &&
+		    !(mldl_cfg->inv_mpu_state->status & (1 << ii)) &&
+		    mldl_cfg->pdata_slave[ii] &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata_slave[ii]->bus) {
+			result = mpu_set_slave(mldl_cfg,
+					gyro_handle,
+					mldl_cfg->slave[ii],
+					mldl_cfg->pdata_slave[ii],
+					mldl_cfg->slave[ii]->type);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+	}
+
+	/* Turn on the master i2c iterface if necessary */
+	if (resume_dmp) {
+		result = mpu_set_i2c_bypass(
+			mldl_cfg, gyro_handle,
+			!(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		/* Now start */
+		result = dmp_start(mldl_cfg, gyro_handle);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	mldl_cfg->inv_mpu_cfg->requested_sensors = sensors;
+
+	return result;
+}
+
+/**
+ *  @brief  suspend the MPU device and all the other sensor
+ *          devices into their low power state.
+ *  @mldl_cfg
+ *              a pointer to the struct mldl_cfg internal data
+ *              structure.
+ *  @gyro_handle
+ *              the main file handle to the MPU device.
+ *  @accel_handle
+ *              an handle to the accelerometer device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the accelerometer device operates on the same
+ *              primary bus of MPU.
+ *  @compass_handle
+ *              an handle to the compass device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the compass device operates on the same
+ *              primary bus of MPU.
+ *  @pressure_handle
+ *              an handle to the pressure sensor device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the pressure sensor device operates on the same
+ *              primary bus of MPU.
+ *  @accel
+ *              whether suspending the accelerometer device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @compass
+ *              whether suspending the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @pressure
+ *              whether suspending the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  INV_SUCCESS or a non-zero error code.
+ */
+int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
+		    void *gyro_handle,
+		    void *accel_handle,
+		    void *compass_handle,
+		    void *pressure_handle,
+		    unsigned long sensors)
+{
+	int result = INV_SUCCESS;
+	int ii;
+	struct ext_slave_descr **slave = mldl_cfg->slave;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	bool suspend_dmp = ((sensors & INV_DMP_PROCESSOR) == INV_DMP_PROCESSOR);
+	bool suspend_slave[EXT_SLAVE_NUM_TYPES];
+
+	suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
+		((sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO))
+			== (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
+	suspend_slave[EXT_SLAVE_TYPE_ACCEL] =
+		((sensors & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL);
+	suspend_slave[EXT_SLAVE_TYPE_COMPASS] =
+		((sensors & INV_THREE_AXIS_COMPASS) == INV_THREE_AXIS_COMPASS);
+	suspend_slave[EXT_SLAVE_TYPE_PRESSURE] =
+		((sensors & INV_THREE_AXIS_PRESSURE) ==
+			INV_THREE_AXIS_PRESSURE);
+
+	if (suspend_dmp) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = dmp_stop(mldl_cfg, gyro_handle);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	/* Gyro */
+	if (suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] &&
+	    !(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED)) {
+		result = mpu3050_pwr_mgmt(
+			mldl_cfg, gyro_handle, 0,
+			suspend_dmp && suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE],
+			(unsigned char)(sensors & INV_X_GYRO),
+			(unsigned char)(sensors & INV_Y_GYRO),
+			(unsigned char)(sensors & INV_Z_GYRO));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		bool is_suspended = mldl_cfg->inv_mpu_state->status & (1 << ii);
+		if (!slave[ii]   || !pdata_slave[ii] ||
+		    is_suspended || !suspend_slave[ii])
+			continue;
+
+		if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
+			result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		result = slave[ii]->suspend(accel_handle,
+						  slave[ii],
+						  pdata_slave[ii]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
+			result = mpu_set_slave(mldl_cfg, gyro_handle,
+					       NULL, NULL,
+					       slave[ii]->type);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		mldl_cfg->inv_mpu_state->status |= (1 << ii);
+	}
+
+	/* Re-enable the i2c master if there are configured slaves and DMP */
+	if (!suspend_dmp) {
+		result = mpu_set_i2c_bypass(
+			mldl_cfg, gyro_handle,
+			!(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	mldl_cfg->inv_mpu_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
+
+	return result;
+}
+
+int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
+		       void *gyro_handle,
+		       void *slave_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result;
+	int bypass_result;
+	int remain_bypassed = true;
+
+	if (NULL == slave || NULL == slave->read) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+
+	if ((EXT_SLAVE_BUS_SECONDARY == pdata->bus)
+	    && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
+		remain_bypassed = false;
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = slave->read(slave_handle, slave, pdata, data);
+
+	if (!remain_bypassed) {
+		bypass_result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
+		if (bypass_result) {
+			LOG_RESULT_LOCATION(bypass_result);
+			return bypass_result;
+		}
+	}
+	return result;
+}
+
+int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
+			 void *gyro_handle,
+			 void *slave_handle,
+			 struct ext_slave_config *data,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	int remain_bypassed = true;
+
+	if (NULL == slave || NULL == slave->config) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+
+	if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
+	    && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
+		remain_bypassed = false;
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = slave->config(slave_handle, slave, pdata, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (!remain_bypassed) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
+			     void *gyro_handle,
+			     void *slave_handle,
+			     struct ext_slave_config *data,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata)
+{
+	int result;
+	int remain_bypassed = true;
+
+	if (NULL == slave || NULL == slave->get_config) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+
+	if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
+	    && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
+		remain_bypassed = false;
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = slave->get_config(slave_handle, slave, pdata, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (!remain_bypassed) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+/**
+ * @}
+ */
