108 files changed, 39787 insertions, 0 deletions

diff --git a/drivers/misc/ab8500-pwm.c b/drivers/misc/ab8500-pwm.c
new file mode 100644
index 00000000000..35903154ca2
--- /dev/null
+++ b/drivers/misc/ab8500-pwm.c
@@ -0,0 +1,168 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Arun R Murthy <arun.murthy@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/pwm.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+
+/*
+ * PWM Out generators
+ * Bank: 0x10
+ */
+#define AB8500_PWM_OUT_CTRL1_REG        0x60
+#define AB8500_PWM_OUT_CTRL2_REG        0x61
+#define AB8500_PWM_OUT_CTRL7_REG        0x66
+
+/* backlight driver constants */
+#define ENABLE_PWM                      1
+#define DISABLE_PWM                     0
+
+struct pwm_device {
+        struct device *dev;
+        struct list_head node;
+        const char *label;
+        unsigned int pwm_id;
+};
+
+static LIST_HEAD(pwm_list);
+
+int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
+{
+        int ret = 0;
+        unsigned int higher_val, lower_val;
+        u8 reg;
+
+        /*
+         * get the first 8 bits that are be written to
+         * AB8500_PWM_OUT_CTRL1_REG[0:7]
+         */
+        lower_val = duty_ns & 0x00FF;
+        /*
+         * get bits [9:10] that are to be written to
+         * AB8500_PWM_OUT_CTRL2_REG[0:1]
+         */
+        higher_val = ((duty_ns & 0x0300) >> 8);
+
+        reg = AB8500_PWM_OUT_CTRL1_REG + ((pwm->pwm_id - 1) * 2);
+
+        ret = abx500_set_register_interruptible(pwm->dev, AB8500_MISC,
+                        reg, (u8)lower_val);
+        if (ret < 0)
+                return ret;
+        ret = abx500_set_register_interruptible(pwm->dev, AB8500_MISC,
+                        (reg + 1), (u8)higher_val);
+
+        return ret;
+}
+EXPORT_SYMBOL(pwm_config);
+
+int pwm_enable(struct pwm_device *pwm)
+{
+        int ret;
+
+        ret = abx500_mask_and_set_register_interruptible(pwm->dev,
+                                AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
+                                1 << (pwm->pwm_id-1), ENABLE_PWM);
+        if (ret < 0)
+                dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
+                                                        pwm->label, ret);
+        return ret;
+}
+EXPORT_SYMBOL(pwm_enable);
+
+void pwm_disable(struct pwm_device *pwm)
+{
+        int ret;
+
+        ret = abx500_mask_and_set_register_interruptible(pwm->dev,
+                                AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
+                                1 << (pwm->pwm_id-1), DISABLE_PWM);
+        if (ret < 0)
+                dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
+                                                        pwm->label, ret);
+        return;
+}
+EXPORT_SYMBOL(pwm_disable);
+
+struct pwm_device *pwm_request(int pwm_id, const char *label)
+{
+        struct pwm_device *pwm;
+
+        list_for_each_entry(pwm, &pwm_list, node) {
+                if (pwm->pwm_id == pwm_id) {
+                        pwm->label = label;
+                        pwm->pwm_id = pwm_id;
+                        return pwm;
+                }
+        }
+
+        return ERR_PTR(-ENOENT);
+}
+EXPORT_SYMBOL(pwm_request);
+
+void pwm_free(struct pwm_device *pwm)
+{
+        pwm_disable(pwm);
+}
+EXPORT_SYMBOL(pwm_free);
+
+static int __devinit ab8500_pwm_probe(struct platform_device *pdev)
+{
+        struct pwm_device *pwm;
+        /*
+         * Nothing to be done in probe, this is required to get the
+         * device which is required for ab8500 read and write
+         */
+        pwm = kzalloc(sizeof(struct pwm_device), GFP_KERNEL);
+        if (pwm == NULL) {
+                dev_err(&pdev->dev, "failed to allocate memory\n");
+                return -ENOMEM;
+        }
+        pwm->dev = &pdev->dev;
+        pwm->pwm_id = pdev->id;
+        list_add_tail(&pwm->node, &pwm_list);
+        platform_set_drvdata(pdev, pwm);
+        dev_dbg(pwm->dev, "pwm probe successful\n");
+        return 0;
+}
+
+static int __devexit ab8500_pwm_remove(struct platform_device *pdev)
+{
+        struct pwm_device *pwm = platform_get_drvdata(pdev);
+        list_del(&pwm->node);
+        dev_dbg(&pdev->dev, "pwm driver removed\n");
+        kfree(pwm);
+        return 0;
+}
+
+static struct platform_driver ab8500_pwm_driver = {
+        .driver = {
+                .name = "ab8500-pwm",
+                .owner = THIS_MODULE,
+        },
+        .probe = ab8500_pwm_probe,
+        .remove = __devexit_p(ab8500_pwm_remove),
+};
+
+static int __init ab8500_pwm_init(void)
+{
+        return platform_driver_register(&ab8500_pwm_driver);
+}
+
+static void __exit ab8500_pwm_exit(void)
+{
+        platform_driver_unregister(&ab8500_pwm_driver);
+}
+
+subsys_initcall(ab8500_pwm_init);
+module_exit(ab8500_pwm_exit);
+MODULE_AUTHOR("Arun MURTHY <arun.murthy@stericsson.com>");
+MODULE_DESCRIPTION("AB8500 Pulse Width Modulation Driver");
+MODULE_ALIAS("platform:ab8500-pwm");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/misc/akm8975.c b/drivers/misc/akm8975.c
new file mode 100644
index 00000000000..aef7985d4ce
--- /dev/null
+++ b/drivers/misc/akm8975.c
@@ -0,0 +1,732 @@
+/* drivers/misc/akm8975.c - akm8975 compass driver
+ *
+ * Copyright (C) 2007-2008 HTC Corporation.
+ * Author: Hou-Kun Chen <houkun.chen@gmail.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*
+ * Revised by AKM 2009/04/02
+ * Revised by Motorola 2010/05/27
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <linux/gpio.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/freezer.h>
+#include <linux/akm8975.h>
+#include <linux/earlysuspend.h>
+
+#define AK8975DRV_CALL_DBG 0
+#if AK8975DRV_CALL_DBG
+#define FUNCDBG(msg)    pr_err("%s:%s\n", __func__, msg);
+#else
+#define FUNCDBG(msg)
+#endif
+
+#define AK8975DRV_DATA_DBG 0
+#define MAX_FAILURE_COUNT 10
+
+struct akm8975_data {
+        struct i2c_client *this_client;
+        struct akm8975_platform_data *pdata;
+        struct input_dev *input_dev;
+        struct work_struct work;
+        struct mutex flags_lock;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        struct early_suspend early_suspend;
+#endif
+};
+
+/*
+* Because misc devices can not carry a pointer from driver register to
+* open, we keep this global. This limits the driver to a single instance.
+*/
+struct akm8975_data *akmd_data;
+
+static DECLARE_WAIT_QUEUE_HEAD(open_wq);
+
+static atomic_t open_flag;
+
+static short m_flag;
+static short a_flag;
+static short t_flag;
+static short mv_flag;
+
+static short akmd_delay;
+
+static ssize_t akm8975_show(struct device *dev, struct device_attribute *attr,
+                                 char *buf)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        return sprintf(buf, "%u\n", i2c_smbus_read_byte_data(client,
+                                                             AK8975_REG_CNTL));
+}
+static ssize_t akm8975_store(struct device *dev, struct device_attribute *attr,
+                            const char *buf, size_t count)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        unsigned long val;
+        strict_strtoul(buf, 10, &val);
+        if (val > 0xff)
+                return -EINVAL;
+        i2c_smbus_write_byte_data(client, AK8975_REG_CNTL, val);
+        return count;
+}
+static DEVICE_ATTR(akm_ms1, S_IWUSR | S_IRUGO, akm8975_show, akm8975_store);
+
+static int akm8975_i2c_rxdata(struct akm8975_data *akm, char *buf, int length)
+{
+        struct i2c_msg msgs[] = {
+                {
+                        .addr = akm->this_client->addr,
+                        .flags = 0,
+                        .len = 1,
+                        .buf = buf,
+                },
+                {
+                        .addr = akm->this_client->addr,
+                        .flags = I2C_M_RD,
+                        .len = length,
+                        .buf = buf,
+                },
+        };
+
+        FUNCDBG("called");
+
+        if (i2c_transfer(akm->this_client->adapter, msgs, 2) < 0) {
+                pr_err("akm8975_i2c_rxdata: transfer error\n");
+                return EIO;
+        } else
+                return 0;
+}
+
+static int akm8975_i2c_txdata(struct akm8975_data *akm, char *buf, int length)
+{
+        struct i2c_msg msgs[] = {
+                {
+                        .addr = akm->this_client->addr,
+                        .flags = 0,
+                        .len = length,
+                        .buf = buf,
+                },
+        };
+
+        FUNCDBG("called");
+
+        if (i2c_transfer(akm->this_client->adapter, msgs, 1) < 0) {
+                pr_err("akm8975_i2c_txdata: transfer error\n");
+                return -EIO;
+        } else
+                return 0;
+}
+
+static void akm8975_ecs_report_value(struct akm8975_data *akm, short *rbuf)
+{
+        struct akm8975_data *data = i2c_get_clientdata(akm->this_client);
+
+        FUNCDBG("called");
+
+#if AK8975DRV_DATA_DBG
+        pr_info("akm8975_ecs_report_value: yaw = %d, pitch = %d, roll = %d\n",
+                                 rbuf[0], rbuf[1], rbuf[2]);
+        pr_info("tmp = %d, m_stat= %d, g_stat=%d\n", rbuf[3], rbuf[4], rbuf[5]);
+        pr_info("Acceleration:   x = %d LSB, y = %d LSB, z = %d LSB\n",
+                                 rbuf[6], rbuf[7], rbuf[8]);
+        pr_info("Magnetic:       x = %d LSB, y = %d LSB, z = %d LSB\n\n",
+                                 rbuf[9], rbuf[10], rbuf[11]);
+#endif
+        mutex_lock(&akm->flags_lock);
+        /* Report magnetic sensor information */
+        if (m_flag) {
+                input_report_abs(data->input_dev, ABS_RX, rbuf[0]);
+                input_report_abs(data->input_dev, ABS_RY, rbuf[1]);
+                input_report_abs(data->input_dev, ABS_RZ, rbuf[2]);
+                input_report_abs(data->input_dev, ABS_RUDDER, rbuf[4]);
+        }
+
+        /* Report acceleration sensor information */
+        if (a_flag) {
+                input_report_abs(data->input_dev, ABS_X, rbuf[6]);
+                input_report_abs(data->input_dev, ABS_Y, rbuf[7]);
+                input_report_abs(data->input_dev, ABS_Z, rbuf[8]);
+                input_report_abs(data->input_dev, ABS_WHEEL, rbuf[5]);
+        }
+
+        /* Report temperature information */
+        if (t_flag)
+                input_report_abs(data->input_dev, ABS_THROTTLE, rbuf[3]);
+
+        if (mv_flag) {
+                input_report_abs(data->input_dev, ABS_HAT0X, rbuf[9]);
+                input_report_abs(data->input_dev, ABS_HAT0Y, rbuf[10]);
+                input_report_abs(data->input_dev, ABS_BRAKE, rbuf[11]);
+        }
+        mutex_unlock(&akm->flags_lock);
+
+        input_sync(data->input_dev);
+}
+
+static void akm8975_ecs_close_done(struct akm8975_data *akm)
+{
+        FUNCDBG("called");
+        mutex_lock(&akm->flags_lock);
+        m_flag = 1;
+        a_flag = 1;
+        t_flag = 1;
+        mv_flag = 1;
+        mutex_unlock(&akm->flags_lock);
+}
+
+static int akm_aot_open(struct inode *inode, struct file *file)
+{
+        int ret = -1;
+
+        FUNCDBG("called");
+        if (atomic_cmpxchg(&open_flag, 0, 1) == 0) {
+                wake_up(&open_wq);
+                ret = 0;
+        }
+
+        ret = nonseekable_open(inode, file);
+        if (ret)
+                return ret;
+
+        file->private_data = akmd_data;
+
+        return ret;
+}
+
+static int akm_aot_release(struct inode *inode, struct file *file)
+{
+        FUNCDBG("called");
+        atomic_set(&open_flag, 0);
+        wake_up(&open_wq);
+        return 0;
+}
+
+static int akm_aot_ioctl(struct inode *inode, struct file *file,
+              unsigned int cmd, unsigned long arg)
+{
+        void __user *argp = (void __user *) arg;
+        short flag;
+        struct akm8975_data *akm = file->private_data;
+
+        FUNCDBG("called");
+
+        switch (cmd) {
+        case ECS_IOCTL_APP_SET_MFLAG:
+        case ECS_IOCTL_APP_SET_AFLAG:
+        case ECS_IOCTL_APP_SET_MVFLAG:
+                if (copy_from_user(&flag, argp, sizeof(flag)))
+                        return -EFAULT;
+                if (flag < 0 || flag > 1)
+                        return -EINVAL;
+                break;
+        case ECS_IOCTL_APP_SET_DELAY:
+                if (copy_from_user(&flag, argp, sizeof(flag)))
+                        return -EFAULT;
+                break;
+        default:
+                break;
+        }
+
+        mutex_lock(&akm->flags_lock);
+        switch (cmd) {
+        case ECS_IOCTL_APP_SET_MFLAG:
+          m_flag = flag;
+                break;
+        case ECS_IOCTL_APP_GET_MFLAG:
+                flag = m_flag;
+                break;
+        case ECS_IOCTL_APP_SET_AFLAG:
+                a_flag = flag;
+                break;
+        case ECS_IOCTL_APP_GET_AFLAG:
+                flag = a_flag;
+                break;
+        case ECS_IOCTL_APP_SET_MVFLAG:
+                mv_flag = flag;
+                break;
+        case ECS_IOCTL_APP_GET_MVFLAG:
+                flag = mv_flag;
+                break;
+        case ECS_IOCTL_APP_SET_DELAY:
+                akmd_delay = flag;
+                break;
+        case ECS_IOCTL_APP_GET_DELAY:
+                flag = akmd_delay;
+                break;
+        default:
+                return -ENOTTY;
+        }
+        mutex_unlock(&akm->flags_lock);
+
+        switch (cmd) {
+        case ECS_IOCTL_APP_GET_MFLAG:
+        case ECS_IOCTL_APP_GET_AFLAG:
+        case ECS_IOCTL_APP_GET_MVFLAG:
+        case ECS_IOCTL_APP_GET_DELAY:
+                if (copy_to_user(argp, &flag, sizeof(flag)))
+                        return -EFAULT;
+                break;
+        default:
+                break;
+        }
+
+        return 0;
+}
+
+static int akmd_open(struct inode *inode, struct file *file)
+{
+        int err = 0;
+
+        FUNCDBG("called");
+        err = nonseekable_open(inode, file);
+        if (err)
+                return err;
+
+        file->private_data = akmd_data;
+        return 0;
+}
+
+static int akmd_release(struct inode *inode, struct file *file)
+{
+        struct akm8975_data *akm = file->private_data;
+
+        FUNCDBG("called");
+        akm8975_ecs_close_done(akm);
+        return 0;
+}
+
+static int akmd_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+                      unsigned long arg)
+{
+        void __user *argp = (void __user *) arg;
+
+        char rwbuf[16];
+        int ret = -1;
+        int status;
+        short value[12];
+        short delay;
+        struct akm8975_data *akm = file->private_data;
+
+        FUNCDBG("called");
+
+        switch (cmd) {
+        case ECS_IOCTL_READ:
+        case ECS_IOCTL_WRITE:
+                if (copy_from_user(&rwbuf, argp, sizeof(rwbuf)))
+                        return -EFAULT;
+                break;
+
+        case ECS_IOCTL_SET_YPR:
+                if (copy_from_user(&value, argp, sizeof(value)))
+                        return -EFAULT;
+                break;
+
+        default:
+                break;
+        }
+
+        switch (cmd) {
+        case ECS_IOCTL_READ:
+                if (rwbuf[0] < 1)
+                        return -EINVAL;
+
+                ret = akm8975_i2c_rxdata(akm, &rwbuf[1], rwbuf[0]);
+                if (ret < 0)
+                        return ret;
+                break;
+
+        case ECS_IOCTL_WRITE:
+                if (rwbuf[0] < 2)
+                        return -EINVAL;
+
+                ret = akm8975_i2c_txdata(akm, &rwbuf[1], rwbuf[0]);
+                if (ret < 0)
+                        return ret;
+                break;
+        case ECS_IOCTL_SET_YPR:
+                akm8975_ecs_report_value(akm, value);
+                break;
+
+        case ECS_IOCTL_GET_OPEN_STATUS:
+                wait_event_interruptible(open_wq,
+                                         (atomic_read(&open_flag) != 0));
+                status = atomic_read(&open_flag);
+                break;
+        case ECS_IOCTL_GET_CLOSE_STATUS:
+                wait_event_interruptible(open_wq,
+                                         (atomic_read(&open_flag) == 0));
+                status = atomic_read(&open_flag);
+                break;
+
+        case ECS_IOCTL_GET_DELAY:
+                delay = akmd_delay;
+                break;
+
+        default:
+                FUNCDBG("Unknown cmd\n");
+                return -ENOTTY;
+        }
+
+        switch (cmd) {
+        case ECS_IOCTL_READ:
+                if (copy_to_user(argp, &rwbuf, sizeof(rwbuf)))
+                        return -EFAULT;
+                break;
+        case ECS_IOCTL_GET_OPEN_STATUS:
+        case ECS_IOCTL_GET_CLOSE_STATUS:
+                if (copy_to_user(argp, &status, sizeof(status)))
+                        return -EFAULT;
+                break;
+        case ECS_IOCTL_GET_DELAY:
+                if (copy_to_user(argp, &delay, sizeof(delay)))
+                        return -EFAULT;
+                break;
+        default:
+                break;
+        }
+
+        return 0;
+}
+
+/* needed to clear the int. pin */
+static void akm_work_func(struct work_struct *work)
+{
+        struct akm8975_data *akm =
+            container_of(work, struct akm8975_data, work);
+
+        FUNCDBG("called");
+        enable_irq(akm->this_client->irq);
+}
+
+static irqreturn_t akm8975_interrupt(int irq, void *dev_id)
+{
+        struct akm8975_data *akm = dev_id;
+        FUNCDBG("called");
+
+        disable_irq_nosync(akm->this_client->irq);
+        schedule_work(&akm->work);
+        return IRQ_HANDLED;
+}
+
+static int akm8975_power_off(struct akm8975_data *akm)
+{
+#if AK8975DRV_CALL_DBG
+        pr_info("%s\n", __func__);
+#endif
+        if (akm->pdata->power_off)
+                akm->pdata->power_off();
+
+        return 0;
+}
+
+static int akm8975_power_on(struct akm8975_data *akm)
+{
+        int err;
+
+#if AK8975DRV_CALL_DBG
+        pr_info("%s\n", __func__);
+#endif
+        if (akm->pdata->power_on) {
+                err = akm->pdata->power_on();
+                if (err < 0)
+                        return err;
+        }
+        return 0;
+}
+
+static int akm8975_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+        struct akm8975_data *akm = i2c_get_clientdata(client);
+
+#if AK8975DRV_CALL_DBG
+        pr_info("%s\n", __func__);
+#endif
+        /* TO DO: might need more work after power mgmt
+           is enabled */
+        return akm8975_power_off(akm);
+}
+
+static int akm8975_resume(struct i2c_client *client)
+{
+        struct akm8975_data *akm = i2c_get_clientdata(client);
+
+#if AK8975DRV_CALL_DBG
+        pr_info("%s\n", __func__);
+#endif
+        /* TO DO: might need more work after power mgmt
+           is enabled */
+        return akm8975_power_on(akm);
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void akm8975_early_suspend(struct early_suspend *handler)
+{
+        struct akm8975_data *akm;
+        akm = container_of(handler, struct akm8975_data, early_suspend);
+
+#if AK8975DRV_CALL_DBG
+        pr_info("%s\n", __func__);
+#endif
+        akm8975_suspend(akm->this_client, PMSG_SUSPEND);
+}
+
+static void akm8975_early_resume(struct early_suspend *handler)
+{
+        struct akm8975_data *akm;
+        akm = container_of(handler, struct akm8975_data, early_suspend);
+
+#if AK8975DRV_CALL_DBG
+        pr_info("%s\n", __func__);
+#endif
+        akm8975_resume(akm->this_client);
+}
+#endif
+
+
+static int akm8975_init_client(struct i2c_client *client)
+{
+        struct akm8975_data *data;
+        int ret;
+
+        data = i2c_get_clientdata(client);
+
+        ret = request_irq(client->irq, akm8975_interrupt, IRQF_TRIGGER_RISING,
+                                "akm8975", data);
+
+        if (ret < 0) {
+                pr_err("akm8975_init_client: request irq failed\n");
+                goto err;
+        }
+
+        init_waitqueue_head(&open_wq);
+
+        mutex_lock(&data->flags_lock);
+        m_flag = 1;
+        a_flag = 1;
+        t_flag = 1;
+        mv_flag = 1;
+        mutex_unlock(&data->flags_lock);
+
+        return 0;
+err:
+  return ret;
+}
+
+static const struct file_operations akmd_fops = {
+        .owner = THIS_MODULE,
+        .open = akmd_open,
+        .release = akmd_release,
+        .unlocked_ioctl = akmd_ioctl,
+};
+
+static const struct file_operations akm_aot_fops = {
+        .owner = THIS_MODULE,
+        .open = akm_aot_open,
+        .release = akm_aot_release,
+        .unlocked_ioctl = akm_aot_ioctl,
+};
+
+static struct miscdevice akm_aot_device = {
+        .minor = MISC_DYNAMIC_MINOR,
+        .name = "akm8975_aot",
+        .fops = &akm_aot_fops,
+};
+
+static struct miscdevice akmd_device = {
+        .minor = MISC_DYNAMIC_MINOR,
+        .name = "akm8975_dev",
+        .fops = &akmd_fops,
+};
+
+int akm8975_probe(struct i2c_client *client,
+                  const struct i2c_device_id *devid)
+{
+        struct akm8975_data *akm;
+        int err;
+        FUNCDBG("called");
+
+        if (client->dev.platform_data == NULL) {
+                dev_err(&client->dev, "platform data is NULL. exiting.\n");
+                err = -ENODEV;
+                goto exit_platform_data_null;
+        }
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                dev_err(&client->dev, "platform data is NULL. exiting.\n");
+                err = -ENODEV;
+                goto exit_check_functionality_failed;
+        }
+
+        akm = kzalloc(sizeof(struct akm8975_data), GFP_KERNEL);
+        if (!akm) {
+                dev_err(&client->dev,
+                        "failed to allocate memory for module data\n");
+                err = -ENOMEM;
+                goto exit_alloc_data_failed;
+        }
+
+        akm->pdata = client->dev.platform_data;
+
+        mutex_init(&akm->flags_lock);
+        INIT_WORK(&akm->work, akm_work_func);
+        i2c_set_clientdata(client, akm);
+
+        err = akm8975_power_on(akm);
+        if (err < 0)
+                goto exit_power_on_failed;
+
+        akm8975_init_client(client);
+        akm->this_client = client;
+        akmd_data = akm;
+
+        akm->input_dev = input_allocate_device();
+        if (!akm->input_dev) {
+                err = -ENOMEM;
+                dev_err(&akm->this_client->dev,
+                        "input device allocate failed\n");
+                goto exit_input_dev_alloc_failed;
+        }
+
+        set_bit(EV_ABS, akm->input_dev->evbit);
+
+        /* yaw */
+        input_set_abs_params(akm->input_dev, ABS_RX, 0, 23040, 0, 0);
+        /* pitch */
+        input_set_abs_params(akm->input_dev, ABS_RY, -11520, 11520, 0, 0);
+        /* roll */
+        input_set_abs_params(akm->input_dev, ABS_RZ, -5760, 5760, 0, 0);
+        /* x-axis acceleration */
+        input_set_abs_params(akm->input_dev, ABS_X, -5760, 5760, 0, 0);
+        /* y-axis acceleration */
+        input_set_abs_params(akm->input_dev, ABS_Y, -5760, 5760, 0, 0);
+        /* z-axis acceleration */
+        input_set_abs_params(akm->input_dev, ABS_Z, -5760, 5760, 0, 0);
+        /* temparature */
+        input_set_abs_params(akm->input_dev, ABS_THROTTLE, -30, 85, 0, 0);
+        /* status of magnetic sensor */
+        input_set_abs_params(akm->input_dev, ABS_RUDDER, 0, 3, 0, 0);
+        /* status of acceleration sensor */
+        input_set_abs_params(akm->input_dev, ABS_WHEEL, 0, 3, 0, 0);
+        /* x-axis of raw magnetic vector */
+        input_set_abs_params(akm->input_dev, ABS_HAT0X, -20480, 20479, 0, 0);
+        /* y-axis of raw magnetic vector */
+        input_set_abs_params(akm->input_dev, ABS_HAT0Y, -20480, 20479, 0, 0);
+        /* z-axis of raw magnetic vector */
+        input_set_abs_params(akm->input_dev, ABS_BRAKE, -20480, 20479, 0, 0);
+
+        akm->input_dev->name = "compass";
+
+        err = input_register_device(akm->input_dev);
+        if (err) {
+                pr_err("akm8975_probe: Unable to register input device: %s\n",
+                                         akm->input_dev->name);
+                goto exit_input_register_device_failed;
+        }
+
+        err = misc_register(&akmd_device);
+        if (err) {
+                pr_err("akm8975_probe: akmd_device register failed\n");
+                goto exit_misc_device_register_failed;
+        }
+
+        err = misc_register(&akm_aot_device);
+        if (err) {
+                pr_err("akm8975_probe: akm_aot_device register failed\n");
+                goto exit_misc_device_register_failed;
+        }
+
+        err = device_create_file(&client->dev, &dev_attr_akm_ms1);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        akm->early_suspend.suspend = akm8975_early_suspend;
+        akm->early_suspend.resume = akm8975_early_resume;
+        register_early_suspend(&akm->early_suspend);
+#endif
+        return 0;
+
+exit_misc_device_register_failed:
+exit_input_register_device_failed:
+        input_free_device(akm->input_dev);
+exit_input_dev_alloc_failed:
+        akm8975_power_off(akm);
+exit_power_on_failed:
+        kfree(akm);
+exit_alloc_data_failed:
+exit_check_functionality_failed:
+exit_platform_data_null:
+        return err;
+}
+
+static int __devexit akm8975_remove(struct i2c_client *client)
+{
+        struct akm8975_data *akm = i2c_get_clientdata(client);
+        FUNCDBG("called");
+        free_irq(client->irq, NULL);
+        input_unregister_device(akm->input_dev);
+        misc_deregister(&akmd_device);
+        misc_deregister(&akm_aot_device);
+        akm8975_power_off(akm);
+        kfree(akm);
+        return 0;
+}
+
+static const struct i2c_device_id akm8975_id[] = {
+        { "akm8975", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, akm8975_id);
+
+static struct i2c_driver akm8975_driver = {
+        .probe = akm8975_probe,
+        .remove = akm8975_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
+        .resume = akm8975_resume,
+        .suspend = akm8975_suspend,
+#endif
+        .id_table = akm8975_id,
+        .driver = {
+                .name = "akm8975",
+        },
+};
+
+static int __init akm8975_init(void)
+{
+        pr_info("AK8975 compass driver: init\n");
+        FUNCDBG("AK8975 compass driver: init\n");
+        return i2c_add_driver(&akm8975_driver);
+}
+
+static void __exit akm8975_exit(void)
+{
+        FUNCDBG("AK8975 compass driver: exit\n");
+        i2c_del_driver(&akm8975_driver);
+}
+
+module_init(akm8975_init);
+module_exit(akm8975_exit);
+
+MODULE_AUTHOR("Hou-Kun Chen <hk_chen@htc.com>");
+MODULE_DESCRIPTION("AK8975 compass driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/apanic.c b/drivers/misc/apanic.c
new file mode 100644
index 00000000000..ca875f89da7
--- /dev/null
+++ b/drivers/misc/apanic.c
@@ -0,0 +1,606 @@
+/* drivers/misc/apanic.c
+ *
+ * Copyright (C) 2009 Google, Inc.
+ * Author: San Mehat <san@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/wakelock.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/mtd/mtd.h>
+#include <linux/notifier.h>
+#include <linux/mtd/mtd.h>
+#include <linux/debugfs.h>
+#include <linux/fs.h>
+#include <linux/proc_fs.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/preempt.h>
+
+extern void ram_console_enable_console(int);
+
+struct panic_header {
+        u32 magic;
+#define PANIC_MAGIC 0xdeadf00d
+
+        u32 version;
+#define PHDR_VERSION   0x01
+
+        u32 console_offset;
+        u32 console_length;
+
+        u32 threads_offset;
+        u32 threads_length;
+};
+
+struct apanic_data {
+        struct mtd_info         *mtd;
+        struct panic_header     curr;
+        void                    *bounce;
+        struct proc_dir_entry   *apanic_console;
+        struct proc_dir_entry   *apanic_threads;
+};
+
+static struct apanic_data drv_ctx;
+static struct work_struct proc_removal_work;
+static DEFINE_MUTEX(drv_mutex);
+
+static unsigned int *apanic_bbt;
+static unsigned int apanic_erase_blocks;
+static unsigned int apanic_good_blocks;
+
+static void set_bb(unsigned int block, unsigned int *bbt)
+{
+        unsigned int flag = 1;
+
+        BUG_ON(block >= apanic_erase_blocks);
+
+        flag = flag << (block%32);
+        apanic_bbt[block/32] |= flag;
+        apanic_good_blocks--;
+}
+
+static unsigned int get_bb(unsigned int block, unsigned int *bbt)
+{
+        unsigned int flag;
+
+        BUG_ON(block >= apanic_erase_blocks);
+
+        flag = 1 << (block%32);
+        return apanic_bbt[block/32] & flag;
+}
+
+static void alloc_bbt(struct mtd_info *mtd, unsigned int *bbt)
+{
+        int bbt_size;
+        apanic_erase_blocks = (mtd->size)>>(mtd->erasesize_shift);
+        bbt_size = (apanic_erase_blocks+32)/32;
+
+        apanic_bbt = kmalloc(bbt_size*4, GFP_KERNEL);
+        memset(apanic_bbt, 0, bbt_size*4);
+        apanic_good_blocks = apanic_erase_blocks;
+}
+static void scan_bbt(struct mtd_info *mtd, unsigned int *bbt)
+{
+        int i;
+
+        for (i = 0; i < apanic_erase_blocks; i++) {
+                if (mtd->block_isbad(mtd, i*mtd->erasesize))
+                        set_bb(i, apanic_bbt);
+        }
+}
+
+#define APANIC_INVALID_OFFSET 0xFFFFFFFF
+
+static unsigned int phy_offset(struct mtd_info *mtd, unsigned int offset)
+{
+        unsigned int logic_block = offset>>(mtd->erasesize_shift);
+        unsigned int phy_block;
+        unsigned good_block = 0;
+
+        for (phy_block = 0; phy_block < apanic_erase_blocks; phy_block++) {
+                if (!get_bb(phy_block, apanic_bbt))
+                        good_block++;
+                if (good_block == (logic_block + 1))
+                        break;
+        }
+
+        if (good_block != (logic_block + 1))
+                return APANIC_INVALID_OFFSET;
+
+        return offset + ((phy_block-logic_block)<<mtd->erasesize_shift);
+}
+
+static void apanic_erase_callback(struct erase_info *done)
+{
+        wait_queue_head_t *wait_q = (wait_queue_head_t *) done->priv;
+        wake_up(wait_q);
+}
+
+static int apanic_proc_read(char *buffer, char **start, off_t offset,
+                               int count, int *peof, void *dat)
+{
+        struct apanic_data *ctx = &drv_ctx;
+        size_t file_length;
+        off_t file_offset;
+        unsigned int page_no;
+        off_t page_offset;
+        int rc;
+        size_t len;
+
+        if (!count)
+                return 0;
+
+        mutex_lock(&drv_mutex);
+
+        switch ((int) dat) {
+        case 1: /* apanic_console */
+                file_length = ctx->curr.console_length;
+                file_offset = ctx->curr.console_offset;
+                break;
+        case 2: /* apanic_threads */
+                file_length = ctx->curr.threads_length;
+                file_offset = ctx->curr.threads_offset;
+                break;
+        default:
+                pr_err("Bad dat (%d)\n", (int) dat);
+                mutex_unlock(&drv_mutex);
+                return -EINVAL;
+        }
+
+        if ((offset + count) > file_length) {
+                mutex_unlock(&drv_mutex);
+                return 0;
+        }
+
+        /* We only support reading a maximum of a flash page */
+        if (count > ctx->mtd->writesize)
+                count = ctx->mtd->writesize;
+
+        page_no = (file_offset + offset) / ctx->mtd->writesize;
+        page_offset = (file_offset + offset) % ctx->mtd->writesize;
+
+
+        if (phy_offset(ctx->mtd, (page_no * ctx->mtd->writesize))
+                == APANIC_INVALID_OFFSET) {
+                pr_err("apanic: reading an invalid address\n");
+                mutex_unlock(&drv_mutex);
+                return -EINVAL;
+        }
+        rc = ctx->mtd->read(ctx->mtd,
+                phy_offset(ctx->mtd, (page_no * ctx->mtd->writesize)),
+                ctx->mtd->writesize,
+                &len, ctx->bounce);
+
+        if (page_offset)
+                count -= page_offset;
+        memcpy(buffer, ctx->bounce + page_offset, count);
+
+        *start = count;
+
+        if ((offset + count) == file_length)
+                *peof = 1;
+
+        mutex_unlock(&drv_mutex);
+        return count;
+}
+
+static void mtd_panic_erase(void)
+{
+        struct apanic_data *ctx = &drv_ctx;
+        struct erase_info erase;
+        DECLARE_WAITQUEUE(wait, current);
+        wait_queue_head_t wait_q;
+        int rc, i;
+
+        init_waitqueue_head(&wait_q);
+        erase.mtd = ctx->mtd;
+        erase.callback = apanic_erase_callback;
+        erase.len = ctx->mtd->erasesize;
+        erase.priv = (u_long)&wait_q;
+        for (i = 0; i < ctx->mtd->size; i += ctx->mtd->erasesize) {
+                erase.addr = i;
+                set_current_state(TASK_INTERRUPTIBLE);
+                add_wait_queue(&wait_q, &wait);
+
+                if (get_bb(erase.addr>>ctx->mtd->erasesize_shift, apanic_bbt)) {
+                        printk(KERN_WARNING
+                               "apanic: Skipping erase of bad "
+                               "block @%llx\n", erase.addr);
+                        set_current_state(TASK_RUNNING);
+                        remove_wait_queue(&wait_q, &wait);
+                        continue;
+                }
+
+                rc = ctx->mtd->erase(ctx->mtd, &erase);
+                if (rc) {
+                        set_current_state(TASK_RUNNING);
+                        remove_wait_queue(&wait_q, &wait);
+                        printk(KERN_ERR
+                               "apanic: Erase of 0x%llx, 0x%llx failed\n",
+                               (unsigned long long) erase.addr,
+                               (unsigned long long) erase.len);
+                        if (rc == -EIO) {
+                                if (ctx->mtd->block_markbad(ctx->mtd,
+                                                            erase.addr)) {
+                                        printk(KERN_ERR
+                                               "apanic: Err marking blk bad\n");
+                                        goto out;
+                                }
+                                printk(KERN_INFO
+                                       "apanic: Marked a bad block"
+                                       " @%llx\n", erase.addr);
+                                set_bb(erase.addr>>ctx->mtd->erasesize_shift,
+                                        apanic_bbt);
+                                continue;
+                        }
+                        goto out;
+                }
+                schedule();
+                remove_wait_queue(&wait_q, &wait);
+        }
+        printk(KERN_DEBUG "apanic: %s partition erased\n",
+               CONFIG_APANIC_PLABEL);
+out:
+        return;
+}
+
+static void apanic_remove_proc_work(struct work_struct *work)
+{
+        struct apanic_data *ctx = &drv_ctx;
+
+        mutex_lock(&drv_mutex);
+        mtd_panic_erase();
+        memset(&ctx->curr, 0, sizeof(struct panic_header));
+        if (ctx->apanic_console) {
+                remove_proc_entry("apanic_console", NULL);
+                ctx->apanic_console = NULL;
+        }
+        if (ctx->apanic_threads) {
+                remove_proc_entry("apanic_threads", NULL);
+                ctx->apanic_threads = NULL;
+        }
+        mutex_unlock(&drv_mutex);
+}
+
+static int apanic_proc_write(struct file *file, const char __user *buffer,
+                                unsigned long count, void *data)
+{
+        schedule_work(&proc_removal_work);
+        return count;
+}
+
+static void mtd_panic_notify_add(struct mtd_info *mtd)
+{
+        struct apanic_data *ctx = &drv_ctx;
+        struct panic_header *hdr = ctx->bounce;
+        size_t len;
+        int rc;
+        int    proc_entry_created = 0;
+
+        if (strcmp(mtd->name, CONFIG_APANIC_PLABEL))
+                return;
+
+        ctx->mtd = mtd;
+
+        alloc_bbt(mtd, apanic_bbt);
+        scan_bbt(mtd, apanic_bbt);
+
+        if (apanic_good_blocks == 0) {
+                printk(KERN_ERR "apanic: no any good blocks?!\n");
+                goto out_err;
+        }
+
+        rc = mtd->read(mtd, phy_offset(mtd, 0), mtd->writesize,
+                        &len, ctx->bounce);
+        if (rc && rc == -EBADMSG) {
+                printk(KERN_WARNING
+                       "apanic: Bad ECC on block 0 (ignored)\n");
+        } else if (rc && rc != -EUCLEAN) {
+                printk(KERN_ERR "apanic: Error reading block 0 (%d)\n", rc);
+                goto out_err;
+        }
+
+        if (len != mtd->writesize) {
+                printk(KERN_ERR "apanic: Bad read size (%d)\n", rc);
+                goto out_err;
+        }
+
+        printk(KERN_INFO "apanic: Bound to mtd partition '%s'\n", mtd->name);
+
+        if (hdr->magic != PANIC_MAGIC) {
+                printk(KERN_INFO "apanic: No panic data available\n");
+                mtd_panic_erase();
+                return;
+        }
+
+        if (hdr->version != PHDR_VERSION) {
+                printk(KERN_INFO "apanic: Version mismatch (%d != %d)\n",
+                       hdr->version, PHDR_VERSION);
+                mtd_panic_erase();
+                return;
+        }
+
+        memcpy(&ctx->curr, hdr, sizeof(struct panic_header));
+
+        printk(KERN_INFO "apanic: c(%u, %u) t(%u, %u)\n",
+               hdr->console_offset, hdr->console_length,
+               hdr->threads_offset, hdr->threads_length);
+
+        if (hdr->console_length) {
+                ctx->apanic_console = create_proc_entry("apanic_console",
+                                                      S_IFREG | S_IRUGO, NULL);
+                if (!ctx->apanic_console)
+                        printk(KERN_ERR "%s: failed creating procfile\n",
+                               __func__);
+                else {
+                        ctx->apanic_console->read_proc = apanic_proc_read;
+                        ctx->apanic_console->write_proc = apanic_proc_write;
+                        ctx->apanic_console->size = hdr->console_length;
+                        ctx->apanic_console->data = (void *) 1;
+                        proc_entry_created = 1;
+                }
+        }
+
+        if (hdr->threads_length) {
+                ctx->apanic_threads = create_proc_entry("apanic_threads",
+                                                       S_IFREG | S_IRUGO, NULL);
+                if (!ctx->apanic_threads)
+                        printk(KERN_ERR "%s: failed creating procfile\n",
+                               __func__);
+                else {
+                        ctx->apanic_threads->read_proc = apanic_proc_read;
+                        ctx->apanic_threads->write_proc = apanic_proc_write;
+                        ctx->apanic_threads->size = hdr->threads_length;
+                        ctx->apanic_threads->data = (void *) 2;
+                        proc_entry_created = 1;
+                }
+        }
+
+        if (!proc_entry_created)
+                mtd_panic_erase();
+
+        return;
+out_err:
+        ctx->mtd = NULL;
+}
+
+static void mtd_panic_notify_remove(struct mtd_info *mtd)
+{
+        struct apanic_data *ctx = &drv_ctx;
+        if (mtd == ctx->mtd) {
+                ctx->mtd = NULL;
+                printk(KERN_INFO "apanic: Unbound from %s\n", mtd->name);
+        }
+}
+
+static struct mtd_notifier mtd_panic_notifier = {
+        .add    = mtd_panic_notify_add,
+        .remove = mtd_panic_notify_remove,
+};
+
+static int in_panic = 0;
+
+static int apanic_writeflashpage(struct mtd_info *mtd, loff_t to,
+                                 const u_char *buf)
+{
+        int rc;
+        size_t wlen;
+        int panic = in_interrupt() | in_atomic();
+
+        if (panic && !mtd->panic_write) {
+                printk(KERN_EMERG "%s: No panic_write available\n", __func__);
+                return 0;
+        } else if (!panic && !mtd->write) {
+                printk(KERN_EMERG "%s: No write available\n", __func__);
+                return 0;
+        }
+
+        to = phy_offset(mtd, to);
+        if (to == APANIC_INVALID_OFFSET) {
+                printk(KERN_EMERG "apanic: write to invalid address\n");
+                return 0;
+        }
+
+        if (panic)
+                rc = mtd->panic_write(mtd, to, mtd->writesize, &wlen, buf);
+        else
+                rc = mtd->write(mtd, to, mtd->writesize, &wlen, buf);
+
+        if (rc) {
+                printk(KERN_EMERG
+                       "%s: Error writing data to flash (%d)\n",
+                       __func__, rc);
+                return rc;
+        }
+
+        return wlen;
+}
+
+extern int log_buf_copy(char *dest, int idx, int len);
+extern void log_buf_clear(void);
+
+/*
+ * Writes the contents of the console to the specified offset in flash.
+ * Returns number of bytes written
+ */
+static int apanic_write_console(struct mtd_info *mtd, unsigned int off)
+{
+        struct apanic_data *ctx = &drv_ctx;
+        int saved_oip;
+        int idx = 0;
+        int rc, rc2;
+        unsigned int last_chunk = 0;
+
+        while (!last_chunk) {
+                saved_oip = oops_in_progress;
+                oops_in_progress = 1;
+                rc = log_buf_copy(ctx->bounce, idx, mtd->writesize);
+                if (rc < 0)
+                        break;
+
+                if (rc != mtd->writesize)
+                        last_chunk = rc;
+
+                oops_in_progress = saved_oip;
+                if (rc <= 0)
+                        break;
+                if (rc != mtd->writesize)
+                        memset(ctx->bounce + rc, 0, mtd->writesize - rc);
+
+                rc2 = apanic_writeflashpage(mtd, off, ctx->bounce);
+                if (rc2 <= 0) {
+                        printk(KERN_EMERG
+                               "apanic: Flash write failed (%d)\n", rc2);
+                        return idx;
+                }
+                if (!last_chunk)
+                        idx += rc2;
+                else
+                        idx += last_chunk;
+                off += rc2;
+        }
+        return idx;
+}
+
+static int apanic(struct notifier_block *this, unsigned long event,
+                        void *ptr)
+{
+        struct apanic_data *ctx = &drv_ctx;
+        struct panic_header *hdr = (struct panic_header *) ctx->bounce;
+        int console_offset = 0;
+        int console_len = 0;
+        int threads_offset = 0;
+        int threads_len = 0;
+        int rc;
+
+        if (in_panic)
+                return NOTIFY_DONE;
+        in_panic = 1;
+#ifdef CONFIG_PREEMPT
+        /* Ensure that cond_resched() won't try to preempt anybody */
+        add_preempt_count(PREEMPT_ACTIVE);
+#endif
+        touch_softlockup_watchdog();
+
+        if (!ctx->mtd)
+                goto out;
+
+        if (ctx->curr.magic) {
+                printk(KERN_EMERG "Crash partition in use!\n");
+                goto out;
+        }
+        console_offset = ctx->mtd->writesize;
+
+        /*
+         * Write out the console
+         */
+        console_len = apanic_write_console(ctx->mtd, console_offset);
+        if (console_len < 0) {
+                printk(KERN_EMERG "Error writing console to panic log! (%d)\n",
+                       console_len);
+                console_len = 0;
+        }
+
+        /*
+         * Write out all threads
+         */
+        threads_offset = ALIGN(console_offset + console_len,
+                               ctx->mtd->writesize);
+        if (!threads_offset)
+                threads_offset = ctx->mtd->writesize;
+
+        ram_console_enable_console(0);
+
+        log_buf_clear();
+        show_state_filter(0);
+        threads_len = apanic_write_console(ctx->mtd, threads_offset);
+        if (threads_len < 0) {
+                printk(KERN_EMERG "Error writing threads to panic log! (%d)\n",
+                       threads_len);
+                threads_len = 0;
+        }
+
+        /*
+         * Finally write the panic header
+         */
+        memset(ctx->bounce, 0, PAGE_SIZE);
+        hdr->magic = PANIC_MAGIC;
+        hdr->version = PHDR_VERSION;
+
+        hdr->console_offset = console_offset;
+        hdr->console_length = console_len;
+
+        hdr->threads_offset = threads_offset;
+        hdr->threads_length = threads_len;
+
+        rc = apanic_writeflashpage(ctx->mtd, 0, ctx->bounce);
+        if (rc <= 0) {
+                printk(KERN_EMERG "apanic: Header write failed (%d)\n",
+                       rc);
+                goto out;
+        }
+
+        printk(KERN_EMERG "apanic: Panic dump sucessfully written to flash\n");
+
+ out:
+#ifdef CONFIG_PREEMPT
+        sub_preempt_count(PREEMPT_ACTIVE);
+#endif
+        in_panic = 0;
+        return NOTIFY_DONE;
+}
+
+static struct notifier_block panic_blk = {
+        .notifier_call  = apanic,
+};
+
+static int panic_dbg_get(void *data, u64 *val)
+{
+        apanic(NULL, 0, NULL);
+        return 0;
+}
+
+static int panic_dbg_set(void *data, u64 val)
+{
+        BUG();
+        return -1;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(panic_dbg_fops, panic_dbg_get, panic_dbg_set, "%llu\n");
+
+int __init apanic_init(void)
+{
+        register_mtd_user(&mtd_panic_notifier);
+        atomic_notifier_chain_register(&panic_notifier_list, &panic_blk);
+        debugfs_create_file("apanic", 0644, NULL, NULL, &panic_dbg_fops);
+        memset(&drv_ctx, 0, sizeof(drv_ctx));
+        drv_ctx.bounce = (void *) __get_free_page(GFP_KERNEL);
+        INIT_WORK(&proc_removal_work, apanic_remove_proc_work);
+        printk(KERN_INFO "Android kernel panic handler initialized (bind=%s)\n",
+               CONFIG_APANIC_PLABEL);
+        return 0;
+}
+
+module_init(apanic_init);
diff --git a/drivers/misc/bcm4329_rfkill.c b/drivers/misc/bcm4329_rfkill.c
new file mode 100644
index 00000000000..a077326f255
--- /dev/null
+++ b/drivers/misc/bcm4329_rfkill.c
@@ -0,0 +1,207 @@
+/*
+ * drivers/misc/bcm4329_rfkill.c
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/err.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/rfkill.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+
+struct bcm4329_rfkill_data {
+        int gpio_reset;
+        int gpio_shutdown;
+        int delay;
+        struct clk *bt_32k_clk;
+};
+
+static struct bcm4329_rfkill_data *bcm4329_rfkill;
+
+static int bcm4329_bt_rfkill_set_power(void *data, bool blocked)
+{
+        if (blocked) {
+                if (bcm4329_rfkill->gpio_shutdown)
+                        gpio_direction_output(bcm4329_rfkill->gpio_shutdown, 0);
+                if (bcm4329_rfkill->gpio_reset)
+                        gpio_direction_output(bcm4329_rfkill->gpio_reset, 0);
+                if (bcm4329_rfkill->bt_32k_clk)
+                        clk_disable(bcm4329_rfkill->bt_32k_clk);
+        } else {
+                if (bcm4329_rfkill->bt_32k_clk)
+                        clk_enable(bcm4329_rfkill->bt_32k_clk);
+                if (bcm4329_rfkill->gpio_shutdown)
+                {
+                        gpio_direction_output(bcm4329_rfkill->gpio_shutdown, 0);
+                        msleep(100);
+                        gpio_direction_output(bcm4329_rfkill->gpio_shutdown, 1);
+                        msleep(100);
+                }
+                if (bcm4329_rfkill->gpio_reset)
+                {
+                        gpio_direction_output(bcm4329_rfkill->gpio_reset, 0);
+                        msleep(100);
+                        gpio_direction_output(bcm4329_rfkill->gpio_reset, 1);
+                        msleep(100);
+                }
+        }
+
+        return 0;
+}
+
+static const struct rfkill_ops bcm4329_bt_rfkill_ops = {
+        .set_block = bcm4329_bt_rfkill_set_power,
+};
+
+static int bcm4329_rfkill_probe(struct platform_device *pdev)
+{
+        struct rfkill *bt_rfkill;
+        struct resource *res;
+        int ret;
+        bool enable = false;  /* off */
+        bool default_sw_block_state;
+
+        bcm4329_rfkill = kzalloc(sizeof(*bcm4329_rfkill), GFP_KERNEL);
+        if (!bcm4329_rfkill)
+                return -ENOMEM;
+
+        bcm4329_rfkill->bt_32k_clk = clk_get(&pdev->dev, "bcm4329_32k_clk");
+        if (IS_ERR(bcm4329_rfkill->bt_32k_clk)) {
+                pr_warn("%s: can't find bcm4329_32k_clk.\
+                                assuming 32k clock to chip\n", __func__);
+                bcm4329_rfkill->bt_32k_clk = NULL;
+        }
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_IO,
+                                                "bcm4329_nreset_gpio");
+        if (res) {
+                bcm4329_rfkill->gpio_reset = res->start;
+                tegra_gpio_enable(bcm4329_rfkill->gpio_reset);
+                ret = gpio_request(bcm4329_rfkill->gpio_reset,
+                                                "bcm4329_nreset_gpio");
+        } else {
+                pr_warn("%s : can't find reset gpio.\n", __func__);
+                bcm4329_rfkill->gpio_reset = 0;
+        }
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_IO,
+                                                "bcm4329_nshutdown_gpio");
+        if (res) {
+                bcm4329_rfkill->gpio_shutdown = res->start;
+                tegra_gpio_enable(bcm4329_rfkill->gpio_shutdown);
+                ret = gpio_request(bcm4329_rfkill->gpio_shutdown,
+                                                "bcm4329_nshutdown_gpio");
+        } else {
+                pr_warn("%s : can't find shutdown gpio.\n", __func__);
+                bcm4329_rfkill->gpio_shutdown = 0;
+        }
+
+        /* make sure at-least one of the GPIO is defined */
+        if (!bcm4329_rfkill->gpio_reset && !bcm4329_rfkill->gpio_shutdown)
+                goto free_bcm_res;
+
+        if (bcm4329_rfkill->bt_32k_clk && enable)
+                clk_enable(bcm4329_rfkill->bt_32k_clk);
+        if (bcm4329_rfkill->gpio_shutdown)
+                gpio_direction_output(bcm4329_rfkill->gpio_shutdown, enable);
+        if (bcm4329_rfkill->gpio_reset)
+                gpio_direction_output(bcm4329_rfkill->gpio_reset, enable);
+
+        bt_rfkill = rfkill_alloc("bcm4329 Bluetooth", &pdev->dev,
+                                RFKILL_TYPE_BLUETOOTH, &bcm4329_bt_rfkill_ops,
+                                NULL);
+
+        if (unlikely(!bt_rfkill))
+                goto free_bcm_res;
+
+        default_sw_block_state = !enable;
+        rfkill_set_states(bt_rfkill, default_sw_block_state, false);
+
+        ret = rfkill_register(bt_rfkill);
+
+        if (unlikely(ret)) {
+                rfkill_destroy(bt_rfkill);
+                goto free_bcm_res;
+        }
+
+        return 0;
+
+free_bcm_res:
+        if (bcm4329_rfkill->gpio_shutdown)
+                gpio_free(bcm4329_rfkill->gpio_shutdown);
+        if (bcm4329_rfkill->gpio_reset)
+                gpio_free(bcm4329_rfkill->gpio_reset);
+        if (bcm4329_rfkill->bt_32k_clk && enable)
+                clk_disable(bcm4329_rfkill->bt_32k_clk);
+        if (bcm4329_rfkill->bt_32k_clk)
+                clk_put(bcm4329_rfkill->bt_32k_clk);
+        kfree(bcm4329_rfkill);
+        return -ENODEV;
+}
+
+static int bcm4329_rfkill_remove(struct platform_device *pdev)
+{
+        struct rfkill *bt_rfkill = platform_get_drvdata(pdev);
+
+        if (bcm4329_rfkill->bt_32k_clk)
+                clk_put(bcm4329_rfkill->bt_32k_clk);
+        rfkill_unregister(bt_rfkill);
+        rfkill_destroy(bt_rfkill);
+        if (bcm4329_rfkill->gpio_shutdown)
+                gpio_free(bcm4329_rfkill->gpio_shutdown);
+        if (bcm4329_rfkill->gpio_reset)
+                gpio_free(bcm4329_rfkill->gpio_reset);
+        kfree(bcm4329_rfkill);
+
+        return 0;
+}
+
+static struct platform_driver bcm4329_rfkill_driver = {
+        .probe = bcm4329_rfkill_probe,
+        .remove = bcm4329_rfkill_remove,
+        .driver = {
+                   .name = "bcm4329_rfkill",
+                   .owner = THIS_MODULE,
+        },
+};
+
+static int __init bcm4329_rfkill_init(void)
+{
+        return platform_driver_register(&bcm4329_rfkill_driver);
+}
+
+static void __exit bcm4329_rfkill_exit(void)
+{
+        platform_driver_unregister(&bcm4329_rfkill_driver);
+}
+
+module_init(bcm4329_rfkill_init);
+module_exit(bcm4329_rfkill_exit);
+
+MODULE_DESCRIPTION("BCM4329 rfkill");
+MODULE_AUTHOR("NVIDIA");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/inv_mpu/Kconfig b/drivers/misc/inv_mpu/Kconfig
new file mode 100644
index 00000000000..4c231b576f6
--- /dev/null
+++ b/drivers/misc/inv_mpu/Kconfig
@@ -0,0 +1,70 @@
+config MPU_SENSORS_TIMERIRQ
+        tristate "MPU Timer IRQ"
+        help
+          If you say yes here you get access to the timerirq device handle which
+          can be used to select on.  This can be used instead of IRQ's, sleeping,
+          or timer threads.  Reading from this device returns the same type of
+          information as reading from the MPU and slave IRQ's.
+
+menuconfig INV_SENSORS
+        tristate "Motion Processing Unit"
+        depends on I2C
+        default y
+
+if INV_SENSORS
+
+choice
+        tristate "MPU Master"
+        depends on I2C && INV_SENSORS
+        default MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_MPU3050
+        tristate "MPU3050"
+        depends on I2C
+        select MPU_SENSORS_MPU3050_GYRO
+        help
+          If you say yes here you get support for the MPU3050 Gyroscope driver
+          This driver can also be built as a module.  If so, the module
+          will be called mpu3050.
+
+config MPU_SENSORS_MPU6050A2
+        tristate "MPU6050A2"
+        depends on I2C
+        select MPU_SENSORS_MPU6050_GYRO
+        help
+          If you say yes here you get support for the MPU6050A2 Gyroscope driver
+          This driver can also be built as a module.  If so, the module
+          will be called mpu6050a2.
+
+config MPU_SENSORS_MPU6050B1
+        tristate "MPU6050B1"
+        depends on I2C
+        select MPU_SENSORS_MPU6050_GYRO
+        help
+          If you say yes here you get support for the MPU6050 Gyroscope driver
+          This driver can also be built as a module.  If so, the module
+          will be called mpu6050b1.
+
+endchoice
+
+config MPU_SENSORS_MPU3050_GYRO
+        bool "MPU3050 built in gyroscope"
+        depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_MPU6050_GYRO
+        bool "MPU6050 built in gyroscope"
+        depends on MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+
+source "drivers/misc/inv_mpu/accel/Kconfig"
+source "drivers/misc/inv_mpu/compass/Kconfig"
+source "drivers/misc/inv_mpu/pressure/Kconfig"
+
+config MPU_USERSPACE_DEBUG
+        bool "MPU Userspace debugging ioctls"
+        help
+          Allows the ioctls MPU_SET_MPU_PLATFORM_DATA and
+          MPU_SET_EXT_SLAVE_PLATFORM_DATA, which allows userspace applications
+          to override the slave address and orientation.  This is dangerous
+          and could cause the devices not to work.
+
+endif #INV_SENSORS
diff --git a/drivers/misc/inv_mpu/Makefile b/drivers/misc/inv_mpu/Makefile
new file mode 100644
index 00000000000..f10c6844a08
--- /dev/null
+++ b/drivers/misc/inv_mpu/Makefile
@@ -0,0 +1,18 @@
+
+# Kernel makefile for motions sensors
+#
+#
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
+EXTRA_CFLAGS += -DINV_CACHE_DMP=1
+
+obj-$(CONFIG_MPU_SENSORS_TIMERIRQ)+= timerirq.o
+obj-$(CONFIG_INV_SENSORS) += mpuirq.o slaveirq.o
+
+obj-$(CONFIG_MPU_SENSORS_MPU3050) += mpu3050/
+obj-$(CONFIG_MPU_SENSORS_MPU6050B1) += mpu6050/
+
+obj-y += accel/
+obj-y += compass/
+obj-y += pressure/
diff --git a/drivers/misc/inv_mpu/accel/Kconfig b/drivers/misc/inv_mpu/accel/Kconfig
new file mode 100644
index 00000000000..4e280bd876b
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/Kconfig
@@ -0,0 +1,133 @@
+menuconfig INV_SENSORS_ACCELEROMETERS
+        bool "Accelerometer Slave Sensors"
+        default y
+        ---help---
+          Say Y here to get to see options for device drivers for various
+          accelerometrs for integration with the MPU3050 or MPU6050 driver.
+          This option alone does not add any kernel code.
+
+          If you say N, all options in this submenu will be skipped and disabled.
+
+if INV_SENSORS_ACCELEROMETERS
+
+config MPU_SENSORS_ADXL34X
+        tristate "ADI adxl34x"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the ADI adxl345 or adxl346 accelerometers.
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_BMA222
+        tristate "Bosch BMA222"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the Bosch BMA222 accelerometer
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_BMA150
+        tristate "Bosch BMA150"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the Bosch BMA150 accelerometer
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_BMA250
+        tristate "Bosch BMA250"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the Bosch BMA250 accelerometer
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_KXSD9
+        tristate "Kionix KXSD9"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the Kionix KXSD9 accelerometer
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_KXTF9
+        tristate "Kionix KXTF9"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the Kionix KXFT9 accelerometer
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_LIS331DLH
+        tristate "ST lis331dlh"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the ST lis331dlh accelerometer
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_LIS3DH
+        tristate "ST lis3dh"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the ST lis3dh accelerometer
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_LSM303DLX_A
+        tristate "ST lsm303dlx"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the ST lsm303dlh and lsm303dlm accelerometers
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_MMA8450
+        tristate "Freescale mma8450"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the Freescale mma8450 accelerometer
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_MMA845X
+        tristate "Freescale mma8451/8452/8453"
+        depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the Freescale mma8451/8452/8453 accelerometer
+          This support is for integration with the MPU3050 gyroscope device
+          driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_MPU6050_ACCEL
+        tristate "MPU6050 built in accelerometer"
+        depends on MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+        help
+          This enables support for the MPU6050 built in accelerometer.
+          This the built in support for integration with the MPU6050 gyroscope
+          device driver.  This is the only accelerometer supported with the
+          MPU6050.  Specifying another accelerometer in the board file will
+          result in runtime errors.
+
+endif
diff --git a/drivers/misc/inv_mpu/accel/Makefile b/drivers/misc/inv_mpu/accel/Makefile
new file mode 100644
index 00000000000..1f0f5bec677
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/Makefile
@@ -0,0 +1,38 @@
+#
+# Accel Slaves to MPUxxxx
+#
+obj-$(CONFIG_MPU_SENSORS_ADXL34X) += inv_mpu_adxl34x.o
+inv_mpu_adxl34x-objs += adxl34x.o
+
+obj-$(CONFIG_MPU_SENSORS_BMA150) += inv_mpu_bma150.o
+inv_mpu_bma150-objs += bma150.o
+
+obj-$(CONFIG_MPU_SENSORS_KXTF9) += inv_mpu_kxtf9.o
+inv_mpu_kxtf9-objs += kxtf9.o
+
+obj-$(CONFIG_MPU_SENSORS_BMA222) += inv_mpu_bma222.o
+inv_mpu_bma222-objs += bma222.o
+
+obj-$(CONFIG_MPU_SENSORS_BMA250) += inv_mpu_bma250.o
+inv_mpu_bma250-objs += bma250.o
+
+obj-$(CONFIG_MPU_SENSORS_KXSD9) += inv_mpu_kxsd9.o
+inv_mpu_kxsd9-objs += kxsd9.o
+
+obj-$(CONFIG_MPU_SENSORS_LIS331DLH) += inv_mpu_lis331.o
+inv_mpu_lis331-objs += lis331.o
+
+obj-$(CONFIG_MPU_SENSORS_LIS3DH) += inv_mpu_lis3dh.o
+inv_mpu_lis3dh-objs += lis3dh.o
+
+obj-$(CONFIG_MPU_SENSORS_LSM303DLX_A) += inv_mpu_lsm303dlx_a.o
+inv_mpu_lsm303dlx_a-objs += lsm303dlx_a.o
+
+obj-$(CONFIG_MPU_SENSORS_MMA8450) += inv_mpu_mma8450.o
+inv_mpu_mma8450-objs += mma8450.o
+
+obj-$(CONFIG_MPU_SENSORS_MMA845X) += inv_mpu_mma845x.o
+inv_mpu_mma845x-objs += mma845x.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
diff --git a/drivers/misc/inv_mpu/accel/adxl34x.c b/drivers/misc/inv_mpu/accel/adxl34x.c
new file mode 100644
index 00000000000..f2bff8a7592
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/adxl34x.c
@@ -0,0 +1,728 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   adxl34x.c
+ *      @brief  Accelerometer setup and handling methods for AD adxl345 and
+ *              adxl346.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+/* registers */
+#define ADXL34X_ODR_REG                 (0x2C)
+#define ADXL34X_PWR_REG                 (0x2D)
+#define ADXL34X_DATAFORMAT_REG          (0x31)
+
+/* masks */
+#define ADXL34X_ODR_MASK                (0x0F)
+#define ADXL34X_PWR_SLEEP_MASK          (0x04)
+#define ADXL34X_PWR_MEAS_MASK           (0x08)
+#define ADXL34X_DATAFORMAT_JUSTIFY_MASK (0x04)
+#define ADXL34X_DATAFORMAT_FSR_MASK     (0x03)
+
+/* -------------------------------------------------------------------------- */
+
+struct adxl34x_config {
+        unsigned int odr;               /** < output data rate in mHz */
+        unsigned int fsr;               /** < full scale range mg */
+        unsigned int fsr_reg_mask;      /** < register setting for fsr */
+};
+
+struct adxl34x_private_data {
+        struct adxl34x_config suspend;  /** < suspend configuration */
+        struct adxl34x_config resume;   /** < resume configuration */
+};
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_set_odr(void *mlsl_handle,
+                           struct ext_slave_platform_data *pdata,
+                           struct adxl34x_config *config,
+                           int apply,
+                           long odr)
+{
+        int result = INV_SUCCESS;
+        unsigned char new_odr_mask;
+
+        /* ADXL346 (Rev. A) pages 13, 24 */
+        if (odr >= 3200000) {
+                new_odr_mask = 0x0F;
+                config->odr = 3200000;
+        } else if (odr >= 1600000) {
+                new_odr_mask = 0x0E;
+                config->odr = 1600000;
+        } else if (odr >= 800000) {
+                new_odr_mask = 0x0D;
+                config->odr = 800000;
+        } else if (odr >= 400000) {
+                new_odr_mask = 0x0C;
+                config->odr = 400000;
+        } else if (odr >= 200000) {
+                new_odr_mask = 0x0B;
+                config->odr = 200000;
+        } else if (odr >= 100000) {
+                new_odr_mask = 0x0A;
+                config->odr = 100000;
+        } else if (odr >= 50000) {
+                new_odr_mask = 0x09;
+                config->odr = 50000;
+        } else if (odr >= 25000) {
+                new_odr_mask = 0x08;
+                config->odr = 25000;
+        } else if (odr >= 12500) {
+                new_odr_mask = 0x07;
+                config->odr = 12500;
+        } else if (odr >= 6250) {
+                new_odr_mask = 0x06;
+                config->odr = 6250;
+        } else if (odr >= 3130) {
+                new_odr_mask = 0x05;
+                config->odr = 3130;
+        } else if (odr >= 1560) {
+                new_odr_mask = 0x04;
+                config->odr = 1560;
+        } else if (odr >= 780) {
+                new_odr_mask = 0x03;
+                config->odr = 780;
+        } else if (odr >= 390) {
+                new_odr_mask = 0x02;
+                config->odr = 390;
+        } else if (odr >= 200) {
+                new_odr_mask = 0x01;
+                config->odr = 200;
+        } else {
+                new_odr_mask = 0x00;
+                config->odr = 100;
+        }
+
+        if (apply) {
+                unsigned char reg_odr;
+                result = inv_serial_read(mlsl_handle, pdata->address,
+                                ADXL34X_ODR_REG, 1, &reg_odr);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                reg_odr &= ~ADXL34X_ODR_MASK;
+                reg_odr |= new_odr_mask;
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                ADXL34X_ODR_REG, reg_odr);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("ODR: %d mHz\n", config->odr);
+        }
+        return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range in milli gees (mg).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_set_fsr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct adxl34x_config *config,
+                          int apply,
+                          long fsr)
+{
+        int result = INV_SUCCESS;
+
+        if (fsr <= 2000) {
+                config->fsr_reg_mask = 0x00;
+                config->fsr = 2000;
+        } else if (fsr <= 4000) {
+                config->fsr_reg_mask = 0x01;
+                config->fsr = 4000;
+        } else if (fsr <= 8000) {
+                config->fsr_reg_mask = 0x02;
+                config->fsr = 8000;
+        } else { /* 8001 -> oo */
+                config->fsr_reg_mask = 0x03;
+                config->fsr = 16000;
+        }
+
+        if (apply) {
+                unsigned char reg_df;
+                result = inv_serial_read(mlsl_handle, pdata->address,
+                                ADXL34X_DATAFORMAT_REG, 1, &reg_df);
+                reg_df &= ~ADXL34X_DATAFORMAT_FSR_MASK;
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                ADXL34X_DATAFORMAT_REG,
+                                reg_df | config->fsr_reg_mask);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("FSR: %d mg\n", config->fsr);
+        }
+        return result;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_get_config(void *mlsl_handle,
+                    struct ext_slave_descr *slave,
+                                struct ext_slave_platform_data *pdata,
+                                struct ext_slave_config *data)
+{
+        struct adxl34x_private_data *private_data =
+                        (struct adxl34x_private_data *)(pdata->private_data);
+
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        struct adxl34x_private_data *private_data =
+                        (struct adxl34x_private_data *)(pdata->private_data);
+
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return adxl34x_set_odr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return adxl34x_set_odr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return adxl34x_set_fsr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return adxl34x_set_fsr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_suspend(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata)
+{
+        int result;
+
+        /*
+        struct adxl34x_config *suspend_config =
+                &((struct adxl34x_private_data *)pdata->private_data)->suspend;
+
+        result = adxl34x_set_odr(mlsl_handle, pdata, suspend_config,
+                                true, suspend_config->odr);
+        if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+}
+        result = adxl34x_set_fsr(mlsl_handle, pdata, suspend_config,
+                                true, suspend_config->fsr);
+        if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+}
+        */
+
+        /*
+          Page 25
+          When clearing the sleep bit, it is recommended that the part
+          be placed into standby mode and then set back to measurement mode
+          with a subsequent write.
+          This is done to ensure that the device is properly biased if sleep
+          mode is manually disabled; otherwise, the first few samples of data
+          after the sleep bit is cleared may have additional noise,
+          especially if the device was asleep when the bit was cleared. */
+
+        /* go in standy-by mode (suspends measurements) */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        ADXL34X_PWR_REG, ADXL34X_PWR_MEAS_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* and then in sleep */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        ADXL34X_PWR_REG,
+                        ADXL34X_PWR_MEAS_MASK | ADXL34X_PWR_SLEEP_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_resume(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        struct adxl34x_config *resume_config =
+                &((struct adxl34x_private_data *)pdata->private_data)->resume;
+        unsigned char reg;
+
+        /*
+          Page 25
+          When clearing the sleep bit, it is recommended that the part
+          be placed into standby mode and then set back to measurement mode
+          with a subsequent write.
+          This is done to ensure that the device is properly biased if sleep
+          mode is manually disabled; otherwise, the first few samples of data
+          after the sleep bit is cleared may have additional noise,
+          especially if the device was asleep when the bit was cleared. */
+
+        /* remove sleep, but leave in stand-by */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        ADXL34X_PWR_REG, ADXL34X_PWR_MEAS_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = adxl34x_set_odr(mlsl_handle, pdata, resume_config,
+                                true, resume_config->odr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /*
+          -> FSR
+          -> Justiy bit for Big endianess
+          -> resulution to 10 bits
+        */
+        reg = ADXL34X_DATAFORMAT_JUSTIFY_MASK;
+        reg |= resume_config->fsr_reg_mask;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        ADXL34X_DATAFORMAT_REG, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* go in measurement mode */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        ADXL34X_PWR_REG, 0x00);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* DATA_FORMAT: full resolution of +/-2g; data is left justified */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         0x31, reg);
+
+        return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_init(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata)
+{
+        int result;
+        long range;
+
+        struct adxl34x_private_data *private_data;
+        private_data = (struct adxl34x_private_data *)
+            kzalloc(sizeof(struct adxl34x_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        result = adxl34x_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                                false, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = adxl34x_set_odr(mlsl_handle, pdata, &private_data->resume,
+                                false, 200000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        range = range_fixedpoint_to_long_mg(slave->range);
+        result = adxl34x_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                                false, range);
+        result = adxl34x_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                                false, range);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = adxl34x_suspend(mlsl_handle, slave, pdata);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_exit(void *mlsl_handle,
+                 struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_read(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        unsigned char *data)
+{
+        int result;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 slave->read_reg, slave->read_len, data);
+        return result;
+}
+
+static struct ext_slave_descr adxl34x_descr = {
+        .init             = adxl34x_init,
+        .exit             = adxl34x_exit,
+        .suspend          = adxl34x_suspend,
+        .resume           = adxl34x_resume,
+        .read             = adxl34x_read,
+        .config           = adxl34x_config,
+        .get_config       = adxl34x_get_config,
+        .name             = "adxl34x",  /* 5 or 6 */
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_ADXL34X,
+        .read_reg         = 0x32,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {2, 0},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *adxl34x_get_slave_descr(void)
+{
+        return &adxl34x_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct adxl34x_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int adxl34x_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct adxl34x_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        adxl34x_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int adxl34x_mod_remove(struct i2c_client *client)
+{
+        struct adxl34x_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                adxl34x_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id adxl34x_mod_id[] = {
+        { "adxl34x", ACCEL_ID_ADXL34X },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, adxl34x_mod_id);
+
+static struct i2c_driver adxl34x_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = adxl34x_mod_probe,
+        .remove = adxl34x_mod_remove,
+        .id_table = adxl34x_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "adxl34x_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init adxl34x_mod_init(void)
+{
+        int res = i2c_add_driver(&adxl34x_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "adxl34x_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit adxl34x_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&adxl34x_mod_driver);
+}
+
+module_init(adxl34x_mod_init);
+module_exit(adxl34x_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate ADXL34X sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("adxl34x_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/bma150.c b/drivers/misc/inv_mpu/accel/bma150.c
new file mode 100644
index 00000000000..745d90a6744
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/bma150.c
@@ -0,0 +1,777 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   bma150.c
+ *      @brief  Accelerometer setup and handling methods for Bosch BMA150.
+ */
+
+/* -------------------------------------------------------------------------- */
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+
+/* -------------------------------------------------------------------------- */
+/* registers */
+#define BMA150_CTRL_REG                 (0x14)
+#define BMA150_INT_REG                  (0x15)
+#define BMA150_PWR_REG                  (0x0A)
+
+/* masks */
+#define BMA150_CTRL_MASK                (0x18)
+#define BMA150_CTRL_MASK_ODR            (0xF8)
+#define BMA150_CTRL_MASK_FSR            (0xE7)
+#define BMA150_INT_MASK_WUP             (0xF8)
+#define BMA150_INT_MASK_IRQ             (0xDF)
+#define BMA150_PWR_MASK_SLEEP           (0x01)
+#define BMA150_PWR_MASK_SOFT_RESET      (0x02)
+
+/* -------------------------------------------------------------------------- */
+struct bma150_config {
+        unsigned int odr;       /** < output data rate mHz */
+        unsigned int fsr;       /** < full scale range mgees */
+        unsigned int irq_type;  /** < type of IRQ, see bma150_set_irq */
+        unsigned char ctrl_reg; /** < control register value */
+        unsigned char int_reg;  /** < interrupt control register value */
+};
+
+struct bma150_private_data {
+        struct bma150_config suspend;   /** < suspend configuration */
+        struct bma150_config resume;    /** < resume configuration */
+};
+
+/**
+ *  @brief Simply disables the IRQ since it is not usable on BMA150 devices.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *              configuration to apply to, suspend or resume
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param irq_type
+ *              the type of IRQ.  Valid values are
+ *              - MPU_SLAVE_IRQ_TYPE_NONE
+ *              - MPU_SLAVE_IRQ_TYPE_MOTION
+ *              - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ *              The only supported IRQ type is MPU_SLAVE_IRQ_TYPE_NONE which
+ *              corresponds to disabling the IRQ completely.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_set_irq(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct bma150_config *config,
+                          int apply,
+                          long irq_type)
+{
+        int result = INV_SUCCESS;
+
+        if (irq_type != MPU_SLAVE_IRQ_TYPE_NONE)
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+        config->irq_type = MPU_SLAVE_IRQ_TYPE_NONE;
+        config->int_reg = 0x00;
+
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                         BMA150_CTRL_REG, config->ctrl_reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                         BMA150_INT_REG, config->int_reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_set_odr(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct bma150_config *config,
+                        int apply,
+                        long odr)
+{
+        unsigned char odr_bits = 0;
+        unsigned char wup_bits = 0;
+        int result = INV_SUCCESS;
+
+        if (odr > 100000) {
+                config->odr = 190000;
+                odr_bits = 0x03;
+        } else if (odr > 50000) {
+                config->odr = 100000;
+                odr_bits = 0x02;
+        } else if (odr > 25000) {
+                config->odr = 50000;
+                odr_bits = 0x01;
+        } else if (odr > 0) {
+                config->odr = 25000;
+                odr_bits = 0x00;
+        } else {
+                config->odr = 0;
+                wup_bits = 0x00;
+        }
+
+        config->int_reg &= BMA150_INT_MASK_WUP;
+        config->ctrl_reg &= BMA150_CTRL_MASK_ODR;
+        config->ctrl_reg |= odr_bits;
+
+        MPL_LOGV("ODR: %d\n", config->odr);
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        BMA150_CTRL_REG, config->ctrl_reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        BMA150_INT_REG, config->int_reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_set_fsr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct bma150_config *config,
+                          int apply,
+                          long fsr)
+{
+        unsigned char fsr_bits;
+        int result = INV_SUCCESS;
+
+        if (fsr <= 2048) {
+                fsr_bits = 0x00;
+                config->fsr = 2048;
+        } else if (fsr <= 4096) {
+                fsr_bits = 0x08;
+                config->fsr = 4096;
+        } else {
+                fsr_bits = 0x10;
+                config->fsr = 8192;
+        }
+
+        config->ctrl_reg &= BMA150_CTRL_MASK_FSR;
+        config->ctrl_reg |= fsr_bits;
+
+        MPL_LOGV("FSR: %d\n", config->fsr);
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA150_CTRL_REG, config->ctrl_reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA150_CTRL_REG, config->ctrl_reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_init(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char reg;
+        long range;
+
+        struct bma150_private_data *private_data;
+        private_data = (struct bma150_private_data *)
+            kzalloc(sizeof(struct bma150_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        BMA150_PWR_REG, BMA150_PWR_MASK_SOFT_RESET);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(1);
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 BMA150_CTRL_REG, 1, &reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        private_data->resume.ctrl_reg = reg;
+        private_data->suspend.ctrl_reg = reg;
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 BMA150_INT_REG, 1, &reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        private_data->resume.int_reg = reg;
+        private_data->suspend.int_reg = reg;
+
+        result = bma150_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                                false, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma150_set_odr(mlsl_handle, pdata, &private_data->resume,
+                                false, 200000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        range = range_fixedpoint_to_long_mg(slave->range);
+        result = bma150_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                                false, range);
+        result = bma150_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                                false, range);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = bma150_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                                false, MPU_SLAVE_IRQ_TYPE_NONE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma150_set_irq(mlsl_handle, pdata, &private_data->resume,
+                                false, MPU_SLAVE_IRQ_TYPE_NONE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       BMA150_PWR_REG, BMA150_PWR_MASK_SLEEP);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_exit(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        struct bma150_private_data *private_data =
+                        (struct bma150_private_data *)(pdata->private_data);
+
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return bma150_set_odr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return bma150_set_odr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return bma150_set_fsr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return bma150_set_fsr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return bma150_set_irq(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return bma150_set_irq(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_get_config(void *mlsl_handle,
+                                struct ext_slave_descr *slave,
+                                struct ext_slave_platform_data *pdata,
+                                struct ext_slave_config *data)
+{
+        struct bma150_private_data *private_data =
+                        (struct bma150_private_data *)(pdata->private_data);
+
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.irq_type;
+                break;
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char ctrl_reg;
+        unsigned char int_reg;
+
+        struct bma150_private_data *private_data =
+                        (struct bma150_private_data *)(pdata->private_data);
+
+        ctrl_reg = private_data->suspend.ctrl_reg;
+        int_reg = private_data->suspend.int_reg;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        BMA150_PWR_REG, BMA150_PWR_MASK_SOFT_RESET);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(1);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        BMA150_CTRL_REG, ctrl_reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        BMA150_INT_REG, int_reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                BMA150_PWR_REG, BMA150_PWR_MASK_SLEEP);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char ctrl_reg;
+        unsigned char int_reg;
+
+        struct bma150_private_data *private_data =
+                        (struct bma150_private_data *)(pdata->private_data);
+
+        ctrl_reg = private_data->resume.ctrl_reg;
+        int_reg = private_data->resume.int_reg;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA150_PWR_REG, BMA150_PWR_MASK_SOFT_RESET);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(1);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA150_CTRL_REG, ctrl_reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA150_INT_REG, int_reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA150_PWR_REG, 0x00);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        return inv_serial_read(mlsl_handle, pdata->address,
+                               slave->read_reg, slave->read_len, data);
+}
+
+static struct ext_slave_descr bma150_descr = {
+        .init             = bma150_init,
+        .exit             = bma150_exit,
+        .suspend          = bma150_suspend,
+        .resume           = bma150_resume,
+        .read             = bma150_read,
+        .config           = bma150_config,
+        .get_config       = bma150_get_config,
+        .name             = "bma150",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_BMA150,
+        .read_reg         = 0x02,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {2, 0},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *bma150_get_slave_descr(void)
+{
+        return &bma150_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+
+/* Platform data for the MPU */
+struct bma150_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int bma150_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct bma150_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        bma150_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int bma150_mod_remove(struct i2c_client *client)
+{
+        struct bma150_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                bma150_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id bma150_mod_id[] = {
+        { "bma150", ACCEL_ID_BMA150 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, bma150_mod_id);
+
+static struct i2c_driver bma150_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = bma150_mod_probe,
+        .remove = bma150_mod_remove,
+        .id_table = bma150_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "bma150_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init bma150_mod_init(void)
+{
+        int res = i2c_add_driver(&bma150_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "bma150_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit bma150_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&bma150_mod_driver);
+}
+
+module_init(bma150_mod_init);
+module_exit(bma150_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate BMA150 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("bma150_mod");
+
+/**
+ *  @}
+ */
+
diff --git a/drivers/misc/inv_mpu/accel/bma222.c b/drivers/misc/inv_mpu/accel/bma222.c
new file mode 100644
index 00000000000..e9fc99b1a62
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/bma222.c
@@ -0,0 +1,654 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   bma222.c
+ *      @brief  Accelerometer setup and handling methods for Bosch BMA222.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+
+/* -------------------------------------------------------------------------- */
+
+#define BMA222_STATUS_REG       (0x0A)
+#define BMA222_FSR_REG          (0x0F)
+#define ADXL34X_ODR_REG         (0x10)
+#define BMA222_PWR_REG          (0x11)
+#define BMA222_SOFTRESET_REG    (0x14)
+
+#define BMA222_STATUS_RDY_MASK  (0x80)
+#define BMA222_FSR_MASK         (0x0F)
+#define BMA222_ODR_MASK         (0x1F)
+#define BMA222_PWR_SLEEP_MASK   (0x80)
+#define BMA222_PWR_AWAKE_MASK   (0x00)
+#define BMA222_SOFTRESET_MASK   (0xB6)
+#define BMA222_SOFTRESET_MASK   (0xB6)
+
+/* -------------------------------------------------------------------------- */
+
+struct bma222_config {
+        unsigned int odr;               /** < output data rate in mHz */
+        unsigned int fsr;               /** < full scale range mg */
+};
+
+struct bma222_private_data {
+        struct bma222_config suspend;   /** < suspend configuration */
+        struct bma222_config resume;    /** < resume configuration */
+};
+
+
+/* -------------------------------------------------------------------------- */
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_set_odr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct bma222_config *config,
+                          int apply,
+                          long odr)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg_odr;
+
+        if (odr >= 1000000) {
+                reg_odr = 0x0F;
+                config->odr = 1000000;
+        } else if (odr >= 500000) {
+                reg_odr = 0x0E;
+                config->odr = 500000;
+        } else if (odr >= 250000) {
+                reg_odr = 0x0D;
+                config->odr = 250000;
+        } else if (odr >= 125000) {
+                reg_odr = 0x0C;
+                config->odr = 125000;
+        } else if (odr >= 62500) {
+                reg_odr = 0x0B;
+                config->odr = 62500;
+        } else if (odr >= 32000) {
+                reg_odr = 0x0A;
+                config->odr = 32000;
+        } else if (odr >= 16000) {
+                reg_odr = 0x09;
+                config->odr = 16000;
+        } else {
+                reg_odr = 0x08;
+                config->odr = 8000;
+        }
+
+        if (apply) {
+                MPL_LOGV("ODR: %d\n", config->odr);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        ADXL34X_ODR_REG, reg_odr);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_set_fsr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct bma222_config *config,
+                          int apply,
+                          long fsr)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg_fsr_mask;
+
+        if (fsr <= 2000) {
+                reg_fsr_mask = 0x03;
+                config->fsr = 2000;
+        } else if (fsr <= 4000) {
+                reg_fsr_mask = 0x05;
+                config->fsr = 4000;
+        } else if (fsr <= 8000) {
+                reg_fsr_mask = 0x08;
+                config->fsr = 8000;
+        } else { /* 8001 -> oo */
+                reg_fsr_mask = 0x0C;
+                config->fsr = 16000;
+        }
+
+        if (apply) {
+                MPL_LOGV("FSR: %d\n", config->fsr);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA222_FSR_REG, reg_fsr_mask);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_init(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        int result;
+
+        struct bma222_private_data *private_data;
+        private_data = (struct bma222_private_data *)
+            kzalloc(sizeof(struct bma222_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        BMA222_SOFTRESET_REG, BMA222_SOFTRESET_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(1);
+
+        result = bma222_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                                false, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma222_set_odr(mlsl_handle, pdata, &private_data->resume,
+                                false, 200000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = bma222_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                                false, 2000);
+        result = bma222_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                                false, 2000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA222_PWR_REG, BMA222_PWR_SLEEP_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_exit(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_get_config(void *mlsl_handle,
+                                struct ext_slave_descr *slave,
+                                struct ext_slave_platform_data *pdata,
+                                struct ext_slave_config *data)
+{
+        struct bma222_private_data *private_data =
+                        (struct bma222_private_data *)(pdata->private_data);
+
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        struct bma222_private_data *private_data =
+                        (struct bma222_private_data *)(pdata->private_data);
+
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return bma222_set_odr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return bma222_set_odr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return bma222_set_fsr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return bma222_set_fsr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result;
+        struct bma222_config *suspend_config =
+                &((struct bma222_private_data *)pdata->private_data)->suspend;
+
+        result = bma222_set_odr(mlsl_handle, pdata, suspend_config,
+                                true, suspend_config->odr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma222_set_fsr(mlsl_handle, pdata, suspend_config,
+                                true, suspend_config->fsr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA222_PWR_REG, BMA222_PWR_SLEEP_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        msleep(3); /* 3 ms powerup time maximum */
+        return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        struct bma222_config *resume_config =
+                &((struct bma222_private_data *)pdata->private_data)->resume;
+
+        /* Soft reset */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        BMA222_SOFTRESET_REG, BMA222_SOFTRESET_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(10);
+
+        result = bma222_set_odr(mlsl_handle, pdata, resume_config,
+                                true, resume_config->odr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma222_set_fsr(mlsl_handle, pdata, resume_config,
+                                true, resume_config->fsr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        int result = INV_SUCCESS;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                BMA222_STATUS_REG, 1, data);
+        if (data[0] & BMA222_STATUS_RDY_MASK) {
+                result = inv_serial_read(mlsl_handle, pdata->address,
+                                 slave->read_reg, slave->read_len, data);
+        return result;
+        } else
+                return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+static struct ext_slave_descr bma222_descr = {
+        .init             = bma222_init,
+        .exit             = bma222_exit,
+        .suspend          = bma222_suspend,
+        .resume           = bma222_resume,
+        .read             = bma222_read,
+        .config           = bma222_config,
+        .get_config       = bma222_get_config,
+        .name             = "bma222",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_BMA222,
+        .read_reg         = 0x02,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {2, 0},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *bma222_get_slave_descr(void)
+{
+        return &bma222_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+
+struct bma222_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int bma222_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct bma222_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        bma222_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int bma222_mod_remove(struct i2c_client *client)
+{
+        struct bma222_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                bma222_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id bma222_mod_id[] = {
+        { "bma222", ACCEL_ID_BMA222 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, bma222_mod_id);
+
+static struct i2c_driver bma222_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = bma222_mod_probe,
+        .remove = bma222_mod_remove,
+        .id_table = bma222_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "bma222_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init bma222_mod_init(void)
+{
+        int res = i2c_add_driver(&bma222_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "bma222_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit bma222_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&bma222_mod_driver);
+}
+
+module_init(bma222_mod_init);
+module_exit(bma222_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate BMA222 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("bma222_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/bma250.c b/drivers/misc/inv_mpu/accel/bma250.c
new file mode 100644
index 00000000000..6a245f4566a
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/bma250.c
@@ -0,0 +1,787 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   bma250.c
+ *      @brief  Accelerometer setup and handling methods for Bosch BMA250.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+
+/* -------------------------------------------------------------------------- */
+
+/* registers */
+#define BMA250_STATUS_REG       (0x0A)
+#define BMA250_FSR_REG          (0x0F)
+#define BMA250_ODR_REG          (0x10)
+#define BMA250_PWR_REG          (0x11)
+#define BMA250_SOFTRESET_REG    (0x14)
+#define BMA250_INT_TYPE_REG     (0x17)
+#define BMA250_INT_DST_REG      (0x1A)
+#define BMA250_INT_SRC_REG      (0x1E)
+
+/* masks */
+#define BMA250_STATUS_RDY_MASK  (0x80)
+#define BMA250_FSR_MASK         (0x0F)
+#define BMA250_ODR_MASK         (0x1F)
+#define BMA250_PWR_SLEEP_MASK   (0x80)
+#define BMA250_PWR_AWAKE_MASK   (0x00)
+#define BMA250_SOFTRESET_MASK   (0xB6)
+#define BMA250_INT_TYPE_MASK    (0x10)
+#define BMA250_INT_DST_1_MASK   (0x01)
+#define BMA250_INT_DST_2_MASK   (0x80)
+#define BMA250_INT_SRC_MASK     (0x00)
+
+/* -------------------------------------------------------------------------- */
+
+struct bma250_config {
+        unsigned int odr;               /** < output data rate in mHz */
+        unsigned int fsr;               /** < full scale range mg */
+        unsigned char irq_type;
+};
+
+struct bma250_private_data {
+        struct bma250_config suspend;   /** < suspend configuration */
+        struct bma250_config resume;    /** < resume configuration */
+};
+
+/* -------------------------------------------------------------------------- */
+/**
+ *  @brief Sets the IRQ to fire when one of the IRQ events occur.
+ *         Threshold and duration will not be used unless the type is MOT or
+ *         NMOT.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *              configuration to apply to, suspend or resume
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param irq_type
+ *              the type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_set_irq(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct bma250_config *config,
+                          int apply, long irq_type)
+{
+        int result = INV_SUCCESS;
+        unsigned char irqtype_reg;
+        unsigned char irqdst_reg;
+        unsigned char irqsrc_reg;
+
+        switch (irq_type) {
+        case MPU_SLAVE_IRQ_TYPE_DATA_READY:
+                /* data ready int. */
+                irqtype_reg = BMA250_INT_TYPE_MASK;
+                /* routed to interrupt pin 1 */
+                irqdst_reg = BMA250_INT_DST_1_MASK;
+                /* from filtered data */
+                irqsrc_reg = BMA250_INT_SRC_MASK;
+                break;
+        /* unfinished
+        case MPU_SLAVE_IRQ_TYPE_MOTION:
+                reg1 = 0x00;
+                reg2 = config->mot_int1_cfg;
+                reg3 = ;
+                break;
+        */
+        case MPU_SLAVE_IRQ_TYPE_NONE:
+                irqtype_reg = 0x00;
+                irqdst_reg = 0x00;
+                irqsrc_reg = 0x00;
+                break;
+        default:
+                return INV_ERROR_INVALID_PARAMETER;
+                break;
+        }
+
+        config->irq_type = (unsigned char)irq_type;
+
+        if (apply) {
+                /* select the type of interrupt to use */
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA250_INT_TYPE_REG, irqtype_reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                /* select to which interrupt pin to route it to */
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA250_INT_DST_REG, irqdst_reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                /* select whether the interrupt works off filtered or
+                   unfiltered data */
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA250_INT_SRC_REG, irqsrc_reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_set_odr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct bma250_config *config,
+                          int apply,
+                          long odr)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg_odr;
+
+        /* Table uses bandwidth which is half the sample rate */
+        odr = odr >> 1;
+        if (odr >= 1000000) {
+                reg_odr = 0x0F;
+                config->odr = 2000000;
+        } else if (odr >= 500000) {
+                reg_odr = 0x0E;
+                config->odr = 1000000;
+        } else if (odr >= 250000) {
+                reg_odr = 0x0D;
+                config->odr = 500000;
+        } else if (odr >= 125000) {
+                reg_odr = 0x0C;
+                config->odr = 250000;
+        } else if (odr >= 62500) {
+                reg_odr = 0x0B;
+                config->odr = 125000;
+        } else if (odr >= 31250) {
+                reg_odr = 0x0A;
+                config->odr = 62500;
+        } else if (odr >= 15630) {
+                reg_odr = 0x09;
+                config->odr = 31250;
+        } else {
+                reg_odr = 0x08;
+                config->odr = 15630;
+        }
+
+        if (apply) {
+                MPL_LOGV("ODR: %d\n", config->odr);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA250_ODR_REG, reg_odr);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_set_fsr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct bma250_config *config,
+                          int apply,
+                          long fsr)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg_fsr_mask;
+
+        if (fsr <= 2000) {
+                reg_fsr_mask = 0x03;
+                config->fsr = 2000;
+        } else if (fsr <= 4000) {
+                reg_fsr_mask = 0x05;
+                config->fsr = 4000;
+        } else if (fsr <= 8000) {
+                reg_fsr_mask = 0x08;
+                config->fsr = 8000;
+        } else { /* 8001 -> oo */
+                reg_fsr_mask = 0x0C;
+                config->fsr = 16000;
+        }
+
+        if (apply) {
+                MPL_LOGV("FSR: %d\n", config->fsr);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA250_FSR_REG, reg_fsr_mask);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_init(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        long range;
+
+        struct bma250_private_data *private_data;
+        private_data = kzalloc(sizeof(struct bma250_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        BMA250_SOFTRESET_REG, BMA250_SOFTRESET_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(1);
+
+        result = bma250_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                                false, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma250_set_odr(mlsl_handle, pdata, &private_data->resume,
+                                false, 200000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        range = range_fixedpoint_to_long_mg(slave->range);
+        result = bma250_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                                false, range);
+        result = bma250_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                                false, range);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = bma250_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                                false, MPU_SLAVE_IRQ_TYPE_NONE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma250_set_irq(mlsl_handle, pdata, &private_data->resume,
+                                false, MPU_SLAVE_IRQ_TYPE_NONE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA250_PWR_REG, BMA250_PWR_SLEEP_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_exit(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        struct bma250_private_data *private_data =
+                        (struct bma250_private_data *)(pdata->private_data);
+
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return bma250_set_odr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return bma250_set_odr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return bma250_set_fsr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return bma250_set_fsr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return bma250_set_irq(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply,
+                                      *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return bma250_set_irq(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply,
+                                      *((long *)data->data));
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_get_config(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata,
+                             struct ext_slave_config *data)
+{
+        struct bma250_private_data *private_data =
+                        (struct bma250_private_data *)(pdata->private_data);
+
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.irq_type;
+                break;
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result;
+        struct bma250_config *suspend_config =
+                &((struct bma250_private_data *)pdata->private_data)->suspend;
+
+        result = bma250_set_odr(mlsl_handle, pdata, suspend_config,
+                                true, suspend_config->odr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma250_set_fsr(mlsl_handle, pdata, suspend_config,
+                                true, suspend_config->fsr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma250_set_irq(mlsl_handle, pdata, suspend_config,
+                                true, suspend_config->irq_type);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                BMA250_PWR_REG, BMA250_PWR_SLEEP_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        msleep(3); /* 3 ms powerup time maximum */
+        return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        struct bma250_config *resume_config =
+                &((struct bma250_private_data *)pdata->private_data)->resume;
+
+        result = bma250_set_odr(mlsl_handle, pdata, resume_config,
+                                true, resume_config->odr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma250_set_fsr(mlsl_handle, pdata, resume_config,
+                                true, resume_config->fsr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = bma250_set_irq(mlsl_handle, pdata, resume_config,
+                                true, resume_config->irq_type);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        BMA250_PWR_REG, BMA250_PWR_AWAKE_MASK);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        int result = INV_SUCCESS;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                BMA250_STATUS_REG, 1, data);
+        if (1) { /* KLP - workaroud for small data ready window */
+                result = inv_serial_read(mlsl_handle, pdata->address,
+                                slave->read_reg, slave->read_len, data);
+                return result;
+        } else
+                return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+static struct ext_slave_descr bma250_descr = {
+        .init             = bma250_init,
+        .exit             = bma250_exit,
+        .suspend          = bma250_suspend,
+        .resume           = bma250_resume,
+        .read             = bma250_read,
+        .config           = bma250_config,
+        .get_config       = bma250_get_config,
+        .name             = "bma250",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_BMA250,
+        .read_reg         = 0x02,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {2, 0},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *bma250_get_slave_descr(void)
+{
+        return &bma250_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+
+/* Platform data for the MPU */
+struct bma250_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int bma250_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct bma250_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        bma250_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int bma250_mod_remove(struct i2c_client *client)
+{
+        struct bma250_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                bma250_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id bma250_mod_id[] = {
+        { "bma250", ACCEL_ID_BMA250 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, bma250_mod_id);
+
+static struct i2c_driver bma250_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = bma250_mod_probe,
+        .remove = bma250_mod_remove,
+        .id_table = bma250_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "bma250_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init bma250_mod_init(void)
+{
+        int res = i2c_add_driver(&bma250_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "bma250_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit bma250_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&bma250_mod_driver);
+}
+
+module_init(bma250_mod_init);
+module_exit(bma250_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate BMA250 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("bma250_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/cma3000.c b/drivers/misc/inv_mpu/accel/cma3000.c
new file mode 100644
index 00000000000..496d1f29bdc
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/cma3000.c
@@ -0,0 +1,222 @@
+/*
+        $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 ACCELDL
+ *  @brief      Accelerometer setup and handling methods for VTI CMA3000.
+ *
+ *  @{
+ *      @file   cma3000.c
+ *      @brief  Accelerometer setup and handling methods for VTI CMA3000
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+static int cma3000_suspend(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata)
+{
+        int result;
+        /* RAM reset */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address, 0x1d, 0xcd);
+        return result;
+}
+
+static int cma3000_resume(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+
+        return INV_SUCCESS;
+}
+
+static int cma3000_read(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        unsigned char *data)
+{
+        int result;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 slave->reg, slave->len, data);
+        return result;
+}
+
+static struct ext_slave_descr cma3000_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = cma3000_suspend,
+        .resume           = cma3000_resume,
+        .read             = cma3000_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "cma3000",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ID_INVALID,
+        .read_reg         = 0x06,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {2, 0},
+        .trigger          = NULL,
+
+};
+
+static
+struct ext_slave_descr *cma3000_get_slave_descr(void)
+{
+        return &cma3000_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+
+struct cma3000_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int cma3000_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct cma3000_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        cma3000_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int cma3000_mod_remove(struct i2c_client *client)
+{
+        struct cma3000_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                cma3000_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id cma3000_mod_id[] = {
+        { "cma3000", ACCEL_ID_CMA3000 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, cma3000_mod_id);
+
+static struct i2c_driver cma3000_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = cma3000_mod_probe,
+        .remove = cma3000_mod_remove,
+        .id_table = cma3000_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "cma3000_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init cma3000_mod_init(void)
+{
+        int res = i2c_add_driver(&cma3000_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "cma3000_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit cma3000_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&cma3000_mod_driver);
+}
+
+module_init(cma3000_mod_init);
+module_exit(cma3000_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate CMA3000 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("cma3000_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/kxsd9.c b/drivers/misc/inv_mpu/accel/kxsd9.c
new file mode 100644
index 00000000000..5cb4eaf6b4a
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/kxsd9.c
@@ -0,0 +1,264 @@
+/*
+        $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 ACCELDL
+ *  @brief      Accelerometer setup and handling methods for Kionix KXSD9.
+ *
+ *  @{
+ *      @file   kxsd9.c
+ *      @brief  Accelerometer setup and handling methods for Kionix KXSD9.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+static int kxsd9_suspend(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        /* CTRL_REGB: low-power standby mode */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address, 0x0d, 0x0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+/* full scale setting - register and mask */
+#define ACCEL_KIONIX_CTRL_REG      (0x0C)
+#define ACCEL_KIONIX_CTRL_MASK     (0x3)
+
+static int kxsd9_resume(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg;
+
+        /* Full Scale */
+        reg = 0x0;
+        reg &= ~ACCEL_KIONIX_CTRL_MASK;
+        reg |= 0x00;
+        if (slave->range.mantissa == 4) {               /* 4g scale = 4.9951 */
+                reg |= 0x2;
+                slave->range.fraction = 9951;
+        } else if (slave->range.mantissa == 7) {        /* 6g scale = 7.5018 */
+                reg |= 0x1;
+                slave->range.fraction = 5018;
+        } else if (slave->range.mantissa == 9) {        /* 8g scale = 9.9902 */
+                reg |= 0x0;
+                slave->range.fraction = 9902;
+        } else {
+                slave->range.mantissa = 2;              /* 2g scale = 2.5006 */
+                slave->range.fraction = 5006;
+                reg |= 0x3;
+        }
+        reg |= 0xC0;            /* 100Hz LPF */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    ACCEL_KIONIX_CTRL_REG, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* normal operation */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address, 0x0d, 0x40);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return INV_SUCCESS;
+}
+
+static int kxsd9_read(void *mlsl_handle,
+                      struct ext_slave_descr *slave,
+                      struct ext_slave_platform_data *pdata,
+                      unsigned char *data)
+{
+        int result;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 slave->read_reg, slave->read_len, data);
+        return result;
+}
+
+static struct ext_slave_descr kxsd9_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = kxsd9_suspend,
+        .resume           = kxsd9_resume,
+        .read             = kxsd9_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "kxsd9",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_KXSD9,
+        .read_reg         = 0x00,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {2, 5006},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *kxsd9_get_slave_descr(void)
+{
+        return &kxsd9_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct kxsd9_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int kxsd9_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct kxsd9_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        kxsd9_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int kxsd9_mod_remove(struct i2c_client *client)
+{
+        struct kxsd9_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                kxsd9_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id kxsd9_mod_id[] = {
+        { "kxsd9", ACCEL_ID_KXSD9 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, kxsd9_mod_id);
+
+static struct i2c_driver kxsd9_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = kxsd9_mod_probe,
+        .remove = kxsd9_mod_remove,
+        .id_table = kxsd9_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "kxsd9_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init kxsd9_mod_init(void)
+{
+        int res = i2c_add_driver(&kxsd9_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "kxsd9_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit kxsd9_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&kxsd9_mod_driver);
+}
+
+module_init(kxsd9_mod_init);
+module_exit(kxsd9_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate KXSD9 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("kxsd9_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/kxtf9.c b/drivers/misc/inv_mpu/accel/kxtf9.c
new file mode 100644
index 00000000000..80776f249c6
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/kxtf9.c
@@ -0,0 +1,841 @@
+/*
+        $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 ACCELDL
+ *  @brief      Accelerometer setup and handling methods for Kionix KXTF9.
+ *
+ *  @{
+ *      @file   kxtf9.c
+ *      @brief  Accelerometer setup and handling methods for Kionix KXTF9.
+*/
+
+/* -------------------------------------------------------------------------- */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+#define KXTF9_XOUT_HPF_L                (0x00)  /* 0000 0000 */
+#define KXTF9_XOUT_HPF_H                (0x01)  /* 0000 0001 */
+#define KXTF9_YOUT_HPF_L                (0x02)  /* 0000 0010 */
+#define KXTF9_YOUT_HPF_H                (0x03)  /* 0000 0011 */
+#define KXTF9_ZOUT_HPF_L                (0x04)  /* 0001 0100 */
+#define KXTF9_ZOUT_HPF_H                (0x05)  /* 0001 0101 */
+#define KXTF9_XOUT_L                    (0x06)  /* 0000 0110 */
+#define KXTF9_XOUT_H                    (0x07)  /* 0000 0111 */
+#define KXTF9_YOUT_L                    (0x08)  /* 0000 1000 */
+#define KXTF9_YOUT_H                    (0x09)  /* 0000 1001 */
+#define KXTF9_ZOUT_L                    (0x0A)  /* 0001 1010 */
+#define KXTF9_ZOUT_H                    (0x0B)  /* 0001 1011 */
+#define KXTF9_ST_RESP                   (0x0C)  /* 0000 1100 */
+#define KXTF9_WHO_AM_I                  (0x0F)  /* 0000 1111 */
+#define KXTF9_TILT_POS_CUR              (0x10)  /* 0001 0000 */
+#define KXTF9_TILT_POS_PRE              (0x11)  /* 0001 0001 */
+#define KXTF9_INT_SRC_REG1              (0x15)  /* 0001 0101 */
+#define KXTF9_INT_SRC_REG2              (0x16)  /* 0001 0110 */
+#define KXTF9_STATUS_REG                (0x18)  /* 0001 1000 */
+#define KXTF9_INT_REL                   (0x1A)  /* 0001 1010 */
+#define KXTF9_CTRL_REG1                 (0x1B)  /* 0001 1011 */
+#define KXTF9_CTRL_REG2                 (0x1C)  /* 0001 1100 */
+#define KXTF9_CTRL_REG3                 (0x1D)  /* 0001 1101 */
+#define KXTF9_INT_CTRL_REG1             (0x1E)  /* 0001 1110 */
+#define KXTF9_INT_CTRL_REG2             (0x1F)  /* 0001 1111 */
+#define KXTF9_INT_CTRL_REG3             (0x20)  /* 0010 0000 */
+#define KXTF9_DATA_CTRL_REG             (0x21)  /* 0010 0001 */
+#define KXTF9_TILT_TIMER                (0x28)  /* 0010 1000 */
+#define KXTF9_WUF_TIMER                 (0x29)  /* 0010 1001 */
+#define KXTF9_TDT_TIMER                 (0x2B)  /* 0010 1011 */
+#define KXTF9_TDT_H_THRESH              (0x2C)  /* 0010 1100 */
+#define KXTF9_TDT_L_THRESH              (0x2D)  /* 0010 1101 */
+#define KXTF9_TDT_TAP_TIMER             (0x2E)  /* 0010 1110 */
+#define KXTF9_TDT_TOTAL_TIMER           (0x2F)  /* 0010 1111 */
+#define KXTF9_TDT_LATENCY_TIMER         (0x30)  /* 0011 0000 */
+#define KXTF9_TDT_WINDOW_TIMER          (0x31)  /* 0011 0001 */
+#define KXTF9_WUF_THRESH                (0x5A)  /* 0101 1010 */
+#define KXTF9_TILT_ANGLE                (0x5C)  /* 0101 1100 */
+#define KXTF9_HYST_SET                  (0x5F)  /* 0101 1111 */
+
+#define KXTF9_MAX_DUR (0xFF)
+#define KXTF9_MAX_THS (0xFF)
+#define KXTF9_THS_COUNTS_P_G (32)
+
+/* -------------------------------------------------------------------------- */
+
+struct kxtf9_config {
+        unsigned long odr;      /* Output data rate mHz */
+        unsigned int fsr;       /* full scale range mg */
+        unsigned int ths;       /* Motion no-motion thseshold mg */
+        unsigned int dur;       /* Motion no-motion duration ms */
+        unsigned int irq_type;
+        unsigned char reg_ths;
+        unsigned char reg_dur;
+        unsigned char reg_odr;
+        unsigned char reg_int_cfg1;
+        unsigned char reg_int_cfg2;
+        unsigned char ctrl_reg1;
+};
+
+struct kxtf9_private_data {
+        struct kxtf9_config suspend;
+        struct kxtf9_config resume;
+};
+
+static int kxtf9_set_ths(void *mlsl_handle,
+                         struct ext_slave_platform_data *pdata,
+                         struct kxtf9_config *config, int apply, long ths)
+{
+        int result = INV_SUCCESS;
+        if ((ths * KXTF9_THS_COUNTS_P_G / 1000) > KXTF9_MAX_THS)
+                ths = (long)(KXTF9_MAX_THS * 1000) / KXTF9_THS_COUNTS_P_G;
+
+        if (ths < 0)
+                ths = 0;
+
+        config->ths = ths;
+        config->reg_ths = (unsigned char)
+            ((long)(ths * KXTF9_THS_COUNTS_P_G) / 1000);
+        MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_WUF_THRESH,
+                                                 config->reg_ths);
+        return result;
+}
+
+static int kxtf9_set_dur(void *mlsl_handle,
+                         struct ext_slave_platform_data *pdata,
+                         struct kxtf9_config *config, int apply, long dur)
+{
+        int result = INV_SUCCESS;
+        long reg_dur = (dur * config->odr) / 1000000L;
+        config->dur = dur;
+
+        if (reg_dur > KXTF9_MAX_DUR)
+                reg_dur = KXTF9_MAX_DUR;
+
+        config->reg_dur = (unsigned char)reg_dur;
+        MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_WUF_TIMER,
+                                                 (unsigned char)reg_dur);
+        return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int kxtf9_set_irq(void *mlsl_handle,
+                         struct ext_slave_platform_data *pdata,
+                         struct kxtf9_config *config, int apply, long irq_type)
+{
+        int result = INV_SUCCESS;
+        struct kxtf9_private_data *private_data = pdata->private_data;
+
+        config->irq_type = (unsigned char)irq_type;
+        config->ctrl_reg1 &= ~0x22;
+        if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                config->ctrl_reg1 |= 0x20;
+                config->reg_int_cfg1 = 0x38;
+                config->reg_int_cfg2 = 0x00;
+        } else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+                config->ctrl_reg1 |= 0x02;
+                if ((unsigned long)config ==
+                    (unsigned long)&private_data->suspend)
+                        config->reg_int_cfg1 = 0x34;
+                else
+                        config->reg_int_cfg1 = 0x24;
+                config->reg_int_cfg2 = 0xE0;
+        } else {
+                config->reg_int_cfg1 = 0x00;
+                config->reg_int_cfg2 = 0x00;
+        }
+
+        if (apply) {
+                /* Must clear bit 7 before writing new configuration */
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_CTRL_REG1, 0x40);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_INT_CTRL_REG1,
+                                                 config->reg_int_cfg1);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_INT_CTRL_REG2,
+                                                 config->reg_int_cfg2);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_CTRL_REG1,
+                                                 config->ctrl_reg1);
+        }
+        MPL_LOGV("CTRL_REG1: %lx, INT_CFG1: %lx, INT_CFG2: %lx\n",
+                 (unsigned long)config->ctrl_reg1,
+                 (unsigned long)config->reg_int_cfg1,
+                 (unsigned long)config->reg_int_cfg2);
+
+        return result;
+}
+
+/**
+ * Set the Output data rate for the particular configuration
+ *
+ * @param config Config to modify with new ODR
+ * @param odr Output data rate in units of 1/1000Hz
+ */
+static int kxtf9_set_odr(void *mlsl_handle,
+                         struct ext_slave_platform_data *pdata,
+                         struct kxtf9_config *config, int apply, long odr)
+{
+        unsigned char bits;
+        int result = INV_SUCCESS;
+
+        /* Data sheet says there is 12.5 hz, but that seems to produce a single
+         * correct data value, thus we remove it from the table */
+        if (odr > 400000L) {
+                config->odr = 800000L;
+                bits = 0x06;
+        } else if (odr > 200000L) {
+                config->odr = 400000L;
+                bits = 0x05;
+        } else if (odr > 100000L) {
+                config->odr = 200000L;
+                bits = 0x04;
+        } else if (odr > 50000) {
+                config->odr = 100000L;
+                bits = 0x03;
+        } else if (odr > 25000) {
+                config->odr = 50000;
+                bits = 0x02;
+        } else if (odr != 0) {
+                config->odr = 25000;
+                bits = 0x01;
+        } else {
+                config->odr = 0;
+                bits = 0;
+        }
+
+        if (odr != 0)
+                config->ctrl_reg1 |= 0x80;
+        else
+                config->ctrl_reg1 &= ~0x80;
+
+        config->reg_odr = bits;
+        kxtf9_set_dur(mlsl_handle, pdata, config, apply, config->dur);
+        MPL_LOGV("ODR: %ld, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_DATA_CTRL_REG,
+                                                 config->reg_odr);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_CTRL_REG1, 0x40);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_CTRL_REG1,
+                                                 config->ctrl_reg1);
+        }
+        return result;
+}
+
+/**
+ * Set the full scale range of the accels
+ *
+ * @param config pointer to configuration
+ * @param fsr requested full scale range
+ */
+static int kxtf9_set_fsr(void *mlsl_handle,
+                         struct ext_slave_platform_data *pdata,
+                         struct kxtf9_config *config, int apply, long fsr)
+{
+        int result = INV_SUCCESS;
+
+        config->ctrl_reg1 = (config->ctrl_reg1 & 0xE7);
+        if (fsr <= 2000) {
+                config->fsr = 2000;
+                config->ctrl_reg1 |= 0x00;
+        } else if (fsr <= 4000) {
+                config->fsr = 4000;
+                config->ctrl_reg1 |= 0x08;
+        } else {
+                config->fsr = 8000;
+                config->ctrl_reg1 |= 0x10;
+        }
+
+        MPL_LOGV("FSR: %d\n", config->fsr);
+        if (apply) {
+                /* Must clear bit 7 before writing new configuration */
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_CTRL_REG1, 0x40);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 KXTF9_CTRL_REG1,
+                                                 config->ctrl_reg1);
+        }
+        return result;
+}
+
+static int kxtf9_suspend(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char data;
+        struct kxtf9_private_data *private_data = pdata->private_data;
+
+        /* Wake up */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_CTRL_REG1, 0x40);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* INT_CTRL_REG1: */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_INT_CTRL_REG1,
+                                         private_data->suspend.reg_int_cfg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* WUF_THRESH: */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_WUF_THRESH,
+                                         private_data->suspend.reg_ths);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* DATA_CTRL_REG */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_DATA_CTRL_REG,
+                                         private_data->suspend.reg_odr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* WUF_TIMER */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_WUF_TIMER,
+                                         private_data->suspend.reg_dur);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Normal operation  */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_CTRL_REG1,
+                                         private_data->suspend.ctrl_reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 KXTF9_INT_REL, 1, &data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/* full scale setting - register and mask */
+#define ACCEL_KIONIX_CTRL_REG      (0x1b)
+#define ACCEL_KIONIX_CTRL_MASK     (0x18)
+
+static int kxtf9_resume(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        unsigned char data;
+        struct kxtf9_private_data *private_data = pdata->private_data;
+
+        /* Wake up */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_CTRL_REG1, 0x40);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* INT_CTRL_REG1: */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_INT_CTRL_REG1,
+                                         private_data->resume.reg_int_cfg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* WUF_THRESH: */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_WUF_THRESH,
+                                         private_data->resume.reg_ths);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* DATA_CTRL_REG */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_DATA_CTRL_REG,
+                                         private_data->resume.reg_odr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* WUF_TIMER */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_WUF_TIMER,
+                                         private_data->resume.reg_dur);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Normal operation  */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_CTRL_REG1,
+                                         private_data->resume.ctrl_reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 KXTF9_INT_REL, 1, &data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return INV_SUCCESS;
+}
+
+static int kxtf9_init(void *mlsl_handle,
+                      struct ext_slave_descr *slave,
+                      struct ext_slave_platform_data *pdata)
+{
+
+        struct kxtf9_private_data *private_data;
+        int result = INV_SUCCESS;
+
+        private_data = (struct kxtf9_private_data *)
+            kzalloc(sizeof(struct kxtf9_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        /* RAM reset */
+        /* Fastest Reset */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_CTRL_REG1, 0x40);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Fastest Reset */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_DATA_CTRL_REG, 0x36);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Reset */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         KXTF9_CTRL_REG3, 0xcd);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(2);
+
+        pdata->private_data = private_data;
+
+        private_data->resume.ctrl_reg1 = 0xC0;
+        private_data->suspend.ctrl_reg1 = 0x40;
+
+        result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->suspend,
+                               false, 1000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->resume,
+                               false, 2540);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                               false, 50000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->resume,
+                               false, 200000L);
+
+        result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                               false, 2000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                               false, 2000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->suspend,
+                               false, 80);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->resume,
+                               false, 40);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                               false, MPU_SLAVE_IRQ_TYPE_NONE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->resume,
+                               false, MPU_SLAVE_IRQ_TYPE_NONE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int kxtf9_exit(void *mlsl_handle,
+                      struct ext_slave_descr *slave,
+                      struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+static int kxtf9_config(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_config *data)
+{
+        struct kxtf9_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return kxtf9_set_odr(mlsl_handle, pdata,
+                                     &private_data->suspend,
+                                     data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return kxtf9_set_odr(mlsl_handle, pdata,
+                                     &private_data->resume,
+                                     data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return kxtf9_set_fsr(mlsl_handle, pdata,
+                                     &private_data->suspend,
+                                     data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return kxtf9_set_fsr(mlsl_handle, pdata,
+                                     &private_data->resume,
+                                     data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                return kxtf9_set_ths(mlsl_handle, pdata,
+                                     &private_data->suspend,
+                                     data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                return kxtf9_set_ths(mlsl_handle, pdata,
+                                     &private_data->resume,
+                                     data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                return kxtf9_set_dur(mlsl_handle, pdata,
+                                     &private_data->suspend,
+                                     data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                return kxtf9_set_dur(mlsl_handle, pdata,
+                                     &private_data->resume,
+                                     data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return kxtf9_set_irq(mlsl_handle, pdata,
+                                     &private_data->suspend,
+                                     data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return kxtf9_set_irq(mlsl_handle, pdata,
+                                     &private_data->resume,
+                                     data->apply, *((long *)data->data));
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static int kxtf9_get_config(void *mlsl_handle,
+                            struct ext_slave_descr *slave,
+                            struct ext_slave_platform_data *pdata,
+                            struct ext_slave_config *data)
+{
+        struct kxtf9_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.ths;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.ths;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.dur;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.dur;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.irq_type;
+                break;
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static int kxtf9_read(void *mlsl_handle,
+                      struct ext_slave_descr *slave,
+                      struct ext_slave_platform_data *pdata,
+                      unsigned char *data)
+{
+        int result;
+        unsigned char reg;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 KXTF9_INT_SRC_REG2, 1, &reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        if (!(reg & 0x10))
+                return INV_ERROR_ACCEL_DATA_NOT_READY;
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 slave->read_reg, slave->read_len, data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static struct ext_slave_descr kxtf9_descr = {
+        .init             = kxtf9_init,
+        .exit             = kxtf9_exit,
+        .suspend          = kxtf9_suspend,
+        .resume           = kxtf9_resume,
+        .read             = kxtf9_read,
+        .config           = kxtf9_config,
+        .get_config       = kxtf9_get_config,
+        .name             = "kxtf9",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_KXTF9,
+        .read_reg         = 0x06,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {2, 0},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *kxtf9_get_slave_descr(void)
+{
+        return &kxtf9_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct kxtf9_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int kxtf9_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct kxtf9_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        kxtf9_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int kxtf9_mod_remove(struct i2c_client *client)
+{
+        struct kxtf9_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                kxtf9_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id kxtf9_mod_id[] = {
+        { "kxtf9", ACCEL_ID_KXTF9 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, kxtf9_mod_id);
+
+static struct i2c_driver kxtf9_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = kxtf9_mod_probe,
+        .remove = kxtf9_mod_remove,
+        .id_table = kxtf9_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "kxtf9_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init kxtf9_mod_init(void)
+{
+        int res = i2c_add_driver(&kxtf9_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "kxtf9_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit kxtf9_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&kxtf9_mod_driver);
+}
+
+module_init(kxtf9_mod_init);
+module_exit(kxtf9_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate KXTF9 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("kxtf9_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/lis331.c b/drivers/misc/inv_mpu/accel/lis331.c
new file mode 100644
index 00000000000..bcbec252af9
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/lis331.c
@@ -0,0 +1,745 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   lis331.c
+ *      @brief  Accelerometer setup and handling methods for ST LIS331DLH.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* full scale setting - register & mask */
+#define LIS331DLH_CTRL_REG1         (0x20)
+#define LIS331DLH_CTRL_REG2         (0x21)
+#define LIS331DLH_CTRL_REG3         (0x22)
+#define LIS331DLH_CTRL_REG4         (0x23)
+#define LIS331DLH_CTRL_REG5         (0x24)
+#define LIS331DLH_HP_FILTER_RESET   (0x25)
+#define LIS331DLH_REFERENCE         (0x26)
+#define LIS331DLH_STATUS_REG        (0x27)
+#define LIS331DLH_OUT_X_L           (0x28)
+#define LIS331DLH_OUT_X_H           (0x29)
+#define LIS331DLH_OUT_Y_L           (0x2a)
+#define LIS331DLH_OUT_Y_H           (0x2b)
+#define LIS331DLH_OUT_Z_L           (0x2b)
+#define LIS331DLH_OUT_Z_H           (0x2d)
+
+#define LIS331DLH_INT1_CFG          (0x30)
+#define LIS331DLH_INT1_SRC          (0x31)
+#define LIS331DLH_INT1_THS          (0x32)
+#define LIS331DLH_INT1_DURATION     (0x33)
+
+#define LIS331DLH_INT2_CFG          (0x34)
+#define LIS331DLH_INT2_SRC          (0x35)
+#define LIS331DLH_INT2_THS          (0x36)
+#define LIS331DLH_INT2_DURATION     (0x37)
+
+/* CTRL_REG1 */
+#define LIS331DLH_CTRL_MASK         (0x30)
+#define LIS331DLH_SLEEP_MASK        (0x20)
+#define LIS331DLH_PWR_MODE_NORMAL   (0x20)
+
+#define LIS331DLH_MAX_DUR           (0x7F)
+
+
+/* -------------------------------------------------------------------------- */
+
+struct lis331dlh_config {
+        unsigned int odr;
+        unsigned int fsr;       /* full scale range mg */
+        unsigned int ths;       /* Motion no-motion thseshold mg */
+        unsigned int dur;       /* Motion no-motion duration ms */
+        unsigned char reg_ths;
+        unsigned char reg_dur;
+        unsigned char ctrl_reg1;
+        unsigned char irq_type;
+        unsigned char mot_int1_cfg;
+};
+
+struct lis331dlh_private_data {
+        struct lis331dlh_config suspend;
+        struct lis331dlh_config resume;
+};
+
+/* -------------------------------------------------------------------------- */
+static int lis331dlh_set_ths(void *mlsl_handle,
+                             struct ext_slave_platform_data *pdata,
+                             struct lis331dlh_config *config,
+                             int apply, long ths)
+{
+        int result = INV_SUCCESS;
+        if ((unsigned int)ths >= config->fsr)
+                ths = (long)config->fsr - 1;
+
+        if (ths < 0)
+                ths = 0;
+
+        config->ths = ths;
+        config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr));
+        MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS331DLH_INT1_THS,
+                                                 config->reg_ths);
+        return result;
+}
+
+static int lis331dlh_set_dur(void *mlsl_handle,
+                             struct ext_slave_platform_data *pdata,
+                             struct lis331dlh_config *config,
+                             int apply, long dur)
+{
+        int result = INV_SUCCESS;
+        long reg_dur = (dur * config->odr) / 1000000L;
+        config->dur = dur;
+
+        if (reg_dur > LIS331DLH_MAX_DUR)
+                reg_dur = LIS331DLH_MAX_DUR;
+
+        config->reg_dur = (unsigned char)reg_dur;
+        MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS331DLH_INT1_DURATION,
+                                                 (unsigned char)reg_dur);
+        return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int lis331dlh_set_irq(void *mlsl_handle,
+                             struct ext_slave_platform_data *pdata,
+                             struct lis331dlh_config *config,
+                             int apply, long irq_type)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+
+        config->irq_type = (unsigned char)irq_type;
+        if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x02;
+                reg2 = 0x00;
+        } else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+                reg1 = 0x00;
+                reg2 = config->mot_int1_cfg;
+        } else {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        }
+
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS331DLH_CTRL_REG3, reg1);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS331DLH_INT1_CFG, reg2);
+        }
+
+        return result;
+}
+
+/**
+ * Set the Output data rate for the particular configuration
+ *
+ * @param config Config to modify with new ODR
+ * @param odr Output data rate in units of 1/1000Hz
+ */
+static int lis331dlh_set_odr(void *mlsl_handle,
+                             struct ext_slave_platform_data *pdata,
+                             struct lis331dlh_config *config,
+                             int apply, long odr)
+{
+        unsigned char bits;
+        int result = INV_SUCCESS;
+
+        /* normal power modes */
+        if (odr > 400000) {
+                config->odr = 1000000;
+                bits = LIS331DLH_PWR_MODE_NORMAL | 0x18;
+        } else if (odr > 100000) {
+                config->odr = 400000;
+                bits = LIS331DLH_PWR_MODE_NORMAL | 0x10;
+        } else if (odr > 50000) {
+                config->odr = 100000;
+                bits = LIS331DLH_PWR_MODE_NORMAL | 0x08;
+        } else if (odr > 10000) {
+                config->odr = 50000;
+                bits = LIS331DLH_PWR_MODE_NORMAL | 0x00;
+        /* low power modes */
+        } else if (odr > 5000) {
+                config->odr = 10000;
+                bits = 0xC0;
+        } else if (odr > 2000) {
+                config->odr = 5000;
+                bits = 0xA0;
+        } else if (odr > 1000) {
+                config->odr = 2000;
+                bits = 0x80;
+        } else if (odr > 500) {
+                config->odr = 1000;
+                bits = 0x60;
+        } else if (odr > 0) {
+                config->odr = 500;
+                bits = 0x40;
+        } else {
+                config->odr = 0;
+                bits = 0;
+        }
+
+        config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x7);
+        lis331dlh_set_dur(mlsl_handle, pdata, config, apply, config->dur);
+        MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS331DLH_CTRL_REG1,
+                                                 config->ctrl_reg1);
+        return result;
+}
+
+/**
+ * Set the full scale range of the accels
+ *
+ * @param config pointer to configuration
+ * @param fsr requested full scale range
+ */
+static int lis331dlh_set_fsr(void *mlsl_handle,
+                             struct ext_slave_platform_data *pdata,
+                             struct lis331dlh_config *config,
+                             int apply, long fsr)
+{
+        unsigned char reg1 = 0x40;
+        int result = INV_SUCCESS;
+
+        if (fsr <= 2048) {
+                config->fsr = 2048;
+        } else if (fsr <= 4096) {
+                reg1 |= 0x30;
+                config->fsr = 4096;
+        } else {
+                reg1 |= 0x10;
+                config->fsr = 8192;
+        }
+
+        lis331dlh_set_ths(mlsl_handle, pdata, config, apply, config->ths);
+        MPL_LOGV("FSR: %d\n", config->fsr);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS331DLH_CTRL_REG4, reg1);
+
+        return result;
+}
+
+static int lis331dlh_suspend(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+        struct lis331dlh_private_data *private_data =
+                (struct lis331dlh_private_data *)(pdata->private_data);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_CTRL_REG1,
+                                         private_data->suspend.ctrl_reg1);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_CTRL_REG2, 0x0f);
+        reg1 = 0x40;
+        if (private_data->suspend.fsr == 8192)
+                reg1 |= 0x30;
+        else if (private_data->suspend.fsr == 4096)
+                reg1 |= 0x10;
+        /* else bits [4..5] are already zero */
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_CTRL_REG4, reg1);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_INT1_THS,
+                                         private_data->suspend.reg_ths);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_INT1_DURATION,
+                                         private_data->suspend.reg_dur);
+
+        if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x02;
+                reg2 = 0x00;
+        } else if (private_data->suspend.irq_type ==
+                   MPU_SLAVE_IRQ_TYPE_MOTION) {
+                reg1 = 0x00;
+                reg2 = private_data->suspend.mot_int1_cfg;
+        } else {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_CTRL_REG3, reg1);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_INT1_CFG, reg2);
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 LIS331DLH_HP_FILTER_RESET, 1, &reg1);
+        return result;
+}
+
+static int lis331dlh_resume(void *mlsl_handle,
+                            struct ext_slave_descr *slave,
+                            struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+        struct lis331dlh_private_data *private_data =
+                (struct lis331dlh_private_data *)(pdata->private_data);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_CTRL_REG1,
+                                         private_data->resume.ctrl_reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(6);
+
+        /* Full Scale */
+        reg1 = 0x40;
+        if (private_data->resume.fsr == 8192)
+                reg1 |= 0x30;
+        else if (private_data->resume.fsr == 4096)
+                reg1 |= 0x10;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_CTRL_REG4, reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Configure high pass filter */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_CTRL_REG2, 0x0F);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x02;
+                reg2 = 0x00;
+        } else if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+                reg1 = 0x00;
+                reg2 = private_data->resume.mot_int1_cfg;
+        } else {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_CTRL_REG3, reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_INT1_THS,
+                                         private_data->resume.reg_ths);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_INT1_DURATION,
+                                         private_data->resume.reg_dur);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS331DLH_INT1_CFG, reg2);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 LIS331DLH_HP_FILTER_RESET, 1, &reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int lis331dlh_read(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata,
+                          unsigned char *data)
+{
+        int result = INV_SUCCESS;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 LIS331DLH_STATUS_REG, 1, data);
+        if (data[0] & 0x0F) {
+                result = inv_serial_read(mlsl_handle, pdata->address,
+                                         slave->read_reg, slave->read_len,
+                                         data);
+                return result;
+        } else
+                return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+static int lis331dlh_init(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        struct lis331dlh_private_data *private_data;
+        long range;
+        private_data = (struct lis331dlh_private_data *)
+            kzalloc(sizeof(struct lis331dlh_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        private_data->resume.ctrl_reg1 = 0x37;
+        private_data->suspend.ctrl_reg1 = 0x47;
+        private_data->resume.mot_int1_cfg = 0x95;
+        private_data->suspend.mot_int1_cfg = 0x2a;
+
+        lis331dlh_set_odr(mlsl_handle, pdata, &private_data->suspend, false, 0);
+        lis331dlh_set_odr(mlsl_handle, pdata, &private_data->resume,
+                          false, 200000);
+
+        range = range_fixedpoint_to_long_mg(slave->range);
+        lis331dlh_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                        false, range);
+        lis331dlh_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                        false, range);
+
+        lis331dlh_set_ths(mlsl_handle, pdata, &private_data->suspend,
+                          false, 80);
+        lis331dlh_set_ths(mlsl_handle, pdata, &private_data->resume, false, 40);
+
+
+        lis331dlh_set_dur(mlsl_handle, pdata, &private_data->suspend,
+                          false, 1000);
+        lis331dlh_set_dur(mlsl_handle, pdata, &private_data->resume,
+                          false, 2540);
+
+        lis331dlh_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                          false, MPU_SLAVE_IRQ_TYPE_NONE);
+        lis331dlh_set_irq(mlsl_handle, pdata, &private_data->resume,
+                          false, MPU_SLAVE_IRQ_TYPE_NONE);
+        return INV_SUCCESS;
+}
+
+static int lis331dlh_exit(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+static int lis331dlh_config(void *mlsl_handle,
+                            struct ext_slave_descr *slave,
+                            struct ext_slave_platform_data *pdata,
+                            struct ext_slave_config *data)
+{
+        struct lis331dlh_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return lis331dlh_set_odr(mlsl_handle, pdata,
+                                         &private_data->suspend,
+                                         data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return lis331dlh_set_odr(mlsl_handle, pdata,
+                                         &private_data->resume,
+                                         data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return lis331dlh_set_fsr(mlsl_handle, pdata,
+                                         &private_data->suspend,
+                                         data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return lis331dlh_set_fsr(mlsl_handle, pdata,
+                                         &private_data->resume,
+                                         data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                return lis331dlh_set_ths(mlsl_handle, pdata,
+                                         &private_data->suspend,
+                                         data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                return lis331dlh_set_ths(mlsl_handle, pdata,
+                                         &private_data->resume,
+                                         data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                return lis331dlh_set_dur(mlsl_handle, pdata,
+                                         &private_data->suspend,
+                                         data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                return lis331dlh_set_dur(mlsl_handle, pdata,
+                                         &private_data->resume,
+                                         data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return lis331dlh_set_irq(mlsl_handle, pdata,
+                                         &private_data->suspend,
+                                         data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return lis331dlh_set_irq(mlsl_handle, pdata,
+                                         &private_data->resume,
+                                         data->apply, *((long *)data->data));
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static int lis331dlh_get_config(void *mlsl_handle,
+                                struct ext_slave_descr *slave,
+                                struct ext_slave_platform_data *pdata,
+                                struct ext_slave_config *data)
+{
+        struct lis331dlh_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.ths;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.ths;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.dur;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.dur;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.irq_type;
+                break;
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_descr lis331dlh_descr = {
+        .init             = lis331dlh_init,
+        .exit             = lis331dlh_exit,
+        .suspend          = lis331dlh_suspend,
+        .resume           = lis331dlh_resume,
+        .read             = lis331dlh_read,
+        .config           = lis331dlh_config,
+        .get_config       = lis331dlh_get_config,
+        .name             = "lis331dlh",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_LIS331,
+        .read_reg         = (0x28 | 0x80), /* 0x80 for burst reads */
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {2, 480},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *lis331_get_slave_descr(void)
+{
+        return &lis331dlh_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct lis331_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int lis331_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct lis331_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        lis331_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int lis331_mod_remove(struct i2c_client *client)
+{
+        struct lis331_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                lis331_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id lis331_mod_id[] = {
+        { "lis331", ACCEL_ID_LIS331 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, lis331_mod_id);
+
+static struct i2c_driver lis331_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = lis331_mod_probe,
+        .remove = lis331_mod_remove,
+        .id_table = lis331_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "lis331_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init lis331_mod_init(void)
+{
+        int res = i2c_add_driver(&lis331_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "lis331_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit lis331_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&lis331_mod_driver);
+}
+
+module_init(lis331_mod_init);
+module_exit(lis331_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate LIS331 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("lis331_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/lis3dh.c b/drivers/misc/inv_mpu/accel/lis3dh.c
new file mode 100644
index 00000000000..27206e4b847
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/lis3dh.c
@@ -0,0 +1,728 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   lis3dh.c
+ *      @brief  Accelerometer setup and handling methods for ST LIS3DH.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* full scale setting - register & mask */
+#define LIS3DH_CTRL_REG1         (0x20)
+#define LIS3DH_CTRL_REG2         (0x21)
+#define LIS3DH_CTRL_REG3         (0x22)
+#define LIS3DH_CTRL_REG4         (0x23)
+#define LIS3DH_CTRL_REG5         (0x24)
+#define LIS3DH_CTRL_REG6         (0x25)
+#define LIS3DH_REFERENCE         (0x26)
+#define LIS3DH_STATUS_REG        (0x27)
+#define LIS3DH_OUT_X_L           (0x28)
+#define LIS3DH_OUT_X_H           (0x29)
+#define LIS3DH_OUT_Y_L           (0x2a)
+#define LIS3DH_OUT_Y_H           (0x2b)
+#define LIS3DH_OUT_Z_L           (0x2c)
+#define LIS3DH_OUT_Z_H           (0x2d)
+
+#define LIS3DH_INT1_CFG          (0x30)
+#define LIS3DH_INT1_SRC          (0x31)
+#define LIS3DH_INT1_THS          (0x32)
+#define LIS3DH_INT1_DURATION     (0x33)
+
+#define LIS3DH_MAX_DUR (0x7F)
+
+/* -------------------------------------------------------------------------- */
+
+struct lis3dh_config {
+        unsigned long odr;
+        unsigned int fsr;       /* full scale range mg */
+        unsigned int ths;       /* Motion no-motion thseshold mg */
+        unsigned int dur;       /* Motion no-motion duration ms */
+        unsigned char reg_ths;
+        unsigned char reg_dur;
+        unsigned char ctrl_reg1;
+        unsigned char irq_type;
+        unsigned char mot_int1_cfg;
+};
+
+struct lis3dh_private_data {
+        struct lis3dh_config suspend;
+        struct lis3dh_config resume;
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int lis3dh_set_ths(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct lis3dh_config *config, int apply, long ths)
+{
+        int result = INV_SUCCESS;
+        if ((unsigned int)ths > 1000 * config->fsr)
+                ths = (long)1000 * config->fsr;
+
+        if (ths < 0)
+                ths = 0;
+
+        config->ths = ths;
+        config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr));
+        MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS3DH_INT1_THS,
+                                                 config->reg_ths);
+        return result;
+}
+
+static int lis3dh_set_dur(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct lis3dh_config *config, int apply, long dur)
+{
+        int result = INV_SUCCESS;
+        long reg_dur = (dur * config->odr) / 1000000L;
+        config->dur = dur;
+
+        if (reg_dur > LIS3DH_MAX_DUR)
+                reg_dur = LIS3DH_MAX_DUR;
+
+        config->reg_dur = (unsigned char)reg_dur;
+        MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS3DH_INT1_DURATION,
+                                                 (unsigned char)reg_dur);
+        return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int lis3dh_set_irq(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct lis3dh_config *config,
+                          int apply, long irq_type)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+
+        config->irq_type = (unsigned char)irq_type;
+        if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x10;
+                reg2 = 0x00;
+        } else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+                reg1 = 0x40;
+                reg2 = config->mot_int1_cfg;
+        } else {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        }
+
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS3DH_CTRL_REG3, reg1);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS3DH_INT1_CFG, reg2);
+        }
+
+        return result;
+}
+
+/**
+ * Set the Output data rate for the particular configuration
+ *
+ * @param config Config to modify with new ODR
+ * @param odr Output data rate in units of 1/1000Hz
+ */
+static int lis3dh_set_odr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct lis3dh_config *config, int apply, long odr)
+{
+        unsigned char bits;
+        int result = INV_SUCCESS;
+
+        if (odr > 400000L) {
+                config->odr = 1250000L;
+                bits = 0x90;
+        } else if (odr > 200000L) {
+                config->odr = 400000L;
+                bits = 0x70;
+        } else if (odr > 100000L) {
+                config->odr = 200000L;
+                bits = 0x60;
+        } else if (odr > 50000) {
+                config->odr = 100000L;
+                bits = 0x50;
+        } else if (odr > 25000) {
+                config->odr = 50000;
+                bits = 0x40;
+        } else if (odr > 10000) {
+                config->odr = 25000;
+                bits = 0x30;
+        } else if (odr > 1000) {
+                config->odr = 10000;
+                bits = 0x20;
+        } else if (odr > 500) {
+                config->odr = 1000;
+                bits = 0x10;
+        } else {
+                config->odr = 0;
+                bits = 0;
+        }
+
+        config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0xf);
+        lis3dh_set_dur(mlsl_handle, pdata, config, apply, config->dur);
+        MPL_LOGV("ODR: %ld, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS3DH_CTRL_REG1,
+                                                 config->ctrl_reg1);
+        return result;
+}
+
+/**
+ * Set the full scale range of the accels
+ *
+ * @param config pointer to configuration
+ * @param fsr requested full scale range
+ */
+static int lis3dh_set_fsr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct lis3dh_config *config, int apply, long fsr)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1 = 0x48;
+
+        if (fsr <= 2048) {
+                config->fsr = 2048;
+        } else if (fsr <= 4096) {
+                reg1 |= 0x10;
+                config->fsr = 4096;
+        } else if (fsr <= 8192) {
+                reg1 |= 0x20;
+                config->fsr = 8192;
+        } else {
+                reg1 |= 0x30;
+                config->fsr = 16348;
+        }
+
+        lis3dh_set_ths(mlsl_handle, pdata, config, apply, config->ths);
+        MPL_LOGV("FSR: %d\n", config->fsr);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 LIS3DH_CTRL_REG4, reg1);
+
+        return result;
+}
+
+static int lis3dh_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+        struct lis3dh_private_data *private_data = pdata->private_data;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_CTRL_REG1,
+                                         private_data->suspend.ctrl_reg1);
+
+        reg1 = 0x48;
+        if (private_data->suspend.fsr == 16384)
+                reg1 |= 0x30;
+        else if (private_data->suspend.fsr == 8192)
+                reg1 |= 0x20;
+        else if (private_data->suspend.fsr == 4096)
+                reg1 |= 0x10;
+        else if (private_data->suspend.fsr == 2048)
+                reg1 |= 0x00;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_CTRL_REG4, reg1);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_INT1_THS,
+                                         private_data->suspend.reg_ths);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_INT1_DURATION,
+                                         private_data->suspend.reg_dur);
+
+        if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x10;
+                reg2 = 0x00;
+        } else if (private_data->suspend.irq_type ==
+                   MPU_SLAVE_IRQ_TYPE_MOTION) {
+                reg1 = 0x40;
+                reg2 = private_data->suspend.mot_int1_cfg;
+        } else {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_CTRL_REG3, reg1);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_INT1_CFG, reg2);
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 LIS3DH_CTRL_REG6, 1, &reg1);
+
+        return result;
+}
+
+static int lis3dh_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char reg1;
+        unsigned char reg2;
+        struct lis3dh_private_data *private_data = pdata->private_data;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_CTRL_REG1,
+                                         private_data->resume.ctrl_reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(6);
+
+        /* Full Scale */
+        reg1 = 0x48;
+        if (private_data->suspend.fsr == 16384)
+                reg1 |= 0x30;
+        else if (private_data->suspend.fsr == 8192)
+                reg1 |= 0x20;
+        else if (private_data->suspend.fsr == 4096)
+                reg1 |= 0x10;
+        else if (private_data->suspend.fsr == 2048)
+                reg1 |= 0x00;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_CTRL_REG4, reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x10;
+                reg2 = 0x00;
+        } else if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+                reg1 = 0x40;
+                reg2 = private_data->resume.mot_int1_cfg;
+        } else {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_CTRL_REG3, reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_INT1_THS,
+                                         private_data->resume.reg_ths);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_INT1_DURATION,
+                                         private_data->resume.reg_dur);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_INT1_CFG, reg2);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 LIS3DH_CTRL_REG6, 1, &reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int lis3dh_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        int result = INV_SUCCESS;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 LIS3DH_STATUS_REG, 1, data);
+        if (data[0] & 0x0F) {
+                result = inv_serial_read(mlsl_handle, pdata->address,
+                                         slave->read_reg, slave->read_len,
+                                         data);
+                return result;
+        } else
+                return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+static int lis3dh_init(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        int result;
+        long range;
+        struct lis3dh_private_data *private_data;
+        private_data = (struct lis3dh_private_data *)
+            kzalloc(sizeof(struct lis3dh_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        private_data->resume.ctrl_reg1 = 0x67;
+        private_data->suspend.ctrl_reg1 = 0x18;
+        private_data->resume.mot_int1_cfg = 0x95;
+        private_data->suspend.mot_int1_cfg = 0x2a;
+
+        lis3dh_set_odr(mlsl_handle, pdata, &private_data->suspend, false, 0);
+        lis3dh_set_odr(mlsl_handle, pdata, &private_data->resume,
+                       false, 200000L);
+
+        range = range_fixedpoint_to_long_mg(slave->range);
+        lis3dh_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                        false, range);
+        lis3dh_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                        false, range);
+
+        lis3dh_set_ths(mlsl_handle, pdata, &private_data->suspend,
+                        false, 80);
+        lis3dh_set_ths(mlsl_handle, pdata, &private_data->resume,
+                        false, 40);
+
+        lis3dh_set_dur(mlsl_handle, pdata, &private_data->suspend,
+                        false, 1000);
+        lis3dh_set_dur(mlsl_handle, pdata, &private_data->resume,
+                        false, 2540);
+
+        lis3dh_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                        false, MPU_SLAVE_IRQ_TYPE_NONE);
+        lis3dh_set_irq(mlsl_handle, pdata, &private_data->resume,
+                        false, MPU_SLAVE_IRQ_TYPE_NONE);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         LIS3DH_CTRL_REG1, 0x07);
+        msleep(6);
+
+        return INV_SUCCESS;
+}
+
+static int lis3dh_exit(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+static int lis3dh_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        struct lis3dh_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return lis3dh_set_odr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return lis3dh_set_odr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return lis3dh_set_fsr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return lis3dh_set_fsr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                return lis3dh_set_ths(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                return lis3dh_set_ths(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                return lis3dh_set_dur(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                return lis3dh_set_dur(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return lis3dh_set_irq(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return lis3dh_set_irq(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+        return INV_SUCCESS;
+}
+
+static int lis3dh_get_config(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata,
+                             struct ext_slave_config *data)
+{
+        struct lis3dh_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.ths;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.ths;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.dur;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.dur;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.irq_type;
+                break;
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_descr lis3dh_descr = {
+        .init             = lis3dh_init,
+        .exit             = lis3dh_exit,
+        .suspend          = lis3dh_suspend,
+        .resume           = lis3dh_resume,
+        .read             = lis3dh_read,
+        .config           = lis3dh_config,
+        .get_config       = lis3dh_get_config,
+        .name             = "lis3dh",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_LIS3DH,
+        .read_reg         = 0x28 | 0x80, /* 0x80 for burst reads */
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {2, 480},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *lis3dh_get_slave_descr(void)
+{
+        return &lis3dh_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct lis3dh_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int lis3dh_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct lis3dh_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        lis3dh_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int lis3dh_mod_remove(struct i2c_client *client)
+{
+        struct lis3dh_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                lis3dh_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id lis3dh_mod_id[] = {
+        { "lis3dh", ACCEL_ID_LIS3DH },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, lis3dh_mod_id);
+
+static struct i2c_driver lis3dh_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = lis3dh_mod_probe,
+        .remove = lis3dh_mod_remove,
+        .id_table = lis3dh_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "lis3dh_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init lis3dh_mod_init(void)
+{
+        int res = i2c_add_driver(&lis3dh_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "lis3dh_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit lis3dh_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&lis3dh_mod_driver);
+}
+
+module_init(lis3dh_mod_init);
+module_exit(lis3dh_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate LIS3DH sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("lis3dh_mod");
+
+/*
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/lsm303dlx_a.c b/drivers/misc/inv_mpu/accel/lsm303dlx_a.c
new file mode 100644
index 00000000000..576282a0fb1
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/lsm303dlx_a.c
@@ -0,0 +1,881 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   lsm303dlx_a.c
+ *      @brief  Accelerometer setup and handling methods for ST LSM303DLH
+ *              or LSM303DLM accel.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+/* full scale setting - register & mask */
+#define LSM303DLx_CTRL_REG1         (0x20)
+#define LSM303DLx_CTRL_REG2         (0x21)
+#define LSM303DLx_CTRL_REG3         (0x22)
+#define LSM303DLx_CTRL_REG4         (0x23)
+#define LSM303DLx_CTRL_REG5         (0x24)
+#define LSM303DLx_HP_FILTER_RESET   (0x25)
+#define LSM303DLx_REFERENCE         (0x26)
+#define LSM303DLx_STATUS_REG        (0x27)
+#define LSM303DLx_OUT_X_L           (0x28)
+#define LSM303DLx_OUT_X_H           (0x29)
+#define LSM303DLx_OUT_Y_L           (0x2a)
+#define LSM303DLx_OUT_Y_H           (0x2b)
+#define LSM303DLx_OUT_Z_L           (0x2b)
+#define LSM303DLx_OUT_Z_H           (0x2d)
+
+#define LSM303DLx_INT1_CFG          (0x30)
+#define LSM303DLx_INT1_SRC          (0x31)
+#define LSM303DLx_INT1_THS          (0x32)
+#define LSM303DLx_INT1_DURATION     (0x33)
+
+#define LSM303DLx_INT2_CFG          (0x34)
+#define LSM303DLx_INT2_SRC          (0x35)
+#define LSM303DLx_INT2_THS          (0x36)
+#define LSM303DLx_INT2_DURATION     (0x37)
+
+#define LSM303DLx_CTRL_MASK         (0x30)
+#define LSM303DLx_SLEEP_MASK        (0x20)
+#define LSM303DLx_PWR_MODE_NORMAL   (0x20)
+
+#define LSM303DLx_MAX_DUR           (0x7F)
+
+/* -------------------------------------------------------------------------- */
+
+struct lsm303dlx_a_config {
+        unsigned int odr;
+        unsigned int fsr; /** < full scale range mg */
+        unsigned int ths; /** < Motion no-motion thseshold mg */
+        unsigned int dur; /** < Motion no-motion duration ms */
+        unsigned char reg_ths;
+        unsigned char reg_dur;
+        unsigned char ctrl_reg1;
+        unsigned char irq_type;
+        unsigned char mot_int1_cfg;
+};
+
+struct lsm303dlx_a_private_data {
+        struct lsm303dlx_a_config suspend;
+        struct lsm303dlx_a_config resume;
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int lsm303dlx_a_set_ths(void *mlsl_handle,
+                               struct ext_slave_platform_data *pdata,
+                               struct lsm303dlx_a_config *config,
+                               int apply,
+                               long ths)
+{
+        int result = INV_SUCCESS;
+        if ((unsigned int) ths >= config->fsr)
+                ths = (long) config->fsr - 1;
+
+        if (ths < 0)
+                ths = 0;
+
+        config->ths = ths;
+        config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr));
+        MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        LSM303DLx_INT1_THS,
+                                        config->reg_ths);
+        return result;
+}
+
+static int lsm303dlx_a_set_dur(void *mlsl_handle,
+                               struct ext_slave_platform_data *pdata,
+                               struct lsm303dlx_a_config *config,
+                               int apply,
+                               long dur)
+{
+        int result = INV_SUCCESS;
+        long reg_dur = (dur * config->odr) / 1000000L;
+        config->dur = dur;
+
+        if (reg_dur > LSM303DLx_MAX_DUR)
+                reg_dur = LSM303DLx_MAX_DUR;
+
+        config->reg_dur = (unsigned char) reg_dur;
+        MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        LSM303DLx_INT1_DURATION,
+                                        (unsigned char)reg_dur);
+        return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int lsm303dlx_a_set_irq(void *mlsl_handle,
+                               struct ext_slave_platform_data *pdata,
+                               struct lsm303dlx_a_config *config,
+                               int apply,
+                               long irq_type)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+
+        config->irq_type = (unsigned char)irq_type;
+        if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x02;
+                reg2 = 0x00;
+        } else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+                reg1 = 0x00;
+                reg2 = config->mot_int1_cfg;
+        } else {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        }
+
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        LSM303DLx_CTRL_REG3, reg1);
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        LSM303DLx_INT1_CFG, reg2);
+        }
+
+        return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_set_odr(void *mlsl_handle,
+                               struct ext_slave_platform_data *pdata,
+                               struct lsm303dlx_a_config *config,
+                               int apply,
+                               long odr)
+{
+        unsigned char bits;
+        int result = INV_SUCCESS;
+
+        /* normal power modes */
+        if (odr > 400000) {
+                config->odr = 1000000;
+                bits = LSM303DLx_PWR_MODE_NORMAL | 0x18;
+        } else if (odr > 100000) {
+                config->odr = 400000;
+                bits = LSM303DLx_PWR_MODE_NORMAL | 0x10;
+        } else if (odr > 50000) {
+                config->odr = 100000;
+                bits = LSM303DLx_PWR_MODE_NORMAL | 0x08;
+        } else if (odr > 10000) {
+                config->odr = 50000;
+                bits = LSM303DLx_PWR_MODE_NORMAL | 0x00;
+        /* low power modes */
+        } else if (odr > 5000) {
+                config->odr = 10000;
+                bits = 0xC0;
+        } else if (odr > 2000) {
+                config->odr = 5000;
+                bits = 0xA0;
+        } else if (odr > 1000) {
+                config->odr = 2000;
+                bits = 0x80;
+        } else if (odr > 500) {
+                config->odr = 1000;
+                bits = 0x60;
+        } else if (odr > 0) {
+                config->odr = 500;
+                bits = 0x40;
+        } else {
+                config->odr = 0;
+                bits = 0;
+        }
+
+        config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x7);
+        lsm303dlx_a_set_dur(mlsl_handle, pdata, config, apply, config->dur);
+        MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        LSM303DLx_CTRL_REG1,
+                                        config->ctrl_reg1);
+        return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_set_fsr(void *mlsl_handle,
+                               struct ext_slave_platform_data *pdata,
+                               struct lsm303dlx_a_config *config,
+                               int apply,
+                               long fsr)
+{
+        unsigned char reg1 = 0x40;
+        int result = INV_SUCCESS;
+
+        if (fsr <= 2048) {
+                config->fsr = 2048;
+        } else if (fsr <= 4096) {
+                reg1 |= 0x30;
+                config->fsr = 4096;
+        } else {
+                reg1 |= 0x10;
+                config->fsr = 8192;
+        }
+
+        lsm303dlx_a_set_ths(mlsl_handle, pdata,
+                        config, apply, config->ths);
+        MPL_LOGV("FSR: %d\n", config->fsr);
+        if (apply)
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        LSM303DLx_CTRL_REG4, reg1);
+
+        return result;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_suspend(void *mlsl_handle,
+                               struct ext_slave_descr *slave,
+                               struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+        struct lsm303dlx_a_private_data *private_data =
+                (struct lsm303dlx_a_private_data *)(pdata->private_data);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_CTRL_REG1,
+                                       private_data->suspend.ctrl_reg1);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_CTRL_REG2, 0x0f);
+        reg1 = 0x40;
+        if (private_data->suspend.fsr == 8192)
+                reg1 |= 0x30;
+        else if (private_data->suspend.fsr == 4096)
+                reg1 |= 0x10;
+        /* else bits [4..5] are already zero */
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_CTRL_REG4, reg1);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_INT1_THS,
+                                       private_data->suspend.reg_ths);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_INT1_DURATION,
+                                       private_data->suspend.reg_dur);
+
+        if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x02;
+                reg2 = 0x00;
+        } else if (private_data->suspend.irq_type ==
+                   MPU_SLAVE_IRQ_TYPE_MOTION) {
+                reg1 = 0x00;
+                reg2 = private_data->suspend.mot_int1_cfg;
+        } else {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_CTRL_REG3, reg1);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_INT1_CFG, reg2);
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                LSM303DLx_HP_FILTER_RESET, 1, &reg1);
+        return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_resume(void *mlsl_handle,
+                              struct ext_slave_descr *slave,
+                              struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+        struct lsm303dlx_a_private_data *private_data =
+                (struct lsm303dlx_a_private_data *)(pdata->private_data);
+
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_CTRL_REG1,
+                                       private_data->resume.ctrl_reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(6);
+
+        /* Full Scale */
+        reg1 = 0x40;
+        if (private_data->resume.fsr == 8192)
+                reg1 |= 0x30;
+        else if (private_data->resume.fsr == 4096)
+                reg1 |= 0x10;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_CTRL_REG4, reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Configure high pass filter */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_CTRL_REG2, 0x0F);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x02;
+                reg2 = 0x00;
+        } else if (private_data->resume.irq_type ==
+                   MPU_SLAVE_IRQ_TYPE_MOTION) {
+                reg1 = 0x00;
+                reg2 = private_data->resume.mot_int1_cfg;
+        } else {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_CTRL_REG3, reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_INT1_THS,
+                                       private_data->resume.reg_ths);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_INT1_DURATION,
+                                       private_data->resume.reg_dur);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       LSM303DLx_INT1_CFG, reg2);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                LSM303DLx_HP_FILTER_RESET, 1, &reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_read(void *mlsl_handle,
+                            struct ext_slave_descr *slave,
+                            struct ext_slave_platform_data *pdata,
+                            unsigned char *data)
+{
+        int result = INV_SUCCESS;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                LSM303DLx_STATUS_REG, 1, data);
+        if (data[0] & 0x0F) {
+                result = inv_serial_read(mlsl_handle, pdata->address,
+                                        slave->read_reg, slave->read_len, data);
+        return result;
+        } else
+                return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_init(void *mlsl_handle,
+                            struct ext_slave_descr *slave,
+                            struct ext_slave_platform_data *pdata)
+{
+        long range;
+        struct lsm303dlx_a_private_data *private_data;
+        private_data = (struct lsm303dlx_a_private_data *)
+            kzalloc(sizeof(struct lsm303dlx_a_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        private_data->resume.ctrl_reg1 = 0x37;
+        private_data->suspend.ctrl_reg1 = 0x47;
+        private_data->resume.mot_int1_cfg = 0x95;
+        private_data->suspend.mot_int1_cfg = 0x2a;
+
+        lsm303dlx_a_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                        false, 0);
+        lsm303dlx_a_set_odr(mlsl_handle, pdata, &private_data->resume,
+                        false, 200000);
+
+        range = range_fixedpoint_to_long_mg(slave->range);
+        lsm303dlx_a_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                        false, range);
+        lsm303dlx_a_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                        false, range);
+
+        lsm303dlx_a_set_ths(mlsl_handle, pdata, &private_data->suspend,
+                        false, 80);
+        lsm303dlx_a_set_ths(mlsl_handle, pdata, &private_data->resume,
+                        false, 40);
+
+        lsm303dlx_a_set_dur(mlsl_handle, pdata, &private_data->suspend,
+                        false, 1000);
+        lsm303dlx_a_set_dur(mlsl_handle, pdata, &private_data->resume,
+                        false, 2540);
+
+        lsm303dlx_a_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                        false, MPU_SLAVE_IRQ_TYPE_NONE);
+        lsm303dlx_a_set_irq(mlsl_handle, pdata, &private_data->resume,
+                        false, MPU_SLAVE_IRQ_TYPE_NONE);
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_exit(void *mlsl_handle,
+                            struct ext_slave_descr *slave,
+                            struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_config(void *mlsl_handle,
+                              struct ext_slave_descr *slave,
+                              struct ext_slave_platform_data *pdata,
+                              struct ext_slave_config *data)
+{
+        struct lsm303dlx_a_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return lsm303dlx_a_set_odr(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return lsm303dlx_a_set_odr(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return lsm303dlx_a_set_fsr(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return lsm303dlx_a_set_fsr(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                return lsm303dlx_a_set_ths(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                return lsm303dlx_a_set_ths(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                return lsm303dlx_a_set_dur(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                return lsm303dlx_a_set_dur(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return lsm303dlx_a_set_irq(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return lsm303dlx_a_set_irq(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_get_config(void *mlsl_handle,
+                                  struct ext_slave_descr *slave,
+                                  struct ext_slave_platform_data *pdata,
+                                  struct ext_slave_config *data)
+{
+        struct lsm303dlx_a_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.ths;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.ths;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.dur;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.dur;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.irq_type;
+                break;
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_descr lsm303dlx_a_descr = {
+        .init             = lsm303dlx_a_init,
+        .exit             = lsm303dlx_a_exit,
+        .suspend          = lsm303dlx_a_suspend,
+        .resume           = lsm303dlx_a_resume,
+        .read             = lsm303dlx_a_read,
+        .config           = lsm303dlx_a_config,
+        .get_config       = lsm303dlx_a_get_config,
+        .name             = "lsm303dlx_a",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_LSM303DLX,
+        .read_reg         = (0x28 | 0x80), /* 0x80 for burst reads */
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {2, 480},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *lsm303dlx_a_get_slave_descr(void)
+{
+        return &lsm303dlx_a_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct lsm303dlx_a_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int lsm303dlx_a_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct lsm303dlx_a_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        lsm303dlx_a_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int lsm303dlx_a_mod_remove(struct i2c_client *client)
+{
+        struct lsm303dlx_a_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                lsm303dlx_a_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id lsm303dlx_a_mod_id[] = {
+        { "lsm303dlx", ACCEL_ID_LSM303DLX },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, lsm303dlx_a_mod_id);
+
+static struct i2c_driver lsm303dlx_a_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = lsm303dlx_a_mod_probe,
+        .remove = lsm303dlx_a_mod_remove,
+        .id_table = lsm303dlx_a_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "lsm303dlx_a_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init lsm303dlx_a_mod_init(void)
+{
+        int res = i2c_add_driver(&lsm303dlx_a_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "lsm303dlx_a_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit lsm303dlx_a_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&lsm303dlx_a_mod_driver);
+}
+
+module_init(lsm303dlx_a_mod_init);
+module_exit(lsm303dlx_a_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate LSM303DLX_A sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("lsm303dlx_a_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/mma8450.c b/drivers/misc/inv_mpu/accel/mma8450.c
new file mode 100644
index 00000000000..f698ee98bf5
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/mma8450.c
@@ -0,0 +1,804 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   mma8450.c
+ *      @brief  Accelerometer setup and handling methods for Freescale MMA8450.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* full scale setting - register & mask */
+#define ACCEL_MMA8450_XYZ_DATA_CFG      (0x16)
+
+#define ACCEL_MMA8450_CTRL_REG1         (0x38)
+#define ACCEL_MMA8450_CTRL_REG2     (0x39)
+#define ACCEL_MMA8450_CTRL_REG4         (0x3B)
+#define ACCEL_MMA8450_CTRL_REG5         (0x3C)
+
+#define ACCEL_MMA8450_CTRL_REG          (0x38)
+#define ACCEL_MMA8450_CTRL_MASK         (0x03)
+
+#define ACCEL_MMA8450_SLEEP_MASK        (0x03)
+
+/* -------------------------------------------------------------------------- */
+
+struct mma8450_config {
+        unsigned int odr;
+        unsigned int fsr;       /** < full scale range mg */
+        unsigned int ths;       /** < Motion no-motion thseshold mg */
+        unsigned int dur;       /** < Motion no-motion duration ms */
+        unsigned char reg_ths;
+        unsigned char reg_dur;
+        unsigned char ctrl_reg1;
+        unsigned char irq_type;
+        unsigned char mot_int1_cfg;
+};
+
+struct mma8450_private_data {
+        struct mma8450_config suspend;
+        struct mma8450_config resume;
+};
+
+
+/* -------------------------------------------------------------------------- */
+
+static int mma8450_set_ths(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct mma8450_config *config,
+                        int apply,
+                        long ths)
+{
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+static int mma8450_set_dur(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct mma8450_config *config,
+                        int apply,
+                        long dur)
+{
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+/**
+ *  @brief Sets the IRQ to fire when one of the IRQ events occur.
+ *         Threshold and duration will not be used unless the type is MOT or
+ *         NMOT.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *              configuration to apply to, suspend or resume
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param irq_type
+ *              the type of IRQ.  Valid values are
+ *              - MPU_SLAVE_IRQ_TYPE_NONE
+ *              - MPU_SLAVE_IRQ_TYPE_MOTION
+ *              - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_set_irq(void *mlsl_handle,
+                struct ext_slave_platform_data *pdata,
+                struct mma8450_config *config,
+                int apply,
+                long irq_type)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+        unsigned char reg3;
+
+        config->irq_type = (unsigned char)irq_type;
+        if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x01;
+                reg2 = 0x01;
+                reg3 = 0x07;
+        } else if (irq_type == MPU_SLAVE_IRQ_TYPE_NONE) {
+                reg1 = 0x00;
+                reg2 = 0x00;
+                reg3 = 0x00;
+        } else {
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        }
+
+        if (apply) {
+                /* XYZ_DATA_CFG: event flag enabled on Z axis */
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        ACCEL_MMA8450_XYZ_DATA_CFG, reg3);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        ACCEL_MMA8450_CTRL_REG4, reg1);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        ACCEL_MMA8450_CTRL_REG5, reg2);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_set_odr(void *mlsl_handle,
+                struct ext_slave_platform_data *pdata,
+                struct mma8450_config *config,
+                int apply,
+                long odr)
+{
+        unsigned char bits;
+        int result = INV_SUCCESS;
+
+        if (odr > 200000) {
+                config->odr = 400000;
+                bits = 0x00;
+        } else if (odr > 100000) {
+                config->odr = 200000;
+                bits = 0x04;
+        } else if (odr > 50000) {
+                config->odr = 100000;
+                bits = 0x08;
+        } else if (odr > 25000) {
+                config->odr = 50000;
+                bits = 0x0C;
+        } else if (odr > 12500) {
+                config->odr = 25000;
+                bits = 0x40; /* Sleep -> Auto wake mode */
+        } else if (odr > 1563) {
+                config->odr = 12500;
+                bits = 0x10;
+        } else if (odr > 0) {
+                config->odr = 1563;
+                bits = 0x14;
+        } else {
+                config->ctrl_reg1 = 0; /* Set FS1.FS2 to Standby */
+                config->odr = 0;
+                bits = 0;
+        }
+
+        config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x3);
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                ACCEL_MMA8450_CTRL_REG1, 0);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                ACCEL_MMA8450_CTRL_REG1, config->ctrl_reg1);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("ODR: %d mHz, 0x%02x\n",
+                        config->odr, (int)config->ctrl_reg1);
+        }
+        return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_set_fsr(void *mlsl_handle,
+                struct ext_slave_platform_data *pdata,
+                struct mma8450_config *config,
+                int apply,
+                long fsr)
+{
+        unsigned char bits;
+        int result = INV_SUCCESS;
+
+        if (fsr <= 2000) {
+                bits = 0x01;
+                config->fsr = 2000;
+        } else if (fsr <= 4000) {
+                bits = 0x02;
+                config->fsr = 4000;
+        } else {
+                bits = 0x03;
+                config->fsr = 8000;
+        }
+
+        config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0xFC);
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                ACCEL_MMA8450_CTRL_REG1, config->ctrl_reg1);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("FSR: %d mg\n", config->fsr);
+        }
+        return result;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_suspend(void *mlsl_handle,
+                    struct ext_slave_descr *slave,
+                    struct ext_slave_platform_data *pdata)
+{
+        int result;
+        struct mma8450_private_data *private_data = pdata->private_data;
+
+        if (private_data->suspend.fsr == 4000)
+                slave->range.mantissa = 4;
+        else if (private_data->suspend.fsr == 8000)
+                slave->range.mantissa = 8;
+        else
+                slave->range.mantissa = 2;
+        slave->range.fraction = 0;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        ACCEL_MMA8450_CTRL_REG1, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        if (private_data->suspend.ctrl_reg1) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                ACCEL_MMA8450_CTRL_REG1,
+                                private_data->suspend.ctrl_reg1);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        result = mma8450_set_irq(mlsl_handle, pdata,
+                                &private_data->suspend,
+                                true, private_data->suspend.irq_type);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_resume(void *mlsl_handle,
+                   struct ext_slave_descr *slave,
+                   struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        struct mma8450_private_data *private_data = pdata->private_data;
+
+        /* Full Scale */
+        if (private_data->resume.fsr == 4000)
+                slave->range.mantissa = 4;
+        else if (private_data->resume.fsr == 8000)
+                slave->range.mantissa = 8;
+        else
+                slave->range.mantissa = 2;
+        slave->range.fraction = 0;
+
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        ACCEL_MMA8450_CTRL_REG1, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        if (private_data->resume.ctrl_reg1) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                ACCEL_MMA8450_CTRL_REG1,
+                                private_data->resume.ctrl_reg1);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        result = mma8450_set_irq(mlsl_handle, pdata,
+                        &private_data->resume,
+                        true, private_data->resume.irq_type);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_read(void *mlsl_handle,
+                 struct ext_slave_descr *slave,
+                 struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+        int result;
+        unsigned char local_data[4];    /* Status register + 3 bytes data */
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                0x00, sizeof(local_data), local_data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        memcpy(data, &local_data[1], (slave->read_len) - 1);
+
+        MPL_LOGV("Data Not Ready: %02x %02x %02x %02x\n",
+                 local_data[0], local_data[1],
+                 local_data[2], local_data[3]);
+
+        return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_init(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        struct mma8450_private_data *private_data;
+        private_data = (struct mma8450_private_data *)
+            kzalloc(sizeof(struct mma8450_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        mma8450_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                        false, 0);
+        mma8450_set_odr(mlsl_handle, pdata, &private_data->resume,
+                        false, 200000);
+        mma8450_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                        false, 2000);
+        mma8450_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                        false, 2000);
+        mma8450_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                        false,
+                        MPU_SLAVE_IRQ_TYPE_NONE);
+        mma8450_set_irq(mlsl_handle, pdata, &private_data->resume,
+                        false,
+                        MPU_SLAVE_IRQ_TYPE_NONE);
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_exit(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_config(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_config *data)
+{
+        struct mma8450_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return mma8450_set_odr(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return mma8450_set_odr(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return mma8450_set_fsr(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return mma8450_set_fsr(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                return mma8450_set_ths(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                return mma8450_set_ths(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                return mma8450_set_dur(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                return mma8450_set_dur(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return mma8450_set_irq(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return mma8450_set_irq(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_get_config(void *mlsl_handle,
+                                struct ext_slave_descr *slave,
+                                struct ext_slave_platform_data *pdata,
+                                struct ext_slave_config *data)
+{
+        struct mma8450_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.ths;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.ths;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.dur;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.dur;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.irq_type;
+                break;
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_descr mma8450_descr = {
+        .init             = mma8450_init,
+        .exit             = mma8450_exit,
+        .suspend          = mma8450_suspend,
+        .resume           = mma8450_resume,
+        .read             = mma8450_read,
+        .config           = mma8450_config,
+        .get_config       = mma8450_get_config,
+        .name             = "mma8450",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_MMA8450,
+        .read_reg         = 0x00,
+        .read_len         = 4,
+        .endian           = EXT_SLAVE_FS8_BIG_ENDIAN,
+        .range            = {2, 0},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *mma8450_get_slave_descr(void)
+{
+        return &mma8450_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct mma8450_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int mma8450_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct mma8450_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        mma8450_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int mma8450_mod_remove(struct i2c_client *client)
+{
+        struct mma8450_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                mma8450_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id mma8450_mod_id[] = {
+        { "mma8450", ACCEL_ID_MMA8450 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, mma8450_mod_id);
+
+static struct i2c_driver mma8450_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = mma8450_mod_probe,
+        .remove = mma8450_mod_remove,
+        .id_table = mma8450_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "mma8450_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init mma8450_mod_init(void)
+{
+        int res = i2c_add_driver(&mma8450_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "mma8450_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit mma8450_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&mma8450_mod_driver);
+}
+
+module_init(mma8450_mod_init);
+module_exit(mma8450_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate MMA8450 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("mma8450_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/mma845x.c b/drivers/misc/inv_mpu/accel/mma845x.c
new file mode 100644
index 00000000000..5f62b22388b
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/mma845x.c
@@ -0,0 +1,713 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   mma845x.c
+ *      @brief  Accelerometer setup and handling methods for Freescale MMA845X
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+#define ACCEL_MMA845X_XYZ_DATA_CFG      (0x0E)
+#define ACCEL_MMA845X_CTRL_REG1      (0x2A)
+#define ACCEL_MMA845X_CTRL_REG4         (0x2D)
+#define ACCEL_MMA845X_CTRL_REG5         (0x2E)
+
+#define ACCEL_MMA845X_SLEEP_MASK     (0x01)
+
+/* full scale setting - register & mask */
+#define ACCEL_MMA845X_CFG_REG       (0x0E)
+#define ACCEL_MMA845X_CTRL_MASK     (0x03)
+
+/* -------------------------------------------------------------------------- */
+
+struct mma845x_config {
+        unsigned int odr;
+        unsigned int fsr;               /** < full scale range mg */
+        unsigned int ths;               /** < Motion no-motion thseshold mg */
+        unsigned int dur;               /** < Motion no-motion duration ms */
+        unsigned char reg_ths;
+        unsigned char reg_dur;
+        unsigned char ctrl_reg1;
+        unsigned char irq_type;
+        unsigned char mot_int1_cfg;
+};
+
+struct mma845x_private_data {
+        struct mma845x_config suspend;
+        struct mma845x_config resume;
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int mma845x_set_ths(void *mlsl_handle,
+                struct ext_slave_platform_data *pdata,
+                struct mma845x_config *config,
+                int apply,
+                long ths)
+{
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+static int mma845x_set_dur(void *mlsl_handle,
+                struct ext_slave_platform_data *pdata,
+                struct mma845x_config *config,
+                int apply,
+                long dur)
+{
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+/**
+ *  @brief Sets the IRQ to fire when one of the IRQ events occur.
+ *         Threshold and duration will not be used unless the type is MOT or
+ *         NMOT.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *              configuration to apply to, suspend or resume
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param irq_type
+ *              the type of IRQ.  Valid values are
+ *              - MPU_SLAVE_IRQ_TYPE_NONE
+ *              - MPU_SLAVE_IRQ_TYPE_MOTION
+ *              - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_set_irq(void *mlsl_handle,
+                struct ext_slave_platform_data *pdata,
+                struct mma845x_config *config,
+                int apply,
+                long irq_type)
+{
+        int result = INV_SUCCESS;
+        unsigned char reg1;
+        unsigned char reg2;
+
+        config->irq_type = (unsigned char)irq_type;
+        if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                reg1 = 0x01;
+                reg2 = 0x01;
+        } else if (irq_type == MPU_SLAVE_IRQ_TYPE_NONE) {
+                reg1 = 0x00;
+                reg2 = 0x00;
+        } else {
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        }
+
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        ACCEL_MMA845X_CTRL_REG4, reg1);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        ACCEL_MMA845X_CTRL_REG5, reg2);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_set_odr(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct mma845x_config *config,
+                        int apply,
+                        long odr)
+{
+        unsigned char bits;
+        int result = INV_SUCCESS;
+
+        if (odr > 400000) {
+                config->odr = 800000;
+                bits = 0x01;
+        } else if (odr > 200000) {
+                config->odr = 400000;
+                bits = 0x09;
+        } else if (odr > 100000) {
+                config->odr = 200000;
+                bits = 0x11;
+        } else if (odr > 50000) {
+                config->odr = 100000;
+                bits = 0x19;
+        } else if (odr > 12500) {
+                config->odr = 50000;
+                bits = 0x21;
+        } else if (odr > 6250) {
+                config->odr = 12500;
+                bits = 0x29;
+        } else if (odr > 1560) {
+                config->odr = 6250;
+                bits = 0x31;
+        } else if (odr > 0) {
+                config->odr = 1560;
+                bits = 0x39;
+        } else {
+                config->ctrl_reg1 = 0; /* Set FS1.FS2 to Standby */
+                config->odr = 0;
+                bits = 0;
+        }
+
+        config->ctrl_reg1 = bits;
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                ACCEL_MMA845X_CTRL_REG1,
+                                config->ctrl_reg1);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("ODR: %d mHz, 0x%02x\n", config->odr,
+                         (int)config->ctrl_reg1);
+        }
+        return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_set_fsr(void *mlsl_handle,
+                struct ext_slave_platform_data *pdata,
+                struct mma845x_config *config,
+                int apply,
+                long fsr)
+{
+        unsigned char bits;
+        int result = INV_SUCCESS;
+
+        if (fsr <= 2000) {
+                bits = 0x00;
+                config->fsr = 2000;
+        } else if (fsr <= 4000) {
+                bits = 0x01;
+                config->fsr = 4000;
+        } else {
+                bits = 0x02;
+                config->fsr = 8000;
+        }
+
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                ACCEL_MMA845X_XYZ_DATA_CFG,
+                                bits);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("FSR: %d mg\n", config->fsr);
+        }
+        return result;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_suspend(void *mlsl_handle,
+                    struct ext_slave_descr *slave,
+                    struct ext_slave_platform_data *pdata)
+{
+        int result;
+        struct mma845x_private_data *private_data = pdata->private_data;
+
+        /* Full Scale */
+        if (private_data->suspend.fsr == 4000)
+                slave->range.mantissa = 4;
+        else if (private_data->suspend.fsr == 8000)
+                slave->range.mantissa = 8;
+        else
+                slave->range.mantissa = 2;
+
+        slave->range.fraction = 0;
+
+        result = mma845x_set_fsr(mlsl_handle, pdata,
+                                &private_data->suspend,
+                                true, private_data->suspend.fsr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        ACCEL_MMA845X_CTRL_REG1,
+                                        private_data->suspend.ctrl_reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_resume(void *mlsl_handle,
+                   struct ext_slave_descr *slave,
+                   struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        struct mma845x_private_data *private_data = pdata->private_data;
+
+        /* Full Scale */
+        if (private_data->resume.fsr == 4000)
+                slave->range.mantissa = 4;
+        else if (private_data->resume.fsr == 8000)
+                slave->range.mantissa = 8;
+        else
+                slave->range.mantissa = 2;
+
+        slave->range.fraction = 0;
+
+        result = mma845x_set_fsr(mlsl_handle, pdata,
+                        &private_data->resume,
+                        true, private_data->resume.fsr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                        ACCEL_MMA845X_CTRL_REG1,
+                        private_data->resume.ctrl_reg1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_read(void *mlsl_handle,
+                 struct ext_slave_descr *slave,
+                 struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+        int result;
+        unsigned char local_data[7];    /* Status register + 6 bytes data */
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 slave->read_reg, sizeof(local_data),
+                                 local_data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        memcpy(data, &local_data[1], slave->read_len);
+        return result;
+}
+
+static int mma845x_init(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        long range;
+        struct mma845x_private_data *private_data;
+        private_data = (struct mma845x_private_data *)
+            kzalloc(sizeof(struct mma845x_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        mma845x_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                        false, 0);
+        mma845x_set_odr(mlsl_handle, pdata, &private_data->resume,
+                        false, 200000);
+
+        range = range_fixedpoint_to_long_mg(slave->range);
+        mma845x_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                        false, range);
+        mma845x_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                        false, range);
+
+        mma845x_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                        false, MPU_SLAVE_IRQ_TYPE_NONE);
+        mma845x_set_irq(mlsl_handle, pdata, &private_data->resume,
+                        false, MPU_SLAVE_IRQ_TYPE_NONE);
+        return INV_SUCCESS;
+}
+
+static int mma845x_exit(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+static int mma845x_config(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_config *data)
+{
+        struct mma845x_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return mma845x_set_odr(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return mma845x_set_odr(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return mma845x_set_fsr(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return mma845x_set_fsr(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                return mma845x_set_ths(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                return mma845x_set_ths(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                return mma845x_set_dur(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                return mma845x_set_dur(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return mma845x_set_irq(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return mma845x_set_irq(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static int mma845x_get_config(void *mlsl_handle,
+                                struct ext_slave_descr *slave,
+                                struct ext_slave_platform_data *pdata,
+                                struct ext_slave_config *data)
+{
+        struct mma845x_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.ths;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.ths;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.dur;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.dur;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.irq_type;
+                break;
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_descr mma845x_descr = {
+        .init             = mma845x_init,
+        .exit             = mma845x_exit,
+        .suspend          = mma845x_suspend,
+        .resume           = mma845x_resume,
+        .read             = mma845x_read,
+        .config           = mma845x_config,
+        .get_config       = mma845x_get_config,
+        .name             = "mma845x",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_MMA845X,
+        .read_reg         = 0x00,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_FS16_BIG_ENDIAN,
+        .range            = {2, 0},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *mma845x_get_slave_descr(void)
+{
+        return &mma845x_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct mma845x_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int mma845x_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct mma845x_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        mma845x_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int mma845x_mod_remove(struct i2c_client *client)
+{
+        struct mma845x_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                mma845x_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id mma845x_mod_id[] = {
+        { "mma845x", ACCEL_ID_MMA845X },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, mma845x_mod_id);
+
+static struct i2c_driver mma845x_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = mma845x_mod_probe,
+        .remove = mma845x_mod_remove,
+        .id_table = mma845x_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "mma845x_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init mma845x_mod_init(void)
+{
+        int res = i2c_add_driver(&mma845x_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "mma845x_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit mma845x_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&mma845x_mod_driver);
+}
+
+module_init(mma845x_mod_init);
+module_exit(mma845x_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate MMA845X sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("mma845x_mod");
+
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/mpu6050.c b/drivers/misc/inv_mpu/accel/mpu6050.c
new file mode 100644
index 00000000000..c5bb6784a41
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/mpu6050.c
@@ -0,0 +1,695 @@
+/*
+        $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 ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   mpu6050.c
+ *      @brief  Accelerometer setup and handling methods for Invensense MPU6050
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mpu6050b1.h"
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+struct mpu6050_config {
+        unsigned int odr;               /**< output data rate 1/1000 Hz */
+        unsigned int fsr;               /**< full scale range mg */
+        unsigned int ths;               /**< mot/no-mot thseshold mg */
+        unsigned int dur;               /**< mot/no-mot duration ms */
+        unsigned int irq_type;          /**< irq type */
+};
+
+struct mpu6050_private_data {
+        struct mpu6050_config suspend;
+        struct mpu6050_config resume;
+        struct mldl_cfg *mldl_cfg_ref;
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int mpu6050_set_mldl_cfg_ref(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct mldl_cfg *mldl_cfg_ref)
+{
+        struct mpu6050_private_data *private_data =
+                        (struct mpu6050_private_data *)pdata->private_data;
+        private_data->mldl_cfg_ref = mldl_cfg_ref;
+        return 0;
+}
+static int mpu6050_set_lp_mode(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        unsigned char lpa_freq)
+{
+        unsigned char b = 0;
+        /* Reducing the duration setting for lp mode */
+        b = 1;
+        inv_serial_single_write(mlsl_handle, pdata->address,
+                                MPUREG_ACCEL_MOT_DUR, b);
+        /* Setting the cycle bit and LPA wake up freq */
+        inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, 1,
+                        &b);
+        b |= BIT_CYCLE | BIT_PD_PTAT;
+        inv_serial_single_write(mlsl_handle, pdata->address,
+                                MPUREG_PWR_MGMT_1,
+                                b);
+        inv_serial_read(mlsl_handle, pdata->address,
+                        MPUREG_PWR_MGMT_2, 1, &b);
+        b |= lpa_freq & BITS_LPA_WAKE_CTRL;
+        inv_serial_single_write(mlsl_handle, pdata->address,
+                                MPUREG_PWR_MGMT_2, b);
+
+        return INV_SUCCESS;
+}
+
+static int mpu6050_set_fp_mode(void *mlsl_handle,
+                                struct ext_slave_platform_data *pdata)
+{
+        unsigned char b;
+        struct mpu6050_private_data *private_data =
+                        (struct mpu6050_private_data *)pdata->private_data;
+        /* Resetting the cycle bit and LPA wake up freq */
+        inv_serial_read(mlsl_handle, pdata->address,
+                        MPUREG_PWR_MGMT_1, 1, &b);
+        b &= ~BIT_CYCLE & ~BIT_PD_PTAT;
+        inv_serial_single_write(mlsl_handle, pdata->address,
+                                MPUREG_PWR_MGMT_1, b);
+        inv_serial_read(mlsl_handle, pdata->address,
+                        MPUREG_PWR_MGMT_2, 1, &b);
+        b &= ~BITS_LPA_WAKE_CTRL;
+        inv_serial_single_write(mlsl_handle, pdata->address,
+                                MPUREG_PWR_MGMT_2, b);
+        /* Resetting the duration setting for fp mode */
+        b = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB;
+        inv_serial_single_write(mlsl_handle, pdata->address,
+                                MPUREG_ACCEL_MOT_DUR, b);
+
+        return INV_SUCCESS;
+}
+/**
+ * Record the odr for use in computing duration values.
+ *
+ * @param config Config to set, suspend or resume structure
+ * @param odr output data rate in 1/1000 hz
+ */
+static int mpu6050_set_odr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct mpu6050_config *config, long apply, long odr)
+{
+        int result;
+        unsigned char b;
+        unsigned char lpa_freq = 1; /* Default value */
+        long base;
+        int total_divider;
+        struct mpu6050_private_data *private_data =
+                        (struct mpu6050_private_data *)pdata->private_data;
+        struct mldl_cfg *mldl_cfg_ref =
+                        (struct mldl_cfg *)private_data->mldl_cfg_ref;
+
+        if (mldl_cfg_ref) {
+                base = 1000 *
+                        inv_mpu_get_sampling_rate_hz(mldl_cfg_ref->mpu_gyro_cfg)
+                        * (mldl_cfg_ref->mpu_gyro_cfg->divider + 1);
+        } else {
+                /* have no reference to mldl_cfg => assume base rate is 1000 */
+                base = 1000000L;
+        }
+
+        if (odr != 0) {
+                total_divider = (base / odr) - 1;
+                /* final odr MAY be different from requested odr due to
+                   integer truncation */
+                config->odr = base / (total_divider + 1);
+        } else {
+                config->odr = 0;
+                return 0;
+        }
+
+        /* if the DMP and/or gyros are on, don't set the ODR =>
+           the DMP/gyro mldl_cfg->divider setting will handle it */
+        if (apply
+            && (mldl_cfg_ref &&
+            !(mldl_cfg_ref->inv_mpu_cfg->requested_sensors &
+                    INV_DMP_PROCESSOR))) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        MPUREG_SMPLRT_DIV,
+                                        (unsigned char)total_divider);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGI("ODR : %d mHz\n", config->odr);
+        }
+        /* Decide whether to put accel in LP mode or pull out of LP mode
+           based on the odr. */
+        switch (odr) {
+        case 1000:
+                lpa_freq = BITS_LPA_WAKE_1HZ;
+                break;
+        case 2000:
+                lpa_freq = BITS_LPA_WAKE_2HZ;
+                break;
+        case 10000:
+                lpa_freq = BITS_LPA_WAKE_10HZ;
+                break;
+        case 40000:
+                lpa_freq = BITS_LPA_WAKE_40HZ;
+                break;
+        default:
+                inv_serial_read(mlsl_handle, pdata->address,
+                                MPUREG_PWR_MGMT_1, 1, &b);
+                b &= BIT_CYCLE;
+                if (b == BIT_CYCLE) {
+                        MPL_LOGI(" Accel LP - > FP mode. \n ");
+                        mpu6050_set_fp_mode(mlsl_handle, pdata);
+                }
+        }
+        /* If lpa_freq default value was changed, set into LP mode */
+        if (lpa_freq != 1) {
+                MPL_LOGI(" Accel FP - > LP mode. \n ");
+                mpu6050_set_lp_mode(mlsl_handle, pdata, lpa_freq);
+        }
+        return 0;
+}
+
+static int mpu6050_set_fsr(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct mpu6050_config *config, long apply, long fsr)
+{
+        unsigned char fsr_mask;
+        int result;
+
+        if (fsr <= 2000) {
+                config->fsr = 2000;
+                fsr_mask = 0x00;
+        } else if (fsr <= 4000) {
+                config->fsr = 4000;
+                fsr_mask = 0x08;
+        } else if (fsr <= 8000) {
+                config->fsr = 8000;
+                fsr_mask = 0x10;
+        } else { /* fsr = [8001, oo) */
+                config->fsr = 16000;
+                fsr_mask = 0x18;
+        }
+
+        if (apply) {
+                unsigned char reg;
+                result = inv_serial_read(mlsl_handle, pdata->address,
+                                         MPUREG_ACCEL_CONFIG, 1, &reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 MPUREG_ACCEL_CONFIG,
+                                                 reg | fsr_mask);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("FSR: %d\n", config->fsr);
+        }
+        return 0;
+}
+
+static int mpu6050_set_irq(void *mlsl_handle,
+                          struct ext_slave_platform_data *pdata,
+                          struct mpu6050_config *config, long apply,
+                          long irq_type)
+{
+        int result;
+        unsigned char reg_int_cfg;
+
+        switch (irq_type) {
+        case MPU_SLAVE_IRQ_TYPE_DATA_READY:
+                config->irq_type = irq_type;
+                reg_int_cfg = BIT_RAW_RDY_EN;
+                break;
+        /* todo: add MOTION, NO_MOTION, and FREEFALL */
+        case MPU_SLAVE_IRQ_TYPE_NONE:
+                /* Do nothing, not even set the interrupt because it is
+                   shared with the gyro */
+                config->irq_type = irq_type;
+                return 0;
+        default:
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        if (apply) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                                 MPUREG_INT_ENABLE,
+                                                 reg_int_cfg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("irq_type: %d\n", config->irq_type);
+        }
+
+        return 0;
+}
+
+static int mpu6050_set_ths(void *mlsl_handle,
+                          struct ext_slave_platform_data *slave,
+                          struct mpu6050_config *config, long apply, long ths)
+{
+        if (ths < 0)
+                ths = 0;
+
+        config->ths = ths;
+        MPL_LOGV("THS: %d\n", config->ths);
+        return 0;
+}
+
+static int mpu6050_set_dur(void *mlsl_handle,
+                          struct ext_slave_platform_data *slave,
+                          struct mpu6050_config *config, long apply, long dur)
+{
+        if (dur < 0)
+                dur = 0;
+
+        config->dur = dur;
+        MPL_LOGV("DUR: %d\n", config->dur);
+        return 0;
+}
+
+
+static int mpu6050_init(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        int result;
+        struct mpu6050_private_data *private_data;
+
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+
+        result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                                 false, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->resume,
+                                 false, 200000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                                 false, 2000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                                 false, 2000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                                 false, MPU_SLAVE_IRQ_TYPE_NONE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->resume,
+                                 false, MPU_SLAVE_IRQ_TYPE_NONE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = mpu6050_set_ths(mlsl_handle, pdata, &private_data->suspend,
+                                 false, 80);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = mpu6050_set_ths(mlsl_handle, pdata, &private_data->resume,
+                                 false, 40);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = mpu6050_set_dur(mlsl_handle, pdata, &private_data->suspend,
+                                 false, 1000);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = mpu6050_set_dur(mlsl_handle, pdata, &private_data->resume,
+                                 false, 2540);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return 0;
+}
+
+static int mpu6050_exit(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        pdata->private_data = NULL;
+        return 0;
+}
+
+static int mpu6050_suspend(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata)
+{
+        unsigned char reg;
+        int result;
+        struct mpu6050_private_data *private_data =
+                        (struct mpu6050_private_data *)pdata->private_data;
+
+        result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                                true, private_data->suspend.odr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                                true, private_data->suspend.irq_type);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 MPUREG_PWR_MGMT_2, 1, &reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        reg |= (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         MPUREG_PWR_MGMT_2, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return 0;
+}
+
+static int mpu6050_resume(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char reg;
+        struct mpu6050_private_data *private_data =
+                (struct mpu6050_private_data *)pdata->private_data;
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 MPUREG_PWR_MGMT_1, 1, &reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        if (reg & BIT_SLEEP) {
+                result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                        MPUREG_PWR_MGMT_1, reg & ~BIT_SLEEP);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        msleep(2);
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                        MPUREG_PWR_MGMT_2, 1, &reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       MPUREG_PWR_MGMT_2, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* settings */
+
+        result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                                 true, private_data->resume.fsr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->resume,
+                                 true, private_data->resume.odr);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->resume,
+                                 true, private_data->resume.irq_type);
+
+        /* motion, no_motion */
+        /* TODO : port these in their respective _set_thrs and _set_dur
+                  functions and use the APPLY paremeter to apply just like
+                  _set_odr, _set_irq, and _set_fsr. */
+        reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_THR_LSB;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         MPUREG_ACCEL_MOT_THR, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        reg = (unsigned char)
+            ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         MPUREG_ACCEL_ZRMOT_THR, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         MPUREG_ACCEL_MOT_DUR, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        reg = (unsigned char)private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         MPUREG_ACCEL_ZRMOT_DUR, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return 0;
+}
+
+static int mpu6050_read(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        unsigned char *data)
+{
+        int result;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 slave->read_reg, slave->read_len, data);
+        return result;
+}
+
+static int mpu6050_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        struct mpu6050_private_data *private_data =
+                (struct mpu6050_private_data *)pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return mpu6050_set_odr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                return mpu6050_set_odr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return mpu6050_set_fsr(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return mpu6050_set_fsr(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                return mpu6050_set_ths(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                return mpu6050_set_ths(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                return mpu6050_set_dur(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                return mpu6050_set_dur(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return mpu6050_set_irq(mlsl_handle, pdata,
+                                      &private_data->suspend,
+                                      data->apply, *((long *)data->data));
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return mpu6050_set_irq(mlsl_handle, pdata,
+                                      &private_data->resume,
+                                      data->apply, *((long *)data->data));
+        case MPU_SLAVE_CONFIG_INTERNAL_REFERENCE:
+                return mpu6050_set_mldl_cfg_ref(mlsl_handle, pdata,
+                                               (struct mldl_cfg *)data->data);
+                break;
+
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return 0;
+}
+
+static int mpu6050_get_config(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata,
+                             struct ext_slave_config *data)
+{
+        struct mpu6050_private_data *private_data =
+                (struct mpu6050_private_data *)pdata->private_data;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.ths;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.ths;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.dur;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.dur;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                    (unsigned long)private_data->resume.irq_type;
+                break;
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return 0;
+}
+
+static struct ext_slave_descr mpu6050_descr = {
+        .init             = mpu6050_init,
+        .exit             = mpu6050_exit,
+        .suspend          = mpu6050_suspend,
+        .resume           = mpu6050_resume,
+        .read             = mpu6050_read,
+        .config           = mpu6050_config,
+        .get_config       = mpu6050_get_config,
+        .name             = "mpu6050",
+        .type             = EXT_SLAVE_TYPE_ACCEL,
+        .id               = ACCEL_ID_MPU6050,
+        .read_reg         = 0x3B,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {2, 0},
+        .trigger          = NULL,
+};
+
+struct ext_slave_descr *mpu6050_get_slave_descr(void)
+{
+        return &mpu6050_descr;
+}
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/mpu6050.h b/drivers/misc/inv_mpu/accel/mpu6050.h
new file mode 100644
index 00000000000..c347bcb4d77
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/mpu6050.h
@@ -0,0 +1,28 @@
+/*
+        $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/>.
+        $
+ */
+
+
+#ifndef __MPU6050_H__
+#define __MPU6050_H__
+
+#include <linux/mpu.h>
+
+struct ext_slave_descr *mpu6050_get_slave_descr(void);
+
+#endif
diff --git a/drivers/misc/inv_mpu/compass/Kconfig b/drivers/misc/inv_mpu/compass/Kconfig
new file mode 100644
index 00000000000..7e6bac87d31
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/Kconfig
@@ -0,0 +1,130 @@
+menuconfig INV_SENSORS_COMPASS
+        bool "Compass Slave Sensors"
+        default y
+        ---help---
+          Say Y here to get to see options for device drivers for various
+          compasses. This option alone does not add any kernel code.
+
+          If you say N, all options in this submenu will be skipped and disabled.
+
+if INV_SENSORS_COMPASS
+
+config MPU_SENSORS_AK8963
+        tristate "AKM ak8963"
+        help
+          This enables support for the AKM ak8963 compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_AK8975
+        tristate "AKM ak8975"
+        help
+          This enables support for the AKM ak8975 compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_AK8972
+        tristate "AKM ak8972"
+        help
+          This enables support for the AKM ak8972 compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_MMC314X
+        tristate "MEMSIC mmc314x"
+        help
+          This enables support for the MEMSIC mmc314x compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_AMI30X
+        tristate "Aichi Steel ami30X"
+        help
+          This enables support for the Aichi Steel ami304/ami305 compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_AMI306
+        tristate "Aichi Steel ami306"
+        help
+          This enables support for the Aichi Steel ami306 compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_HMC5883
+        tristate "Honeywell hmc5883"
+        help
+          This enables support for the Honeywell hmc5883 compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_LSM303DLX_M
+        tristate "ST lsm303dlx"
+        help
+          This enables support for the ST lsm303dlh and lsm303dlm compasses
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_MMC314XMS
+        tristate "MEMSIC mmc314xMS"
+        help
+          This enables support for the MEMSIC mmc314xMS compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_YAS529
+        tristate "Yamaha yas529"
+        depends on INPUT_YAS_MAGNETOMETER
+        help
+          This enables support for the Yamaha yas529 compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_YAS530
+        tristate "Yamaha yas530"
+        help
+          This enables support for the Yamaha yas530 compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_HSCDTD002B
+        tristate "Alps hscdtd002b"
+        help
+          This enables support for the Alps hscdtd002b compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_HSCDTD004A
+        tristate "Alps hscdtd004a"
+        help
+          This enables support for the Alps hscdtd004a compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+endif
diff --git a/drivers/misc/inv_mpu/compass/Makefile b/drivers/misc/inv_mpu/compass/Makefile
new file mode 100644
index 00000000000..d76ce1b06f3
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/Makefile
@@ -0,0 +1,41 @@
+#
+# Compass Slaves MPUxxxx
+#
+obj-$(CONFIG_MPU_SENSORS_AMI30X) += inv_mpu_ami30x.o
+inv_mpu_ami30x-objs +=  ami30x.o
+
+obj-$(CONFIG_MPU_SENSORS_AMI306) += inv_mpu_ami306.o
+inv_mpu_ami306-objs +=  ami306.o
+
+obj-$(CONFIG_MPU_SENSORS_HMC5883) += inv_mpu_hmc5883.o
+inv_mpu_hmc5883-objs += hmc5883.o
+
+obj-$(CONFIG_MPU_SENSORS_LSM303DLX_M) += inv_mpu_lsm303dlx_m.o
+inv_mpu_lsm303dlx_m-objs +=     lsm303dlx_m.o
+
+obj-$(CONFIG_MPU_SENSORS_MMC314X) += inv_mpu_mmc314x.o
+inv_mpu_mmc314x-objs += mmc314x.o
+
+obj-$(CONFIG_MPU_SENSORS_YAS529) += inv_mpu_yas529.o
+inv_mpu_yas529-objs +=  yas529-kernel.o
+
+obj-$(CONFIG_MPU_SENSORS_YAS530) += inv_mpu_yas530.o
+inv_mpu_yas530-objs +=  yas530.o
+
+obj-$(CONFIG_MPU_SENSORS_HSCDTD002B) += inv_mpu_hscdtd002b.o
+inv_mpu_hscdtd002b-objs +=      hscdtd002b.o
+
+obj-$(CONFIG_MPU_SENSORS_HSCDTD004A) += inv_mpu_hscdtd004a.o
+inv_mpu_hscdtd004a-objs +=      hscdtd004a.o
+
+obj-$(CONFIG_MPU_SENSORS_AK8963) += inv_mpu_ak89xx.o
+inv_mpu_ak89xx-objs +=  ak89xx.o
+
+obj-$(CONFIG_MPU_SENSORS_AK8975) += inv_mpu_ak8975.o
+inv_mpu_ak8975-objs +=  ak8975.o
+
+obj-$(CONFIG_MPU_SENSORS_AK8972) += inv_mpu_ak8972.o
+inv_mpu_ak8972-objs +=  ak8972.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
diff --git a/drivers/misc/inv_mpu/compass/ak8972.c b/drivers/misc/inv_mpu/compass/ak8972.c
new file mode 100644
index 00000000000..7eb15b44039
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ak8972.c
@@ -0,0 +1,499 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   ak8972.c
+ *      @brief  Magnetometer setup and handling methods for the AKM AK8972 compass device.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AK8972_REG_ST1  (0x02)
+#define AK8972_REG_HXL  (0x03)
+#define AK8972_REG_ST2  (0x09)
+
+#define AK8972_REG_CNTL (0x0A)
+#define AK8972_REG_ASAX (0x10)
+#define AK8972_REG_ASAY (0x11)
+#define AK8972_REG_ASAZ (0x12)
+
+#define AK8972_CNTL_MODE_POWER_DOWN         (0x00)
+#define AK8972_CNTL_MODE_SINGLE_MEASUREMENT (0x01)
+#define AK8972_CNTL_MODE_FUSE_ROM_ACCESS    (0x0f)
+
+/* -------------------------------------------------------------------------- */
+struct ak8972_config {
+        char asa[COMPASS_NUM_AXES];     /* axis sensitivity adjustment */
+};
+
+struct ak8972_private_data {
+        struct ak8972_config init;
+};
+
+/* -------------------------------------------------------------------------- */
+static int ak8972_init(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char serial_data[COMPASS_NUM_AXES];
+
+        struct ak8972_private_data *private_data;
+        private_data = (struct ak8972_private_data *)
+            kzalloc(sizeof(struct ak8972_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AK8972_REG_CNTL,
+                                         AK8972_CNTL_MODE_POWER_DOWN);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Wait at least 100us */
+        udelay(100);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AK8972_REG_CNTL,
+                                         AK8972_CNTL_MODE_FUSE_ROM_ACCESS);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Wait at least 200us */
+        udelay(200);
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AK8972_REG_ASAX,
+                                 COMPASS_NUM_AXES, serial_data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        pdata->private_data = private_data;
+
+        private_data->init.asa[0] = serial_data[0];
+        private_data->init.asa[1] = serial_data[1];
+        private_data->init.asa[2] = serial_data[2];
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AK8972_REG_CNTL,
+                                         AK8972_CNTL_MODE_POWER_DOWN);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        udelay(100);
+        return INV_SUCCESS;
+}
+
+static int ak8972_exit(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+static int ak8972_suspend(void *mlsl_handle,
+                   struct ext_slave_descr *slave,
+                   struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AK8972_REG_CNTL,
+                                    AK8972_CNTL_MODE_POWER_DOWN);
+        msleep(1);              /* wait at least 100us */
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int ak8972_resume(void *mlsl_handle,
+                  struct ext_slave_descr *slave,
+                  struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AK8972_REG_CNTL,
+                                    AK8972_CNTL_MODE_SINGLE_MEASUREMENT);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int ak8972_read(void *mlsl_handle,
+                struct ext_slave_descr *slave,
+                struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+        unsigned char regs[8];
+        unsigned char *stat = &regs[0];
+        unsigned char *stat2 = &regs[7];
+        int result = INV_SUCCESS;
+        int status = INV_SUCCESS;
+
+        result =
+            inv_serial_read(mlsl_handle, pdata->address, AK8972_REG_ST1,
+                            8, regs);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Always return the data and the status registers */
+        memcpy(data, &regs[1], 6);
+        data[6] = regs[0];
+        data[7] = regs[7];
+
+        /*
+         * ST : data ready -
+         * Measurement has been completed and data is ready to be read.
+         */
+        if (*stat & 0x01)
+                status = INV_SUCCESS;
+
+        /*
+         * ST2 : data error -
+         * occurs when data read is started outside of a readable period;
+         * data read would not be correct.
+         * Valid in continuous measurement mode only.
+         * In single measurement mode this error should not occour but we
+         * stil account for it and return an error, since the data would be
+         * corrupted.
+         * DERR bit is self-clearing when ST2 register is read.
+         */
+        if (*stat2 & 0x04)
+                status = INV_ERROR_COMPASS_DATA_ERROR;
+        /*
+         * ST2 : overflow -
+         * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT.
+         * This is likely to happen in presence of an external magnetic
+         * disturbance; it indicates, the sensor data is incorrect and should
+         * be ignored.
+         * An error is returned.
+         * HOFL bit clears when a new measurement starts.
+         */
+        if (*stat2 & 0x08)
+                status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+        /*
+         * ST : overrun -
+         * the previous sample was not fetched and lost.
+         * Valid in continuous measurement mode only.
+         * In single measurement mode this error should not occour and we
+         * don't consider this condition an error.
+         * DOR bit is self-clearing when ST2 or any meas. data register is
+         * read.
+         */
+        if (*stat & 0x02) {
+                /* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */
+                status = INV_SUCCESS;
+        }
+
+        /*
+         * trigger next measurement if:
+         *    - stat is non zero;
+         *    - if stat is zero and stat2 is non zero.
+         * Won't trigger if data is not ready and there was no error.
+         */
+        if (*stat != 0x00 || *stat2 != 0x00) {
+                result = inv_serial_single_write(
+                    mlsl_handle, pdata->address,
+                    AK8972_REG_CNTL, AK8972_CNTL_MODE_SINGLE_MEASUREMENT);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        return status;
+}
+
+static int ak8972_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        int result;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_WRITE_REGISTERS:
+                result = inv_serial_write(mlsl_handle, pdata->address,
+                                          data->len,
+                                          (unsigned char *)data->data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                break;
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static int ak8972_get_config(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata,
+                             struct ext_slave_config *data)
+{
+        struct ak8972_private_data *private_data = pdata->private_data;
+        int result;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_READ_REGISTERS:
+                {
+                        unsigned char *serial_data =
+                            (unsigned char *)data->data;
+                        result =
+                            inv_serial_read(mlsl_handle, pdata->address,
+                                            serial_data[0], data->len - 1,
+                                            &serial_data[1]);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        break;
+                }
+        case MPU_SLAVE_READ_SCALE:
+                {
+                        unsigned char *serial_data =
+                            (unsigned char *)data->data;
+                        serial_data[0] = private_data->init.asa[0];
+                        serial_data[1] = private_data->init.asa[1];
+                        serial_data[2] = private_data->init.asa[2];
+                        result = INV_SUCCESS;
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        break;
+                }
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) = 0;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) = 8000;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_read_trigger ak8972_read_trigger = {
+        /*.reg              = */ 0x0A,
+        /*.value            = */ 0x01
+};
+
+static struct ext_slave_descr ak8972_descr = {
+        .init             = ak8972_init,
+        .exit             = ak8972_exit,
+        .suspend          = ak8972_suspend,
+        .resume           = ak8972_resume,
+        .read             = ak8972_read,
+        .config           = ak8972_config,
+        .get_config       = ak8972_get_config,
+        .name             = "ak8972",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_AK8972,
+        .read_reg         = 0x01,
+        .read_len         = 9,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {39321, 6000},
+        .trigger          = &ak8972_read_trigger,
+};
+
+static
+struct ext_slave_descr *ak8972_get_slave_descr(void)
+{
+        return &ak8972_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ak8972_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ak8972_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct ak8972_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        ak8972_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int ak8972_mod_remove(struct i2c_client *client)
+{
+        struct ak8972_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                ak8972_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id ak8972_mod_id[] = {
+        { "ak8972", COMPASS_ID_AK8972 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, ak8972_mod_id);
+
+static struct i2c_driver ak8972_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = ak8972_mod_probe,
+        .remove = ak8972_mod_remove,
+        .id_table = ak8972_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "ak8972_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init ak8972_mod_init(void)
+{
+        int res = i2c_add_driver(&ak8972_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "ak8972_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit ak8972_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&ak8972_mod_driver);
+}
+
+module_init(ak8972_mod_init);
+module_exit(ak8972_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AK8972 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ak8972_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ak8975.c b/drivers/misc/inv_mpu/compass/ak8975.c
new file mode 100644
index 00000000000..3642e29e89a
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ak8975.c
@@ -0,0 +1,500 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   ak8975.c
+ *      @brief  Magnetometer setup and handling methods for the AKM AK8975,
+ *              AKM AK8975B, and AKM AK8975C compass devices.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AK8975_REG_ST1  (0x02)
+#define AK8975_REG_HXL  (0x03)
+#define AK8975_REG_ST2  (0x09)
+
+#define AK8975_REG_CNTL (0x0A)
+#define AK8975_REG_ASAX (0x10)
+#define AK8975_REG_ASAY (0x11)
+#define AK8975_REG_ASAZ (0x12)
+
+#define AK8975_CNTL_MODE_POWER_DOWN         (0x00)
+#define AK8975_CNTL_MODE_SINGLE_MEASUREMENT (0x01)
+#define AK8975_CNTL_MODE_FUSE_ROM_ACCESS    (0x0f)
+
+/* -------------------------------------------------------------------------- */
+struct ak8975_config {
+        char asa[COMPASS_NUM_AXES];     /* axis sensitivity adjustment */
+};
+
+struct ak8975_private_data {
+        struct ak8975_config init;
+};
+
+/* -------------------------------------------------------------------------- */
+static int ak8975_init(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char serial_data[COMPASS_NUM_AXES];
+
+        struct ak8975_private_data *private_data;
+        private_data = (struct ak8975_private_data *)
+            kzalloc(sizeof(struct ak8975_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AK8975_REG_CNTL,
+                                         AK8975_CNTL_MODE_POWER_DOWN);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Wait at least 100us */
+        udelay(100);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AK8975_REG_CNTL,
+                                         AK8975_CNTL_MODE_FUSE_ROM_ACCESS);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Wait at least 200us */
+        udelay(200);
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AK8975_REG_ASAX,
+                                 COMPASS_NUM_AXES, serial_data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        pdata->private_data = private_data;
+
+        private_data->init.asa[0] = serial_data[0];
+        private_data->init.asa[1] = serial_data[1];
+        private_data->init.asa[2] = serial_data[2];
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AK8975_REG_CNTL,
+                                         AK8975_CNTL_MODE_POWER_DOWN);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        udelay(100);
+        return INV_SUCCESS;
+}
+
+static int ak8975_exit(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+static int ak8975_suspend(void *mlsl_handle,
+                   struct ext_slave_descr *slave,
+                   struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AK8975_REG_CNTL,
+                                    AK8975_CNTL_MODE_POWER_DOWN);
+        msleep(1);              /* wait at least 100us */
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int ak8975_resume(void *mlsl_handle,
+                  struct ext_slave_descr *slave,
+                  struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AK8975_REG_CNTL,
+                                    AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int ak8975_read(void *mlsl_handle,
+                struct ext_slave_descr *slave,
+                struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+        unsigned char regs[8];
+        unsigned char *stat = &regs[0];
+        unsigned char *stat2 = &regs[7];
+        int result = INV_SUCCESS;
+        int status = INV_SUCCESS;
+
+        result =
+            inv_serial_read(mlsl_handle, pdata->address, AK8975_REG_ST1,
+                            8, regs);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Always return the data and the status registers */
+        memcpy(data, &regs[1], 6);
+        data[6] = regs[0];
+        data[7] = regs[7];
+
+        /*
+         * ST : data ready -
+         * Measurement has been completed and data is ready to be read.
+         */
+        if (*stat & 0x01)
+                status = INV_SUCCESS;
+
+        /*
+         * ST2 : data error -
+         * occurs when data read is started outside of a readable period;
+         * data read would not be correct.
+         * Valid in continuous measurement mode only.
+         * In single measurement mode this error should not occour but we
+         * stil account for it and return an error, since the data would be
+         * corrupted.
+         * DERR bit is self-clearing when ST2 register is read.
+         */
+        if (*stat2 & 0x04)
+                status = INV_ERROR_COMPASS_DATA_ERROR;
+        /*
+         * ST2 : overflow -
+         * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT.
+         * This is likely to happen in presence of an external magnetic
+         * disturbance; it indicates, the sensor data is incorrect and should
+         * be ignored.
+         * An error is returned.
+         * HOFL bit clears when a new measurement starts.
+         */
+        if (*stat2 & 0x08)
+                status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+        /*
+         * ST : overrun -
+         * the previous sample was not fetched and lost.
+         * Valid in continuous measurement mode only.
+         * In single measurement mode this error should not occour and we
+         * don't consider this condition an error.
+         * DOR bit is self-clearing when ST2 or any meas. data register is
+         * read.
+         */
+        if (*stat & 0x02) {
+                /* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */
+                status = INV_SUCCESS;
+        }
+
+        /*
+         * trigger next measurement if:
+         *    - stat is non zero;
+         *    - if stat is zero and stat2 is non zero.
+         * Won't trigger if data is not ready and there was no error.
+         */
+        if (*stat != 0x00 || *stat2 != 0x00) {
+                result = inv_serial_single_write(
+                    mlsl_handle, pdata->address,
+                    AK8975_REG_CNTL, AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        return status;
+}
+
+static int ak8975_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        int result;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_WRITE_REGISTERS:
+                result = inv_serial_write(mlsl_handle, pdata->address,
+                                          data->len,
+                                          (unsigned char *)data->data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                break;
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static int ak8975_get_config(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata,
+                             struct ext_slave_config *data)
+{
+        struct ak8975_private_data *private_data = pdata->private_data;
+        int result;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_READ_REGISTERS:
+                {
+                        unsigned char *serial_data =
+                            (unsigned char *)data->data;
+                        result =
+                            inv_serial_read(mlsl_handle, pdata->address,
+                                            serial_data[0], data->len - 1,
+                                            &serial_data[1]);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        break;
+                }
+        case MPU_SLAVE_READ_SCALE:
+                {
+                        unsigned char *serial_data =
+                            (unsigned char *)data->data;
+                        serial_data[0] = private_data->init.asa[0];
+                        serial_data[1] = private_data->init.asa[1];
+                        serial_data[2] = private_data->init.asa[2];
+                        result = INV_SUCCESS;
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        break;
+                }
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) = 0;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) = 8000;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_read_trigger ak8975_read_trigger = {
+        /*.reg              = */ 0x0A,
+        /*.value            = */ 0x01
+};
+
+static struct ext_slave_descr ak8975_descr = {
+        .init             = ak8975_init,
+        .exit             = ak8975_exit,
+        .suspend          = ak8975_suspend,
+        .resume           = ak8975_resume,
+        .read             = ak8975_read,
+        .config           = ak8975_config,
+        .get_config       = ak8975_get_config,
+        .name             = "ak8975",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_AK8975,
+        .read_reg         = 0x01,
+        .read_len         = 10,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {9830, 4000},
+        .trigger          = &ak8975_read_trigger,
+};
+
+static
+struct ext_slave_descr *ak8975_get_slave_descr(void)
+{
+        return &ak8975_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ak8975_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ak8975_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct ak8975_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        ak8975_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int ak8975_mod_remove(struct i2c_client *client)
+{
+        struct ak8975_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                ak8975_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id ak8975_mod_id[] = {
+        { "ak8975", COMPASS_ID_AK8975 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, ak8975_mod_id);
+
+static struct i2c_driver ak8975_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = ak8975_mod_probe,
+        .remove = ak8975_mod_remove,
+        .id_table = ak8975_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "ak8975_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init ak8975_mod_init(void)
+{
+        int res = i2c_add_driver(&ak8975_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "ak8975_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit ak8975_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&ak8975_mod_driver);
+}
+
+module_init(ak8975_mod_init);
+module_exit(ak8975_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AK8975 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ak8975_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ak89xx.c b/drivers/misc/inv_mpu/compass/ak89xx.c
new file mode 100644
index 00000000000..d15b0b8dbe6
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ak89xx.c
@@ -0,0 +1,522 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   ak89xx.c
+ *      @brief  Magnetometer setup and handling methods for the AKM
+ *              compass devices.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AK89XX_REG_ST1     (0x02)
+#define AK89XX_REG_HXL     (0x03)
+#define AK89XX_REG_ST2     (0x09)
+
+#define AK89XX_REG_CNTL    (0x0A)
+#define AK89XX_REG_ASAX    (0x10)
+#define AK89XX_REG_ASAY    (0x11)
+#define AK89XX_REG_ASAZ    (0x12)
+
+#define AK89XX_CNTL_MODE_POWER_DOWN         (0x00)
+#define AK89XX_CNTL_MODE_SINGLE_MEASUREMENT (0x01)
+#define AK89XX_CNTL_MODE_FUSE_ROM_ACCESS    (0x0f)
+
+/* -------------------------------------------------------------------------- */
+struct ak89xx_config {
+        char asa[COMPASS_NUM_AXES];     /* axis sensitivity adjustment */
+};
+
+struct ak89xx_private_data {
+        struct ak89xx_config init;
+};
+
+/* -------------------------------------------------------------------------- */
+static int ak89xx_init(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char serial_data[COMPASS_NUM_AXES];
+
+        struct ak89xx_private_data *private_data;
+        private_data = (struct ak89xx_private_data *)
+            kzalloc(sizeof(struct ak89xx_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AK89XX_REG_CNTL,
+                                         AK89XX_CNTL_MODE_POWER_DOWN);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Wait at least 100us */
+        udelay(100);
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AK89XX_REG_CNTL,
+                                         AK89XX_CNTL_MODE_FUSE_ROM_ACCESS);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Wait at least 200us */
+        udelay(200);
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AK89XX_REG_ASAX,
+                                 COMPASS_NUM_AXES, serial_data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        pdata->private_data = private_data;
+
+        private_data->init.asa[0] = serial_data[0];
+        private_data->init.asa[1] = serial_data[1];
+        private_data->init.asa[2] = serial_data[2];
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AK89XX_REG_CNTL,
+                                         AK89XX_CNTL_MODE_POWER_DOWN);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        udelay(100);
+        return INV_SUCCESS;
+}
+
+static int ak89xx_exit(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+static int ak89xx_suspend(void *mlsl_handle,
+                   struct ext_slave_descr *slave,
+                   struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AK89XX_REG_CNTL,
+                                    AK89XX_CNTL_MODE_POWER_DOWN);
+        msleep(1);              /* wait at least 100us */
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int ak89xx_resume(void *mlsl_handle,
+                  struct ext_slave_descr *slave,
+                  struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AK89XX_REG_CNTL,
+                                    AK89XX_CNTL_MODE_SINGLE_MEASUREMENT);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int ak89xx_read(void *mlsl_handle,
+                struct ext_slave_descr *slave,
+                struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+        unsigned char regs[8];
+        unsigned char *stat = &regs[0];
+        unsigned char *stat2 = &regs[7];
+        int result = INV_SUCCESS;
+        int status = INV_SUCCESS;
+
+        result =
+            inv_serial_read(mlsl_handle, pdata->address, AK89XX_REG_ST1,
+                            8, regs);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Always return the data and the status registers */
+        memcpy(data, &regs[1], 6);
+        data[6] = regs[0];
+        data[7] = regs[7];
+
+        /*
+         * ST : data ready -
+         * Measurement has been completed and data is ready to be read.
+         */
+        if (*stat & 0x01)
+                status = INV_SUCCESS;
+
+        /*
+         * ST2 : data error -
+         * occurs when data read is started outside of a readable period;
+         * data read would not be correct.
+         * Valid in continuous measurement mode only.
+         * In single measurement mode this error should not occour but we
+         * stil account for it and return an error, since the data would be
+         * corrupted.
+         * DERR bit is self-clearing when ST2 register is read.
+         *
+         * This bit is always zero on the AK8963.
+         */
+        if (*stat2 & 0x04)
+                status = INV_ERROR_COMPASS_DATA_ERROR;
+        /*
+         * ST2 : overflow -
+         * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT.
+         * This is likely to happen in presence of an external magnetic
+         * disturbance; it indicates, the sensor data is incorrect and should
+         * be ignored.
+         * An error is returned.
+         * HOFL bit clears when a new measurement starts.
+         */
+        if (*stat2 & 0x08)
+                status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+        /*
+         * ST : overrun -
+         * the previous sample was not fetched and lost.
+         * Valid in continuous measurement mode only.
+         * In single measurement mode this error should not occour and we
+         * don't consider this condition an error.
+         * DOR bit is self-clearing when ST2 or any meas. data register is
+         * read.
+         */
+        if (*stat & 0x02) {
+                /* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */
+                status = INV_SUCCESS;
+        }
+
+        /*
+         * trigger next measurement if:
+         *    - stat is non zero;
+         *    - if stat is zero and stat2 is non zero.
+         * Won't trigger if data is not ready and there was no error.
+         */
+        if (*stat != 0x00 || *stat2 != 0x00) {
+                result = inv_serial_single_write(
+                    mlsl_handle, pdata->address,
+                    AK89XX_REG_CNTL, AK89XX_CNTL_MODE_SINGLE_MEASUREMENT);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        return status;
+}
+
+static int ak89xx_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        int result;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_WRITE_REGISTERS:
+                result = inv_serial_write(mlsl_handle, pdata->address,
+                                          data->len,
+                                          (unsigned char *)data->data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                break;
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static int ak89xx_get_config(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata,
+                             struct ext_slave_config *data)
+{
+        struct ak89xx_private_data *private_data = pdata->private_data;
+        int result;
+        if (!data->data)
+                return INV_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_READ_REGISTERS:
+                {
+                        unsigned char *serial_data =
+                            (unsigned char *)data->data;
+                        result =
+                            inv_serial_read(mlsl_handle, pdata->address,
+                                            serial_data[0], data->len - 1,
+                                            &serial_data[1]);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        break;
+                }
+        case MPU_SLAVE_READ_SCALE:
+                {
+                        unsigned char *serial_data =
+                            (unsigned char *)data->data;
+                        serial_data[0] = private_data->init.asa[0];
+                        serial_data[1] = private_data->init.asa[1];
+                        serial_data[2] = private_data->init.asa[2];
+                        result = INV_SUCCESS;
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        break;
+                }
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) = 0;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) = 8000;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_read_trigger ak89xx_read_trigger = {
+        /*.reg              = */ 0x0A,
+        /*.value            = */ 0x01
+};
+
+static struct ext_slave_descr ak89xx_descr = {
+        .init             = ak89xx_init,
+        .exit             = ak89xx_exit,
+        .suspend          = ak89xx_suspend,
+        .resume           = ak89xx_resume,
+        .read             = ak89xx_read,
+        .config           = ak89xx_config,
+        .get_config       = ak89xx_get_config,
+        .name             = "ak89xx",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_AK8963,
+        .read_reg         = 0x01,
+        .read_len         = 10,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {9830, 4000},
+        .trigger          = &ak89xx_read_trigger,
+};
+
+static
+struct ext_slave_descr *ak89xx_get_slave_descr(void)
+{
+        return &ak89xx_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ak89xx_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ak89xx_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct ak89xx_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        /* Override default values based on model. */
+        ak89xx_descr.id = devid->driver_data;
+        /* ak89xx_descr.name = devid->name; */
+        switch (ak89xx_descr.id) {
+        case COMPASS_ID_AK8963:
+                break;
+        case COMPASS_ID_AK8972:
+                ak89xx_descr.read_len = 9;
+                ak89xx_descr.range.mantissa = 39321;
+                ak89xx_descr.range.fraction = 6000;
+                break;
+        case COMPASS_ID_AK8975:
+                break;
+        default:
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        ak89xx_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int ak89xx_mod_remove(struct i2c_client *client)
+{
+        struct ak89xx_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                ak89xx_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id ak89xx_mod_id[] = {
+        { "ak8963", COMPASS_ID_AK8963 },
+        { "ak8972", COMPASS_ID_AK8972 },
+        { "ak8975", COMPASS_ID_AK8975 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, ak89xx_mod_id);
+
+static struct i2c_driver ak89xx_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = ak89xx_mod_probe,
+        .remove = ak89xx_mod_remove,
+        .id_table = ak89xx_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "ak89xx_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init ak89xx_mod_init(void)
+{
+        int res = i2c_add_driver(&ak89xx_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "ak89xx_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit ak89xx_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&ak89xx_mod_driver);
+}
+
+module_init(ak89xx_mod_init);
+module_exit(ak89xx_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AKM AK89XX sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ak89xx_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ami306.c b/drivers/misc/inv_mpu/compass/ami306.c
new file mode 100644
index 00000000000..f645457d161
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ami306.c
@@ -0,0 +1,1020 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   ami306.c
+ *      @brief  Magnetometer setup and handling methods for Aichi AMI306
+ *              compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include "ami_hw.h"
+#include "ami_sensor_def.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AMI306_REG_DATAX                (0x10)
+#define AMI306_REG_STAT1                (0x18)
+#define AMI306_REG_CNTL1                (0x1B)
+#define AMI306_REG_CNTL2                (0x1C)
+#define AMI306_REG_CNTL3                (0x1D)
+#define AMI306_REG_CNTL4_1              (0x5C)
+#define AMI306_REG_CNTL4_2              (0x5D)
+
+#define AMI306_BIT_CNTL1_PC1            (0x80)
+#define AMI306_BIT_CNTL1_ODR1           (0x10)
+#define AMI306_BIT_CNTL1_FS1            (0x02)
+
+#define AMI306_BIT_CNTL2_IEN            (0x10)
+#define AMI306_BIT_CNTL2_DREN           (0x08)
+#define AMI306_BIT_CNTL2_DRP            (0x04)
+#define AMI306_BIT_CNTL3_F0RCE          (0x40)
+
+#define AMI_FINE_MAX                    (96)
+#define AMI_STANDARD_OFFSET             (0x800)
+#define AMI_GAIN_COR_DEFAULT            (1000)
+
+/* -------------------------------------------------------------------------- */
+struct ami306_private_data {
+        int isstandby;
+        unsigned char fine[3];
+        struct ami_sensor_parametor param;
+        struct ami_win_parameter win;
+};
+
+/* -------------------------------------------------------------------------- */
+static inline unsigned short little_u8_to_u16(unsigned char *p_u8)
+{
+        return p_u8[0] | (p_u8[1] << 8);
+}
+
+static int ami306_set_bits8(void *mlsl_handle,
+                            struct ext_slave_platform_data *pdata,
+                            unsigned char reg, unsigned char bits)
+{
+        int result;
+        unsigned char buf;
+
+        result = inv_serial_read(mlsl_handle, pdata->address, reg, 1, &buf);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        buf |= bits;
+        result = inv_serial_single_write(mlsl_handle, pdata->address, reg, buf);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+static int ami306_wait_data_ready(void *mlsl_handle,
+                                  struct ext_slave_platform_data *pdata,
+                                  unsigned long usecs, unsigned long times)
+{
+        int result = 0;
+        unsigned char buf;
+
+        for (; 0 < times; --times) {
+                udelay(usecs);
+                result = inv_serial_read(mlsl_handle, pdata->address,
+                                         AMI_REG_STA1, 1, &buf);
+                if (buf & AMI_STA1_DRDY_BIT)
+                        return 0;
+                else if (buf & AMI_STA1_DOR_BIT)
+                        return INV_ERROR_COMPASS_DATA_OVERFLOW;
+        }
+        return INV_ERROR_COMPASS_DATA_NOT_READY;
+}
+
+static int ami306_read_raw_data(void *mlsl_handle,
+                                struct ext_slave_platform_data *pdata,
+                                short dat[3])
+{
+        int result;
+        unsigned char buf[6];
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_REG_DATAX, sizeof(buf), buf);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dat[0] = little_u8_to_u16(&buf[0]);
+        dat[1] = little_u8_to_u16(&buf[2]);
+        dat[2] = little_u8_to_u16(&buf[4]);
+        return result;
+}
+
+#define AMI_WAIT_DATAREADY_RETRY                3       /* retry times */
+#define AMI_DRDYWAIT                            800     /* u(micro) sec */
+static int ami306_force_mesurement(void *mlsl_handle,
+                                   struct ext_slave_platform_data *pdata,
+                                   short ver[3])
+{
+        int result;
+        int status;
+        result = ami306_set_bits8(mlsl_handle, pdata,
+                                  AMI_REG_CTRL3, AMI_CTRL3_FORCE_BIT);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = ami306_wait_data_ready(mlsl_handle, pdata,
+                                        AMI_DRDYWAIT, AMI_WAIT_DATAREADY_RETRY);
+        if (result && result != INV_ERROR_COMPASS_DATA_OVERFLOW) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /*  READ DATA X,Y,Z */
+        status = ami306_read_raw_data(mlsl_handle, pdata, ver);
+        if (status) {
+                LOG_RESULT_LOCATION(status);
+                return status;
+        }
+
+        return result;
+}
+
+static int ami306_mea(void *mlsl_handle,
+                      struct ext_slave_platform_data *pdata, short val[3])
+{
+        int result = ami306_force_mesurement(mlsl_handle, pdata, val);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        val[0] += AMI_STANDARD_OFFSET;
+        val[1] += AMI_STANDARD_OFFSET;
+        val[2] += AMI_STANDARD_OFFSET;
+        return result;
+}
+
+static int ami306_write_offset(void *mlsl_handle,
+                               struct ext_slave_platform_data *pdata,
+                               unsigned char *fine)
+{
+        int result = 0;
+        unsigned char dat[3];
+        dat[0] = AMI_REG_OFFX;
+        dat[1] = 0x7f & fine[0];
+        dat[2] = 0;
+        result = inv_serial_write(mlsl_handle, pdata->address,
+                                  sizeof(dat), dat);
+        dat[0] = AMI_REG_OFFY;
+        dat[1] = 0x7f & fine[1];
+        dat[2] = 0;
+        result = inv_serial_write(mlsl_handle, pdata->address,
+                                  sizeof(dat), dat);
+        dat[0] = AMI_REG_OFFZ;
+        dat[1] = 0x7f & fine[2];
+        dat[2] = 0;
+        result = inv_serial_write(mlsl_handle, pdata->address,
+                                  sizeof(dat), dat);
+        return result;
+}
+
+static int ami306_start_sensor(void *mlsl_handle,
+                               struct ext_slave_platform_data *pdata)
+{
+        int result = 0;
+        unsigned char buf[3];
+        struct ami306_private_data *private_data = pdata->private_data;
+
+        /* Step 1 */
+        result = ami306_set_bits8(mlsl_handle, pdata,
+                                  AMI_REG_CTRL1,
+                                  AMI_CTRL1_PC1 | AMI_CTRL1_FS1_FORCE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Step 2 */
+        result = ami306_set_bits8(mlsl_handle, pdata,
+                                  AMI_REG_CTRL2, AMI_CTRL2_DREN);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Step 3 */
+        buf[0] = AMI_REG_CTRL4;
+        buf[1] = AMI_CTRL4_HS & 0xFF;
+        buf[2] = (AMI_CTRL4_HS >> 8) & 0xFF;
+        result = inv_serial_write(mlsl_handle, pdata->address,
+                                  sizeof(buf), buf);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Step 4 */
+        result = ami306_write_offset(mlsl_handle, pdata, private_data->fine);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+/**
+ * This function does this.
+ *
+ * @param mlsl_handle this param is this.
+ * @param slave
+ * @param pdata
+ *
+ * @return INV_SUCCESS or non-zero error code.
+ */
+static int ami306_read_param(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata)
+{
+        int result = 0;
+        unsigned char regs[12];
+        struct ami306_private_data *private_data = pdata->private_data;
+        struct ami_sensor_parametor *param = &private_data->param;
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_REG_SENX, sizeof(regs), regs);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Little endian 16 bit registers */
+        param->m_gain.x = little_u8_to_u16(&regs[0]);
+        param->m_gain.y = little_u8_to_u16(&regs[2]);
+        param->m_gain.z = little_u8_to_u16(&regs[4]);
+
+        param->m_interference.xy = regs[7];
+        param->m_interference.xz = regs[6];
+        param->m_interference.yx = regs[9];
+        param->m_interference.yz = regs[8];
+        param->m_interference.zx = regs[11];
+        param->m_interference.zy = regs[10];
+
+        param->m_offset.x = AMI_STANDARD_OFFSET;
+        param->m_offset.y = AMI_STANDARD_OFFSET;
+        param->m_offset.z = AMI_STANDARD_OFFSET;
+
+        param->m_gain_cor.x = AMI_GAIN_COR_DEFAULT;
+        param->m_gain_cor.y = AMI_GAIN_COR_DEFAULT;
+        param->m_gain_cor.z = AMI_GAIN_COR_DEFAULT;
+
+        return result;
+}
+
+static int ami306_initial_b0_adjust(void *mlsl_handle,
+                                    struct ext_slave_descr *slave,
+                                    struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char fine[3] = { 0 };
+        short data[3];
+        int diff[3] = { 0x7fff, 0x7fff, 0x7fff };
+        int fn = 0;
+        int ax = 0;
+        unsigned char buf[3];
+        struct ami306_private_data *private_data = pdata->private_data;
+
+        result = ami306_set_bits8(mlsl_handle, pdata,
+                                  AMI_REG_CTRL2, AMI_CTRL2_DREN);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        buf[0] = AMI_REG_CTRL4;
+        buf[1] = AMI_CTRL4_HS & 0xFF;
+        buf[2] = (AMI_CTRL4_HS >> 8) & 0xFF;
+        result = inv_serial_write(mlsl_handle, pdata->address,
+                                  sizeof(buf), buf);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        for (fn = 0; fn < AMI_FINE_MAX; ++fn) { /* fine 0 -> 95 */
+                fine[0] = fine[1] = fine[2] = fn;
+                result = ami306_write_offset(mlsl_handle, pdata, fine);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                result = ami306_force_mesurement(mlsl_handle, pdata, data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("[%d] x:%-5d y:%-5d z:%-5d\n",
+                         fn, data[0], data[1], data[2]);
+
+                for (ax = 0; ax < 3; ax++) {
+                        /* search point most close to zero. */
+                        if (diff[ax] > abs(data[ax])) {
+                                private_data->fine[ax] = fn;
+                                diff[ax] = abs(data[ax]);
+                        }
+                }
+        }
+        MPL_LOGV("fine x:%-5d y:%-5d z:%-5d\n",
+                 private_data->fine[0], private_data->fine[1],
+                 private_data->fine[2]);
+
+        result = ami306_write_offset(mlsl_handle, pdata, private_data->fine);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Software Reset */
+        result = ami306_set_bits8(mlsl_handle, pdata,
+                                  AMI_REG_CTRL3, AMI_CTRL3_SRST_BIT);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return result;
+}
+
+#define SEH_RANGE_MIN 100
+#define SEH_RANGE_MAX 3950
+static int ami306_search_offset(void *mlsl_handle,
+                                struct ext_slave_descr *slave,
+                                struct ext_slave_platform_data *pdata)
+{
+        int result;
+        int axis;
+        unsigned char regs[6];
+        unsigned char run_flg[3] = { 1, 1, 1 };
+        unsigned char fine[3];
+        unsigned char win_range_fine[3];
+        unsigned short fine_output[3];
+        short val[3];
+        unsigned short cnt[3] = { 0 };
+        struct ami306_private_data *private_data = pdata->private_data;
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_FINEOUTPUT_X, sizeof(regs), regs);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        fine_output[0] = little_u8_to_u16(&regs[0]);
+        fine_output[1] = little_u8_to_u16(&regs[2]);
+        fine_output[2] = little_u8_to_u16(&regs[4]);
+
+        for (axis = 0; axis < 3; ++axis) {
+                if (fine_output[axis] == 0) {
+                        MPL_LOGV("error fine_output %d  axis:%d\n",
+                                 __LINE__, axis);
+                        return -1;
+                }
+                /*  fines per a window */
+                win_range_fine[axis] = (SEH_RANGE_MAX - SEH_RANGE_MIN)
+                    / fine_output[axis];
+        }
+
+        /* get current fine */
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_REG_OFFX, 2, &regs[0]);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_REG_OFFY, 2, &regs[2]);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_REG_OFFZ, 2, &regs[4]);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        fine[0] = (unsigned char)(regs[0] & 0x7f);
+        fine[1] = (unsigned char)(regs[2] & 0x7f);
+        fine[2] = (unsigned char)(regs[4] & 0x7f);
+
+        while (run_flg[0] == 1 || run_flg[1] == 1 || run_flg[2] == 1) {
+
+                result = ami306_mea(mlsl_handle, pdata, val);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGV("val  x:%-5d y:%-5d z:%-5d\n", val[0], val[1], val[2]);
+                MPL_LOGV("now fine x:%-5d y:%-5d z:%-5d\n",
+                         fine[0], fine[1], fine[2]);
+
+                for (axis = 0; axis < 3; ++axis) {
+                        if (axis == 0) {        /* X-axis is reversed */
+                                val[axis] = 0x0FFF & ~val[axis];
+                        }
+                        if (val[axis] < SEH_RANGE_MIN) {
+                                /* At the case of less low limmit. */
+                                fine[axis] -= win_range_fine[axis];
+                                MPL_LOGV("min : fine=%d diff=%d\n",
+                                         fine[axis], win_range_fine[axis]);
+                        }
+                        if (val[axis] > SEH_RANGE_MAX) {
+                                /* At the case of over high limmit. */
+                                fine[axis] += win_range_fine[axis];
+                                MPL_LOGV("max : fine=%d diff=%d\n",
+                                         fine[axis], win_range_fine[axis]);
+                        }
+                        if (SEH_RANGE_MIN <= val[axis] &&
+                            val[axis] <= SEH_RANGE_MAX) {
+                                /* In the current window. */
+                                int diff_fine =
+                                    (val[axis] - AMI_STANDARD_OFFSET) /
+                                    fine_output[axis];
+                                fine[axis] += diff_fine;
+                                run_flg[axis] = 0;
+                                MPL_LOGV("mid : fine=%d diff=%d\n",
+                                         fine[axis], diff_fine);
+                        }
+
+                        if (!(0 <= fine[axis] && fine[axis] < AMI_FINE_MAX)) {
+                                MPL_LOGE("fine err :%d\n", cnt[axis]);
+                                goto out;
+                        }
+                        if (cnt[axis] > 3) {
+                                MPL_LOGE("cnt err :%d\n", cnt[axis]);
+                                goto out;
+                        }
+                        cnt[axis]++;
+                }
+                MPL_LOGV("new fine x:%-5d y:%-5d z:%-5d\n",
+                         fine[0], fine[1], fine[2]);
+
+                /* set current fine */
+                result = ami306_write_offset(mlsl_handle, pdata, fine);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        memcpy(private_data->fine, fine, sizeof(fine));
+out:
+        result = ami306_set_bits8(mlsl_handle, pdata,
+                                  AMI_REG_CTRL3, AMI_CTRL3_SRST_BIT);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        udelay(250 + 50);
+        return 0;
+}
+
+static int ami306_read_win(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata)
+{
+        int result = 0;
+        unsigned char regs[6];
+        struct ami306_private_data *private_data = pdata->private_data;
+        struct ami_win_parameter *win = &private_data->win;
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_REG_OFFOTPX, sizeof(regs), regs);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        win->m_0Gauss_fine.x = (unsigned char)(regs[0] & 0x7f);
+        win->m_0Gauss_fine.y = (unsigned char)(regs[2] & 0x7f);
+        win->m_0Gauss_fine.z = (unsigned char)(regs[4] & 0x7f);
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_REG_OFFX, 2, &regs[0]);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_REG_OFFY, 2, &regs[2]);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_REG_OFFZ, 2, &regs[4]);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        win->m_fine.x = (unsigned char)(regs[0] & 0x7f);
+        win->m_fine.y = (unsigned char)(regs[2] & 0x7f);
+        win->m_fine.z = (unsigned char)(regs[4] & 0x7f);
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI_FINEOUTPUT_X, sizeof(regs), regs);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        win->m_fine_output.x = little_u8_to_u16(&regs[0]);
+        win->m_fine_output.y = little_u8_to_u16(&regs[2]);
+        win->m_fine_output.z = little_u8_to_u16(&regs[4]);
+
+        return result;
+}
+
+static int ami306_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char reg;
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 AMI306_REG_CNTL1, 1, &reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        reg &= ~(AMI306_BIT_CNTL1_PC1 | AMI306_BIT_CNTL1_FS1);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AMI306_REG_CNTL1, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+static int ami306_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        unsigned char regs[] = {
+                AMI306_REG_CNTL4_1,
+                0x7E,
+                0xA0
+        };
+        /* Step1. Set CNTL1 reg to power model active (Write CNTL1:PC1=1) */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AMI306_REG_CNTL1,
+                                         AMI306_BIT_CNTL1_PC1 |
+                                         AMI306_BIT_CNTL1_FS1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Step2. Set CNTL2 reg to DRDY active high and enabled
+           (Write CNTL2:DREN=1) */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AMI306_REG_CNTL2,
+                                         AMI306_BIT_CNTL2_DREN |
+                                         AMI306_BIT_CNTL2_DRP);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Step3. Set CNTL4 reg to for measurement speed: Write CNTL4, 0xA07E */
+        result = inv_serial_write(mlsl_handle, pdata->address,
+                                ARRAY_SIZE(regs), regs);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Step4. skipped */
+
+        /* Step5. Set CNTL3 reg to forced measurement period
+           (Write CNTL3:FORCE=1) */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AMI306_REG_CNTL3,
+                                         AMI306_BIT_CNTL3_F0RCE);
+
+        return result;
+}
+
+static int ami306_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        int result = INV_SUCCESS;
+        int ii;
+        short val[COMPASS_NUM_AXES];
+
+        result = ami306_mea(mlsl_handle, pdata, val);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        for (ii = 0; ii < COMPASS_NUM_AXES; ii++) {
+                val[ii] -= AMI_STANDARD_OFFSET;
+                data[2 * ii] = val[ii] & 0xFF;
+                data[(2 * ii) + 1] = (val[ii] >> 8) & 0xFF;
+        }
+        return result;
+}
+
+static int ami306_init(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        int result;
+        struct ami306_private_data *private_data;
+        private_data = (struct ami306_private_data *)
+            kzalloc(sizeof(struct ami306_private_data), GFP_KERNEL);
+
+        if (!private_data)
+                return INV_ERROR_MEMORY_EXAUSTED;
+
+        pdata->private_data = private_data;
+        result = ami306_set_bits8(mlsl_handle, pdata,
+                                  AMI_REG_CTRL1,
+                                  AMI_CTRL1_PC1 | AMI_CTRL1_FS1_FORCE);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Read Parameters */
+        result = ami306_read_param(mlsl_handle, slave, pdata);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Read Window */
+        result = ami306_initial_b0_adjust(mlsl_handle, slave, pdata);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = ami306_start_sensor(mlsl_handle, pdata);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = ami306_read_win(mlsl_handle, slave, pdata);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         AMI306_REG_CNTL1, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return INV_SUCCESS;
+}
+
+static int ami306_exit(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata)
+{
+        kfree(pdata->private_data);
+        return INV_SUCCESS;
+}
+
+static int ami306_config(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata,
+                         struct ext_slave_config *data)
+{
+        if (!data->data) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        switch (data->key) {
+        case MPU_SLAVE_PARAM:
+        case MPU_SLAVE_WINDOW:
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static int ami306_get_config(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata,
+                             struct ext_slave_config *data)
+{
+        int result;
+        struct ami306_private_data *private_data = pdata->private_data;
+        if (!data->data) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        switch (data->key) {
+        case MPU_SLAVE_PARAM:
+                if (sizeof(struct ami_sensor_parametor) > data->len) {
+                        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                        return INV_ERROR_INVALID_PARAMETER;
+                }
+                if (data->apply) {
+                        result = ami306_read_param(mlsl_handle, slave, pdata);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                }
+                memcpy(data->data, &private_data->param,
+                       sizeof(struct ami_sensor_parametor));
+                break;
+        case MPU_SLAVE_WINDOW:
+                if (sizeof(struct ami_win_parameter) > data->len) {
+                        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                        return INV_ERROR_INVALID_PARAMETER;
+                }
+                if (data->apply) {
+                        result = ami306_read_win(mlsl_handle, slave, pdata);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                }
+                memcpy(data->data, &private_data->win,
+                       sizeof(struct ami_win_parameter));
+                break;
+        case MPU_SLAVE_SEARCHOFFSET:
+                if (sizeof(struct ami_win_parameter) > data->len) {
+                        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                        return INV_ERROR_INVALID_PARAMETER;
+                }
+                if (data->apply) {
+                        result = ami306_search_offset(mlsl_handle,
+                                                      slave, pdata);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        /* Start sensor */
+                        result = ami306_start_sensor(mlsl_handle, pdata);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        result = ami306_read_win(mlsl_handle, slave, pdata);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                }
+                memcpy(data->data, &private_data->win,
+                       sizeof(struct ami_win_parameter));
+                break;
+        case MPU_SLAVE_READWINPARAMS:
+                if (sizeof(struct ami_win_parameter) > data->len) {
+                        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                        return INV_ERROR_INVALID_PARAMETER;
+                }
+                if (data->apply) {
+                        result = ami306_initial_b0_adjust(mlsl_handle,
+                                                          slave, pdata);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        /* Start sensor */
+                        result = ami306_start_sensor(mlsl_handle, pdata);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        result = ami306_read_win(mlsl_handle, slave, pdata);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                }
+                memcpy(data->data, &private_data->win,
+                       sizeof(struct ami_win_parameter));
+                break;
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) = 0;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) = 50000;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        case MPU_SLAVE_READ_SCALE:
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+                return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_read_trigger ami306_read_trigger = {
+        /*.reg              = */ AMI_REG_CTRL3,
+        /*.value            = */ AMI_CTRL3_FORCE_BIT
+};
+
+static struct ext_slave_descr ami306_descr = {
+        .init             = ami306_init,
+        .exit             = ami306_exit,
+        .suspend          = ami306_suspend,
+        .resume           = ami306_resume,
+        .read             = ami306_read,
+        .config           = ami306_config,
+        .get_config       = ami306_get_config,
+        .name             = "ami306",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_AMI306,
+        .read_reg         = 0x0E,
+        .read_len         = 13,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {5461, 3333},
+        .trigger          = &ami306_read_trigger,
+};
+
+static
+struct ext_slave_descr *ami306_get_slave_descr(void)
+{
+        return &ami306_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ami306_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ami306_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct ami306_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        ami306_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int ami306_mod_remove(struct i2c_client *client)
+{
+        struct ami306_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                ami306_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id ami306_mod_id[] = {
+        { "ami306", COMPASS_ID_AMI306 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, ami306_mod_id);
+
+static struct i2c_driver ami306_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = ami306_mod_probe,
+        .remove = ami306_mod_remove,
+        .id_table = ami306_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "ami306_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init ami306_mod_init(void)
+{
+        int res = i2c_add_driver(&ami306_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "ami306_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit ami306_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&ami306_mod_driver);
+}
+
+module_init(ami306_mod_init);
+module_exit(ami306_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AMI306 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ami306_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ami30x.c b/drivers/misc/inv_mpu/compass/ami30x.c
new file mode 100644
index 00000000000..0c4937c4426
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ami30x.c
@@ -0,0 +1,308 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   ami30x.c
+ *      @brief  Magnetometer setup and handling methods for Aichi AMI304
+ *              and AMI305 compass devices.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AMI30X_REG_DATAX (0x10)
+#define AMI30X_REG_STAT1 (0x18)
+#define AMI30X_REG_CNTL1 (0x1B)
+#define AMI30X_REG_CNTL2 (0x1C)
+#define AMI30X_REG_CNTL3 (0x1D)
+
+#define AMI30X_BIT_CNTL1_PC1  (0x80)
+#define AMI30X_BIT_CNTL1_ODR1 (0x10)
+#define AMI30X_BIT_CNTL1_FS1  (0x02)
+
+#define AMI30X_BIT_CNTL2_IEN  (0x10)
+#define AMI30X_BIT_CNTL2_DREN (0x08)
+#define AMI30X_BIT_CNTL2_DRP  (0x04)
+#define AMI30X_BIT_CNTL3_F0RCE (0x40)
+
+/* -------------------------------------------------------------------------- */
+static int ami30x_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char reg;
+        result =
+            inv_serial_read(mlsl_handle, pdata->address, AMI30X_REG_CNTL1,
+                            1, &reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        reg &= ~(AMI30X_BIT_CNTL1_PC1 | AMI30X_BIT_CNTL1_FS1);
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AMI30X_REG_CNTL1, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+static int ami30x_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        /* Set CNTL1 reg to power model active */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AMI30X_REG_CNTL1,
+                                    AMI30X_BIT_CNTL1_PC1 |
+                                    AMI30X_BIT_CNTL1_FS1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Set CNTL2 reg to DRDY active high and enabled */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AMI30X_REG_CNTL2,
+                                    AMI30X_BIT_CNTL2_DREN |
+                                    AMI30X_BIT_CNTL2_DRP);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Set CNTL3 reg to forced measurement period */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    AMI30X_REG_CNTL3, AMI30X_BIT_CNTL3_F0RCE);
+
+        return result;
+}
+
+static int ami30x_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        unsigned char stat;
+        int result = INV_SUCCESS;
+
+        /* Read status reg and check if data ready (DRDY) */
+        result =
+            inv_serial_read(mlsl_handle, pdata->address, AMI30X_REG_STAT1,
+                            1, &stat);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        if (stat & 0x40) {
+                result =
+                    inv_serial_read(mlsl_handle, pdata->address,
+                                    AMI30X_REG_DATAX, 6, (unsigned char *)data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                /* start another measurement */
+                result =
+                    inv_serial_single_write(mlsl_handle, pdata->address,
+                                            AMI30X_REG_CNTL3,
+                                            AMI30X_BIT_CNTL3_F0RCE);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                return INV_SUCCESS;
+        }
+
+        return INV_ERROR_COMPASS_DATA_NOT_READY;
+}
+
+
+/* For AMI305,the range field needs to be modified to {9830.4f} */
+static struct ext_slave_descr ami30x_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = ami30x_suspend,
+        .resume           = ami30x_resume,
+        .read             = ami30x_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "ami30x",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_AMI30X,
+        .read_reg         = 0x06,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {5461, 3333},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *ami30x_get_slave_descr(void)
+{
+        return &ami30x_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ami30x_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ami30x_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct ami30x_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        ami30x_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int ami30x_mod_remove(struct i2c_client *client)
+{
+        struct ami30x_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                ami30x_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id ami30x_mod_id[] = {
+        { "ami30x", COMPASS_ID_AMI30X },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, ami30x_mod_id);
+
+static struct i2c_driver ami30x_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = ami30x_mod_probe,
+        .remove = ami30x_mod_remove,
+        .id_table = ami30x_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "ami30x_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init ami30x_mod_init(void)
+{
+        int res = i2c_add_driver(&ami30x_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "ami30x_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit ami30x_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&ami30x_mod_driver);
+}
+
+module_init(ami30x_mod_init);
+module_exit(ami30x_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AMI30X sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ami30x_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ami_hw.h b/drivers/misc/inv_mpu/compass/ami_hw.h
new file mode 100644
index 00000000000..32a04e91cdc
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ami_hw.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2010 Information System Products Co.,Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef AMI_HW_H
+#define AMI_HW_H
+
+#define AMI_I2C_BUS_NUM                 2
+
+#ifdef  AMI304_MODEL
+#define AMI_I2C_ADDRESS                 0x0F
+#else
+#define AMI_I2C_ADDRESS                 0x0E
+#endif
+
+#define AMI_GPIO_INT                    152
+#define AMI_GPIO_DRDY                   153
+
+/* AMI-Sensor Internal Register Address
+ *(Please refer to AMI-Sensor Specifications)
+ */
+#define AMI_MOREINFO_CMDCODE            0x0d
+#define AMI_WHOIAM_CMDCODE              0x0f
+#define AMI_REG_DATAX                   0x10
+#define AMI_REG_DATAY                   0x12
+#define AMI_REG_DATAZ                   0x14
+#define AMI_REG_STA1                    0x18
+#define AMI_REG_CTRL1                   0x1b
+#define AMI_REG_CTRL2                   0x1c
+#define AMI_REG_CTRL3                   0x1d
+#define AMI_REG_B0X                     0x20
+#define AMI_REG_B0Y                     0x22
+#define AMI_REG_B0Z                     0x24
+#define AMI_REG_CTRL5                   0x40
+#define AMI_REG_CTRL4                   0x5c
+#define AMI_REG_TEMP                    0x60
+#define AMI_REG_DELAYX                  0x68
+#define AMI_REG_DELAYY                  0x6e
+#define AMI_REG_DELAYZ                  0x74
+#define AMI_REG_OFFX                    0x6c
+#define AMI_REG_OFFY                    0x72
+#define AMI_REG_OFFZ                    0x78
+#define AMI_FINEOUTPUT_X                0x90
+#define AMI_FINEOUTPUT_Y                0x92
+#define AMI_FINEOUTPUT_Z                0x94
+#define AMI_REG_SENX                    0x96
+#define AMI_REG_SENY                    0x98
+#define AMI_REG_SENZ                    0x9a
+#define AMI_REG_GAINX                   0x9c
+#define AMI_REG_GAINY                   0x9e
+#define AMI_REG_GAINZ                   0xa0
+#define AMI_GETVERSION_CMDCODE          0xe8
+#define AMI_SERIALNUMBER_CMDCODE        0xea
+#define AMI_REG_B0OTPX                  0xa2
+#define AMI_REG_B0OTPY                  0xb8
+#define AMI_REG_B0OTPZ                  0xce
+#define AMI_REG_OFFOTPX                 0xf8
+#define AMI_REG_OFFOTPY                 0xfa
+#define AMI_REG_OFFOTPZ                 0xfc
+
+/* AMI-Sensor Control Bit  (Please refer to AMI-Sensor Specifications) */
+#define AMI_CTRL1_PC1                   0x80
+#define AMI_CTRL1_FS1_FORCE             0x02
+#define AMI_CTRL1_ODR1                  0x10
+#define AMI_CTRL2_DREN                  0x08
+#define AMI_CTRL2_DRP                   0x04
+#define AMI_CTRL3_FORCE_BIT             0x40
+#define AMI_CTRL3_B0_LO_BIT             0x10
+#define AMI_CTRL3_SRST_BIT              0x80
+#define AMI_CTRL4_HS                    0xa07e
+#define AMI_CTRL4_AB                    0x0001
+#define AMI_STA1_DRDY_BIT               0x40
+#define AMI_STA1_DOR_BIT                0x20
+
+#endif
diff --git a/drivers/misc/inv_mpu/compass/ami_sensor_def.h b/drivers/misc/inv_mpu/compass/ami_sensor_def.h
new file mode 100644
index 00000000000..64032e2bf1f
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ami_sensor_def.h
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2010 Information System Products Co.,Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * Definitions for ami306 compass chip.
+ */
+#ifndef AMI_SENSOR_DEF_H
+#define AMI_SENSOR_DEF_H
+
+/*********************************************************************
+ Constant
+ *********************************************************************/
+#define AMI_OK          0x00                    /**< Normal */
+#define AMI_PARAM_ERR   0x01                    /**< Parameter Error  */
+#define AMI_SEQ_ERR     0x02                    /**< Squence Error  */
+#define AMI_SYSTEM_ERR  0x10                    /**< System Error  */
+#define AMI_BLOCK_ERR   0x20                    /**< Block Error */
+#define AMI_ERROR       0x99                    /**< other Error  */
+
+/*********************************************************************
+ Struct definition
+ *********************************************************************/
+/** axis sensitivity(gain) calibration parameter information  */
+struct ami_vector3d {
+        signed short x;                 /**< X-axis  */
+        signed short y;                 /**< Y-axis  */
+        signed short z;                 /**< Z-axis  */
+};
+
+/** axis interference information  */
+struct ami_interference {
+        /**< Y-axis magnetic field for X-axis correction value  */
+        signed short xy;
+        /**< Z-axis magnetic field for X-axis correction value  */
+        signed short xz;
+        /**< X-axis magnetic field for Y-axis correction value  */
+        signed short yx;
+        /**< Z-axis magnetic field for Y-axis correction value  */
+        signed short yz;
+        /**< X-axis magnetic field for Z-axis correction value  */
+        signed short zx;
+        /**< Y-axis magnetic field for Z-axis correction value  */
+        signed short zy;
+};
+
+/** sensor calibration Parameter information  */
+struct ami_sensor_parametor {
+        /**< geomagnetic field sensor gain  */
+        struct ami_vector3d m_gain;
+        /**< geomagnetic field sensor gain correction parameter  */
+        struct ami_vector3d m_gain_cor;
+        /**< geomagnetic field sensor offset  */
+        struct ami_vector3d m_offset;
+        /**< geomagnetic field sensor axis interference parameter */
+        struct ami_interference m_interference;
+#ifdef AMI_6AXIS
+        /**< acceleration sensor gain  */
+        struct ami_vector3d a_gain;
+        /**< acceleration sensor offset  */
+        struct ami_vector3d a_offset;
+        /**< acceleration sensor deviation  */
+        signed short a_deviation;
+#endif
+};
+
+/** G2-Sensor measurement value (voltage ADC value ) */
+struct ami_sensor_rawvalue {
+        /**< geomagnetic field sensor measurement X-axis value
+        (mounted position/direction reference) */
+        unsigned short mx;
+        /**< geomagnetic field sensor measurement Y-axis value
+        (mounted position/direction reference) */
+        unsigned short my;
+        /**< geomagnetic field sensor measurement Z-axis value
+        (mounted position/direction reference) */
+        unsigned short mz;
+#ifdef AMI_6AXIS
+        /**< acceleration sensor measurement X-axis value
+        (mounted position/direction reference) */
+        unsigned short ax;
+        /**< acceleration sensor measurement Y-axis value
+        (mounted position/direction reference) */
+        unsigned short ay;
+        /**< acceleration sensor measurement Z-axis value
+        (mounted position/direction reference) */
+        unsigned short az;
+#endif
+        /**< temperature sensor measurement value  */
+        unsigned short temperature;
+};
+
+/** Window function Parameter information  */
+struct ami_win_parameter {
+        /**< current fine value  */
+        struct ami_vector3d m_fine;
+        /**< change per 1coarse */
+        struct ami_vector3d m_fine_output;
+        /**< fine value at zero gauss */
+        struct ami_vector3d m_0Gauss_fine;
+#ifdef AMI304
+        /**< current b0 value  */
+        struct ami_vector3d m_b0;
+        /**< current coarse value  */
+        struct ami_vector3d m_coar;
+        /**< change per 1fine */
+        struct ami_vector3d m_coar_output;
+        /**< coarse value at zero gauss */
+        struct ami_vector3d m_0Gauss_coar;
+        /**< delay value  */
+        struct ami_vector3d m_delay;
+#endif
+};
+
+/** AMI chip information ex) 1)model 2)s/n 3)ver 4)more info in the chip */
+struct ami_chipinfo {
+        unsigned short info;    /* INFO 0x0d/0x0e reg.  */
+        unsigned short ver;     /* VER  0xe8/0xe9 reg.  */
+        unsigned short sn;      /* SN   0xea/0xeb reg.  */
+        unsigned char wia;      /* WIA  0x0f      reg.  */
+};
+
+/** AMI Driver Information  */
+struct ami_driverinfo {
+        unsigned char remarks[40];      /* Some Information   */
+        unsigned char datetime[30];     /* compiled date&time */
+        unsigned char ver_major;        /* major version */
+        unsigned char ver_middle;       /* middle.. */
+        unsigned char ver_minor;        /* minor .. */
+};
+
+#endif
diff --git a/drivers/misc/inv_mpu/compass/hmc5883.c b/drivers/misc/inv_mpu/compass/hmc5883.c
new file mode 100644
index 00000000000..fdf2ac00565
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/hmc5883.c
@@ -0,0 +1,391 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   hmc5883.c
+ *      @brief  Magnetometer setup and handling methods for Honeywell
+ *              HMC5883 compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+enum HMC_REG {
+        HMC_REG_CONF_A = 0x0,
+        HMC_REG_CONF_B = 0x1,
+        HMC_REG_MODE = 0x2,
+        HMC_REG_X_M = 0x3,
+        HMC_REG_X_L = 0x4,
+        HMC_REG_Z_M = 0x5,
+        HMC_REG_Z_L = 0x6,
+        HMC_REG_Y_M = 0x7,
+        HMC_REG_Y_L = 0x8,
+        HMC_REG_STATUS = 0x9,
+        HMC_REG_ID_A = 0xA,
+        HMC_REG_ID_B = 0xB,
+        HMC_REG_ID_C = 0xC
+};
+
+enum HMC_CONF_A {
+        HMC_CONF_A_DRATE_MASK = 0x1C,
+        HMC_CONF_A_DRATE_0_75 = 0x00,
+        HMC_CONF_A_DRATE_1_5 = 0x04,
+        HMC_CONF_A_DRATE_3 = 0x08,
+        HMC_CONF_A_DRATE_7_5 = 0x0C,
+        HMC_CONF_A_DRATE_15 = 0x10,
+        HMC_CONF_A_DRATE_30 = 0x14,
+        HMC_CONF_A_DRATE_75 = 0x18,
+        HMC_CONF_A_MEAS_MASK = 0x3,
+        HMC_CONF_A_MEAS_NORM = 0x0,
+        HMC_CONF_A_MEAS_POS = 0x1,
+        HMC_CONF_A_MEAS_NEG = 0x2
+};
+
+enum HMC_CONF_B {
+        HMC_CONF_B_GAIN_MASK = 0xE0,
+        HMC_CONF_B_GAIN_0_9 = 0x00,
+        HMC_CONF_B_GAIN_1_2 = 0x20,
+        HMC_CONF_B_GAIN_1_9 = 0x40,
+        HMC_CONF_B_GAIN_2_5 = 0x60,
+        HMC_CONF_B_GAIN_4_0 = 0x80,
+        HMC_CONF_B_GAIN_4_6 = 0xA0,
+        HMC_CONF_B_GAIN_5_5 = 0xC0,
+        HMC_CONF_B_GAIN_7_9 = 0xE0
+};
+
+enum HMC_MODE {
+        HMC_MODE_MASK = 0x3,
+        HMC_MODE_CONT = 0x0,
+        HMC_MODE_SINGLE = 0x1,
+        HMC_MODE_IDLE = 0x2,
+        HMC_MODE_SLEEP = 0x3
+};
+
+/* -------------------------------------------------------------------------- */
+static int hmc5883_suspend(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    HMC_REG_MODE, HMC_MODE_SLEEP);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(3);
+
+        return result;
+}
+
+static int hmc5883_resume(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        /* Use single measurement mode. Start at sleep state. */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    HMC_REG_MODE, HMC_MODE_SLEEP);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Config normal measurement */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    HMC_REG_CONF_A, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Adjust gain to 307 LSB/Gauss */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    HMC_REG_CONF_B, HMC_CONF_B_GAIN_5_5);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+static int hmc5883_read(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        unsigned char *data)
+{
+        unsigned char stat;
+        int result = INV_SUCCESS;
+        unsigned char tmp;
+        short axisFixed;
+
+        /* Read status reg. to check if data is ready */
+        result =
+            inv_serial_read(mlsl_handle, pdata->address, HMC_REG_STATUS, 1,
+                            &stat);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        if (stat & 0x01) {
+                result =
+                    inv_serial_read(mlsl_handle, pdata->address,
+                                    HMC_REG_X_M, 6, (unsigned char *)data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                /* switch YZ axis to proper position */
+                tmp = data[2];
+                data[2] = data[4];
+                data[4] = tmp;
+                tmp = data[3];
+                data[3] = data[5];
+                data[5] = tmp;
+
+                /*drop data if overflows */
+                if ((data[0] == 0xf0) || (data[2] == 0xf0)
+                    || (data[4] == 0xf0)) {
+                        /* trigger next measurement read */
+                        result =
+                            inv_serial_single_write(mlsl_handle,
+                                                    pdata->address,
+                                                    HMC_REG_MODE,
+                                                    HMC_MODE_SINGLE);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        return INV_ERROR_COMPASS_DATA_OVERFLOW;
+                }
+                /* convert to fixed point and apply sensitivity correction for
+                   Z-axis */
+                axisFixed =
+                    (short)((unsigned short)data[5] +
+                            (unsigned short)data[4] * 256);
+                /* scale up by 1.125 (36/32) */
+                axisFixed = (short)(axisFixed * 36);
+                data[4] = axisFixed >> 8;
+                data[5] = axisFixed & 0xFF;
+
+                axisFixed =
+                    (short)((unsigned short)data[3] +
+                            (unsigned short)data[2] * 256);
+                axisFixed = (short)(axisFixed * 32);
+                data[2] = axisFixed >> 8;
+                data[3] = axisFixed & 0xFF;
+
+                axisFixed =
+                    (short)((unsigned short)data[1] +
+                            (unsigned short)data[0] * 256);
+                axisFixed = (short)(axisFixed * 32);
+                data[0] = axisFixed >> 8;
+                data[1] = axisFixed & 0xFF;
+
+                /* trigger next measurement read */
+                result =
+                    inv_serial_single_write(mlsl_handle, pdata->address,
+                                            HMC_REG_MODE, HMC_MODE_SINGLE);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                return INV_SUCCESS;
+        } else {
+                /* trigger next measurement read */
+                result =
+                    inv_serial_single_write(mlsl_handle, pdata->address,
+                                            HMC_REG_MODE, HMC_MODE_SINGLE);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                return INV_ERROR_COMPASS_DATA_NOT_READY;
+        }
+}
+
+static struct ext_slave_descr hmc5883_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = hmc5883_suspend,
+        .resume           = hmc5883_resume,
+        .read             = hmc5883_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "hmc5883",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_HMC5883,
+        .read_reg         = 0x06,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {10673, 6156},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *hmc5883_get_slave_descr(void)
+{
+        return &hmc5883_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct hmc5883_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int hmc5883_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct hmc5883_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        hmc5883_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int hmc5883_mod_remove(struct i2c_client *client)
+{
+        struct hmc5883_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                hmc5883_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id hmc5883_mod_id[] = {
+        { "hmc5883", COMPASS_ID_HMC5883 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, hmc5883_mod_id);
+
+static struct i2c_driver hmc5883_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = hmc5883_mod_probe,
+        .remove = hmc5883_mod_remove,
+        .id_table = hmc5883_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "hmc5883_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init hmc5883_mod_init(void)
+{
+        int res = i2c_add_driver(&hmc5883_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "hmc5883_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit hmc5883_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&hmc5883_mod_driver);
+}
+
+module_init(hmc5883_mod_init);
+module_exit(hmc5883_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate HMC5883 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("hmc5883_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/hscdtd002b.c b/drivers/misc/inv_mpu/compass/hscdtd002b.c
new file mode 100644
index 00000000000..4f6013cbe3d
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/hscdtd002b.c
@@ -0,0 +1,294 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   hscdtd002b.c
+ *      @brief  Magnetometer setup and handling methods for Alps HSCDTD002B
+ *              compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define COMPASS_HSCDTD002B_STAT          (0x18)
+#define COMPASS_HSCDTD002B_CTRL1         (0x1B)
+#define COMPASS_HSCDTD002B_CTRL2         (0x1C)
+#define COMPASS_HSCDTD002B_CTRL3         (0x1D)
+#define COMPASS_HSCDTD002B_DATAX         (0x10)
+
+/* -------------------------------------------------------------------------- */
+static int hscdtd002b_suspend(void *mlsl_handle,
+                              struct ext_slave_descr *slave,
+                              struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        /* Power mode: stand-by */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD002B_CTRL1, 0x00);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(1);              /* turn-off time */
+
+        return result;
+}
+
+static int hscdtd002b_resume(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        /* Soft reset */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD002B_CTRL3, 0x80);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Force state; Power mode: active */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD002B_CTRL1, 0x82);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Data ready enable */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD002B_CTRL2, 0x08);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(1);              /* turn-on time */
+
+        return result;
+}
+
+static int hscdtd002b_read(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata,
+                           unsigned char *data)
+{
+        unsigned char stat;
+        int result = INV_SUCCESS;
+        int status = INV_SUCCESS;
+
+        /* Read status reg. to check if data is ready */
+        result =
+            inv_serial_read(mlsl_handle, pdata->address,
+                            COMPASS_HSCDTD002B_STAT, 1, &stat);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        if (stat & 0x40) {
+                result =
+                    inv_serial_read(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD002B_DATAX, 6,
+                                    (unsigned char *)data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                status = INV_SUCCESS;
+        } else if (stat & 0x20) {
+                status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+        } else {
+                status = INV_ERROR_COMPASS_DATA_NOT_READY;
+        }
+        /* trigger next measurement read */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD002B_CTRL3, 0x40);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return status;
+}
+
+static struct ext_slave_descr hscdtd002b_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = hscdtd002b_suspend,
+        .resume           = hscdtd002b_resume,
+        .read             = hscdtd002b_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "hscdtd002b",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_HSCDTD002B,
+        .read_reg         = 0x10,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {9830, 4000},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *hscdtd002b_get_slave_descr(void)
+{
+        return &hscdtd002b_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct hscdtd002b_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int hscdtd002b_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct hscdtd002b_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        hscdtd002b_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int hscdtd002b_mod_remove(struct i2c_client *client)
+{
+        struct hscdtd002b_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                hscdtd002b_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id hscdtd002b_mod_id[] = {
+        { "hscdtd002b", COMPASS_ID_HSCDTD002B },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, hscdtd002b_mod_id);
+
+static struct i2c_driver hscdtd002b_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = hscdtd002b_mod_probe,
+        .remove = hscdtd002b_mod_remove,
+        .id_table = hscdtd002b_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "hscdtd002b_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init hscdtd002b_mod_init(void)
+{
+        int res = i2c_add_driver(&hscdtd002b_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "hscdtd002b_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit hscdtd002b_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&hscdtd002b_mod_driver);
+}
+
+module_init(hscdtd002b_mod_init);
+module_exit(hscdtd002b_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate HSCDTD002B sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("hscdtd002b_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/hscdtd004a.c b/drivers/misc/inv_mpu/compass/hscdtd004a.c
new file mode 100644
index 00000000000..f0915599bd2
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/hscdtd004a.c
@@ -0,0 +1,318 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   hscdtd004a.c
+ *      @brief  Magnetometer setup and handling methods for Alps HSCDTD004A
+ *              compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define COMPASS_HSCDTD004A_STAT          (0x18)
+#define COMPASS_HSCDTD004A_CTRL1         (0x1B)
+#define COMPASS_HSCDTD004A_CTRL2         (0x1C)
+#define COMPASS_HSCDTD004A_CTRL3         (0x1D)
+#define COMPASS_HSCDTD004A_DATAX         (0x10)
+
+/* -------------------------------------------------------------------------- */
+
+static int hscdtd004a_suspend(void *mlsl_handle,
+                              struct ext_slave_descr *slave,
+                              struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        /* Power mode: stand-by */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD004A_CTRL1, 0x00);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(1);              /* turn-off time */
+
+        return result;
+}
+
+static int hscdtd004a_init(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char data1, data2[2];
+
+        result = inv_serial_read(mlsl_handle, pdata->address, 0xf, 1, &data1);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_serial_read(mlsl_handle, pdata->address, 0xd, 2, data2);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        if (data1 != 0x49 || data2[0] != 0x45 || data2[1] != 0x54) {
+                LOG_RESULT_LOCATION(INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED);
+                return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
+        }
+        return result;
+}
+
+static int hscdtd004a_resume(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        /* Soft reset */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD004A_CTRL3, 0x80);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Normal state; Power mode: active */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD004A_CTRL1, 0x82);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Data ready enable */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD004A_CTRL2, 0x7C);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(1);              /* turn-on time */
+        return result;
+}
+
+static int hscdtd004a_read(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata,
+                           unsigned char *data)
+{
+        unsigned char stat;
+        int result = INV_SUCCESS;
+        int status = INV_SUCCESS;
+
+        /* Read status reg. to check if data is ready */
+        result =
+            inv_serial_read(mlsl_handle, pdata->address,
+                            COMPASS_HSCDTD004A_STAT, 1, &stat);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        if (stat & 0x48) {
+                result =
+                    inv_serial_read(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD004A_DATAX, 6,
+                                    (unsigned char *)data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                status = INV_SUCCESS;
+        } else if (stat & 0x68) {
+                status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+        } else {
+                status = INV_ERROR_COMPASS_DATA_NOT_READY;
+        }
+        /* trigger next measurement read */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    COMPASS_HSCDTD004A_CTRL3, 0x40);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return status;
+
+}
+
+static struct ext_slave_descr hscdtd004a_descr = {
+        .init             = hscdtd004a_init,
+        .exit             = NULL,
+        .suspend          = hscdtd004a_suspend,
+        .resume           = hscdtd004a_resume,
+        .read             = hscdtd004a_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "hscdtd004a",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_HSCDTD004A,
+        .read_reg         = 0x10,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_LITTLE_ENDIAN,
+        .range            = {9830, 4000},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *hscdtd004a_get_slave_descr(void)
+{
+        return &hscdtd004a_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct hscdtd004a_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int hscdtd004a_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct hscdtd004a_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        hscdtd004a_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int hscdtd004a_mod_remove(struct i2c_client *client)
+{
+        struct hscdtd004a_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                hscdtd004a_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id hscdtd004a_mod_id[] = {
+        { "hscdtd004a", COMPASS_ID_HSCDTD004A },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, hscdtd004a_mod_id);
+
+static struct i2c_driver hscdtd004a_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = hscdtd004a_mod_probe,
+        .remove = hscdtd004a_mod_remove,
+        .id_table = hscdtd004a_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "hscdtd004a_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init hscdtd004a_mod_init(void)
+{
+        int res = i2c_add_driver(&hscdtd004a_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "hscdtd004a_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit hscdtd004a_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&hscdtd004a_mod_driver);
+}
+
+module_init(hscdtd004a_mod_init);
+module_exit(hscdtd004a_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate HSCDTD004A sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("hscdtd004a_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/lsm303dlx_m.c b/drivers/misc/inv_mpu/compass/lsm303dlx_m.c
new file mode 100644
index 00000000000..32f8cdddb00
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/lsm303dlx_m.c
@@ -0,0 +1,395 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   lsm303dlx_m.c
+ *      @brief  Magnetometer setup and handling methods for ST LSM303
+ *              compass.
+ *              This magnetometer device is part of a combo chip with the
+ *              ST LIS331DLH accelerometer and the logic in entirely based
+ *              on the Honeywell HMC5883 magnetometer.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+enum LSM_REG {
+        LSM_REG_CONF_A = 0x0,
+        LSM_REG_CONF_B = 0x1,
+        LSM_REG_MODE = 0x2,
+        LSM_REG_X_M = 0x3,
+        LSM_REG_X_L = 0x4,
+        LSM_REG_Z_M = 0x5,
+        LSM_REG_Z_L = 0x6,
+        LSM_REG_Y_M = 0x7,
+        LSM_REG_Y_L = 0x8,
+        LSM_REG_STATUS = 0x9,
+        LSM_REG_ID_A = 0xA,
+        LSM_REG_ID_B = 0xB,
+        LSM_REG_ID_C = 0xC
+};
+
+enum LSM_CONF_A {
+        LSM_CONF_A_DRATE_MASK = 0x1C,
+        LSM_CONF_A_DRATE_0_75 = 0x00,
+        LSM_CONF_A_DRATE_1_5 = 0x04,
+        LSM_CONF_A_DRATE_3 = 0x08,
+        LSM_CONF_A_DRATE_7_5 = 0x0C,
+        LSM_CONF_A_DRATE_15 = 0x10,
+        LSM_CONF_A_DRATE_30 = 0x14,
+        LSM_CONF_A_DRATE_75 = 0x18,
+        LSM_CONF_A_MEAS_MASK = 0x3,
+        LSM_CONF_A_MEAS_NORM = 0x0,
+        LSM_CONF_A_MEAS_POS = 0x1,
+        LSM_CONF_A_MEAS_NEG = 0x2
+};
+
+enum LSM_CONF_B {
+        LSM_CONF_B_GAIN_MASK = 0xE0,
+        LSM_CONF_B_GAIN_0_9 = 0x00,
+        LSM_CONF_B_GAIN_1_2 = 0x20,
+        LSM_CONF_B_GAIN_1_9 = 0x40,
+        LSM_CONF_B_GAIN_2_5 = 0x60,
+        LSM_CONF_B_GAIN_4_0 = 0x80,
+        LSM_CONF_B_GAIN_4_6 = 0xA0,
+        LSM_CONF_B_GAIN_5_5 = 0xC0,
+        LSM_CONF_B_GAIN_7_9 = 0xE0
+};
+
+enum LSM_MODE {
+        LSM_MODE_MASK = 0x3,
+        LSM_MODE_CONT = 0x0,
+        LSM_MODE_SINGLE = 0x1,
+        LSM_MODE_IDLE = 0x2,
+        LSM_MODE_SLEEP = 0x3
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int lsm303dlx_m_suspend(void *mlsl_handle,
+                              struct ext_slave_descr *slave,
+                              struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    LSM_REG_MODE, LSM_MODE_SLEEP);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(3);
+
+        return result;
+}
+
+static int lsm303dlx_m_resume(void *mlsl_handle,
+                             struct ext_slave_descr *slave,
+                             struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        /* Use single measurement mode. Start at sleep state. */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    LSM_REG_MODE, LSM_MODE_SLEEP);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Config normal measurement */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    LSM_REG_CONF_A, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Adjust gain to 320 LSB/Gauss */
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    LSM_REG_CONF_B, LSM_CONF_B_GAIN_5_5);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+static int lsm303dlx_m_read(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata,
+                           unsigned char *data)
+{
+        unsigned char stat;
+        int result = INV_SUCCESS;
+        short axis_fixed;
+
+        /* Read status reg. to check if data is ready */
+        result =
+            inv_serial_read(mlsl_handle, pdata->address, LSM_REG_STATUS, 1,
+                            &stat);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        if (stat & 0x01) {
+                result =
+                    inv_serial_read(mlsl_handle, pdata->address,
+                                    LSM_REG_X_M, 6, (unsigned char *)data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                /*drop data if overflows */
+                if ((data[0] == 0xf0) || (data[2] == 0xf0)
+                    || (data[4] == 0xf0)) {
+                        /* trigger next measurement read */
+                        result =
+                            inv_serial_single_write(mlsl_handle,
+                                                    pdata->address,
+                                                    LSM_REG_MODE,
+                                                    LSM_MODE_SINGLE);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        return INV_ERROR_COMPASS_DATA_OVERFLOW;
+                }
+                /* convert to fixed point and apply sensitivity correction for
+                   Z-axis */
+                axis_fixed =
+                    (short)((unsigned short)data[5] +
+                            (unsigned short)data[4] * 256);
+                /* scale up by 1.125 (36/32) approximate of 1.122 (320/285) */
+                if (slave->id == COMPASS_ID_LSM303DLM) {
+                        /* NOTE/IMPORTANT:
+                           lsm303dlm compass axis definition doesn't
+                           respect the right hand rule. We invert
+                           the sign of the Z axis to fix that. */
+                        axis_fixed = (short)(-1 * axis_fixed * 36);
+                } else {
+                        axis_fixed = (short)(axis_fixed * 36);
+                }
+                data[4] = axis_fixed >> 8;
+                data[5] = axis_fixed & 0xFF;
+
+                axis_fixed =
+                    (short)((unsigned short)data[3] +
+                            (unsigned short)data[2] * 256);
+                axis_fixed = (short)(axis_fixed * 32);
+                data[2] = axis_fixed >> 8;
+                data[3] = axis_fixed & 0xFF;
+
+                axis_fixed =
+                    (short)((unsigned short)data[1] +
+                            (unsigned short)data[0] * 256);
+                axis_fixed = (short)(axis_fixed * 32);
+                data[0] = axis_fixed >> 8;
+                data[1] = axis_fixed & 0xFF;
+
+                /* trigger next measurement read */
+                result =
+                    inv_serial_single_write(mlsl_handle, pdata->address,
+                                            LSM_REG_MODE, LSM_MODE_SINGLE);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                return INV_SUCCESS;
+        } else {
+                /* trigger next measurement read */
+                result =
+                    inv_serial_single_write(mlsl_handle, pdata->address,
+                                            LSM_REG_MODE, LSM_MODE_SINGLE);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                return INV_ERROR_COMPASS_DATA_NOT_READY;
+        }
+}
+
+static struct ext_slave_descr lsm303dlx_m_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = lsm303dlx_m_suspend,
+        .resume           = lsm303dlx_m_resume,
+        .read             = lsm303dlx_m_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "lsm303dlx_m",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = ID_INVALID,
+        .read_reg         = 0x06,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {10240, 0},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *lsm303dlx_m_get_slave_descr(void)
+{
+        return &lsm303dlx_m_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct lsm303dlx_m_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static const struct i2c_device_id lsm303dlx_m_mod_id[] = {
+        { "lsm303dlh", COMPASS_ID_LSM303DLH },
+        { "lsm303dlm", COMPASS_ID_LSM303DLM },
+        {}
+};
+MODULE_DEVICE_TABLE(i2c, lsm303dlx_m_mod_id);
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int lsm303dlx_m_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct lsm303dlx_m_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+        lsm303dlx_m_descr.id = devid->driver_data;
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        lsm303dlx_m_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int lsm303dlx_m_mod_remove(struct i2c_client *client)
+{
+        struct lsm303dlx_m_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                lsm303dlx_m_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static struct i2c_driver lsm303dlx_m_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = lsm303dlx_m_mod_probe,
+        .remove = lsm303dlx_m_mod_remove,
+        .id_table = lsm303dlx_m_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "lsm303dlx_m_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init lsm303dlx_m_mod_init(void)
+{
+        int res = i2c_add_driver(&lsm303dlx_m_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "lsm303dlx_m_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit lsm303dlx_m_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&lsm303dlx_m_mod_driver);
+}
+
+module_init(lsm303dlx_m_mod_init);
+module_exit(lsm303dlx_m_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate lsm303dlx_m sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("lsm303dlx_m_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/mmc314x.c b/drivers/misc/inv_mpu/compass/mmc314x.c
new file mode 100644
index 00000000000..786fadcc3e4
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/mmc314x.c
@@ -0,0 +1,313 @@
+/*
+        $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 COMPASSDL
+ *
+ *  @{
+ *      @file   mmc314x.c
+ *      @brief  Magnetometer setup and handling methods for the
+ *              MEMSIC MMC314x compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+
+static int reset_int = 1000;
+static int read_count = 1;
+static char reset_mode;         /* in Z-init section */
+
+/* -------------------------------------------------------------------------- */
+#define MMC314X_REG_ST (0x00)
+#define MMC314X_REG_X_MSB (0x01)
+
+#define MMC314X_CNTL_MODE_WAKE_UP (0x01)
+#define MMC314X_CNTL_MODE_SET (0x02)
+#define MMC314X_CNTL_MODE_RESET (0x04)
+
+/* -------------------------------------------------------------------------- */
+
+static int mmc314x_suspend(void *mlsl_handle,
+                           struct ext_slave_descr *slave,
+                           struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        return result;
+}
+
+static int mmc314x_resume(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+
+        int result;
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    MMC314X_REG_ST, MMC314X_CNTL_MODE_RESET);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(10);
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    MMC314X_REG_ST, MMC314X_CNTL_MODE_SET);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(10);
+        read_count = 1;
+        return INV_SUCCESS;
+}
+
+static int mmc314x_read(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        unsigned char *data)
+{
+        int result, ii;
+        short tmp[3];
+        unsigned char tmpdata[6];
+
+        if (read_count > 1000)
+                read_count = 1;
+
+        result =
+            inv_serial_read(mlsl_handle, pdata->address, MMC314X_REG_X_MSB,
+                            6, (unsigned char *)data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        for (ii = 0; ii < 6; ii++)
+                tmpdata[ii] = data[ii];
+
+        for (ii = 0; ii < 3; ii++) {
+                tmp[ii] = (short)((tmpdata[2 * ii] << 8) + tmpdata[2 * ii + 1]);
+                tmp[ii] = tmp[ii] - 4096;
+                tmp[ii] = tmp[ii] * 16;
+        }
+
+        for (ii = 0; ii < 3; ii++) {
+                data[2 * ii] = (unsigned char)(tmp[ii] >> 8);
+                data[2 * ii + 1] = (unsigned char)(tmp[ii]);
+        }
+
+        if (read_count % reset_int == 0) {
+                if (reset_mode) {
+                        result =
+                            inv_serial_single_write(mlsl_handle,
+                                                    pdata->address,
+                                                    MMC314X_REG_ST,
+                                                    MMC314X_CNTL_MODE_RESET);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        reset_mode = 0;
+                        return INV_ERROR_COMPASS_DATA_NOT_READY;
+                } else {
+                        result =
+                            inv_serial_single_write(mlsl_handle,
+                                                    pdata->address,
+                                                    MMC314X_REG_ST,
+                                                    MMC314X_CNTL_MODE_SET);
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        reset_mode = 1;
+                        read_count++;
+                        return INV_ERROR_COMPASS_DATA_NOT_READY;
+                }
+        }
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    MMC314X_REG_ST, MMC314X_CNTL_MODE_WAKE_UP);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        read_count++;
+
+        return INV_SUCCESS;
+}
+
+static struct ext_slave_descr mmc314x_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = mmc314x_suspend,
+        .resume           = mmc314x_resume,
+        .read             = mmc314x_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "mmc314x",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_MMC314X,
+        .read_reg         = 0x01,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {400, 0},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *mmc314x_get_slave_descr(void)
+{
+        return &mmc314x_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct mmc314x_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int mmc314x_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct mmc314x_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        mmc314x_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int mmc314x_mod_remove(struct i2c_client *client)
+{
+        struct mmc314x_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                mmc314x_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id mmc314x_mod_id[] = {
+        { "mmc314x", COMPASS_ID_MMC314X },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, mmc314x_mod_id);
+
+static struct i2c_driver mmc314x_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = mmc314x_mod_probe,
+        .remove = mmc314x_mod_remove,
+        .id_table = mmc314x_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "mmc314x_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init mmc314x_mod_init(void)
+{
+        int res = i2c_add_driver(&mmc314x_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "mmc314x_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit mmc314x_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&mmc314x_mod_driver);
+}
+
+module_init(mmc314x_mod_init);
+module_exit(mmc314x_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate MMC314X sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("mmc314x_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/yas529-kernel.c b/drivers/misc/inv_mpu/compass/yas529-kernel.c
new file mode 100644
index 00000000000..f53223fba64
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/yas529-kernel.c
@@ -0,0 +1,611 @@
+/*
+        $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/>.
+        $
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/*----- YAMAHA YAS529 Registers ------*/
+enum YAS_REG {
+        YAS_REG_CMDR            = 0x00, /* 000 < 5 */
+        YAS_REG_XOFFSETR        = 0x20, /* 001 < 5 */
+        YAS_REG_Y1OFFSETR       = 0x40, /* 010 < 5 */
+        YAS_REG_Y2OFFSETR       = 0x60, /* 011 < 5 */
+        YAS_REG_ICOILR          = 0x80, /* 100 < 5 */
+        YAS_REG_CAL             = 0xA0, /* 101 < 5 */
+        YAS_REG_CONFR           = 0xC0, /* 110 < 5 */
+        YAS_REG_DOUTR           = 0xE0  /* 111 < 5 */
+};
+
+/* -------------------------------------------------------------------------- */
+
+static long a1;
+static long a2;
+static long a3;
+static long a4;
+static long a5;
+static long a6;
+static long a7;
+static long a8;
+static long a9;
+
+/* -------------------------------------------------------------------------- */
+static int yas529_sensor_i2c_write(struct i2c_adapter *i2c_adap,
+                                   unsigned char address,
+                                   unsigned int len, unsigned char *data)
+{
+        struct i2c_msg msgs[1];
+        int res;
+
+        if (NULL == data || NULL == i2c_adap)
+                return -EINVAL;
+
+        msgs[0].addr = address;
+        msgs[0].flags = 0;      /* write */
+        msgs[0].buf = (unsigned char *)data;
+        msgs[0].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 1);
+        if (res < 1)
+                return res;
+        else
+                return 0;
+}
+
+static int yas529_sensor_i2c_read(struct i2c_adapter *i2c_adap,
+                                  unsigned char address,
+                                  unsigned char reg,
+                                  unsigned int len, unsigned char *data)
+{
+        struct i2c_msg msgs[2];
+        int res;
+
+        if (NULL == data || NULL == i2c_adap)
+                return -EINVAL;
+
+        msgs[0].addr = address;
+        msgs[0].flags = I2C_M_RD;
+        msgs[0].buf = data;
+        msgs[0].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 1);
+        if (res < 1)
+                return res;
+        else
+                return 0;
+}
+
+static int yas529_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        return result;
+}
+
+static int yas529_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        unsigned char dummyData[1] = { 0 };
+        unsigned char dummyRegister = 0;
+        unsigned char rawData[6];
+        unsigned char calData[9];
+
+        short xoffset, y1offset, y2offset;
+        short d2, d3, d4, d5, d6, d7, d8, d9;
+
+        /* YAS529 Application Manual MS-3C - Section 4.4.5 */
+        /* =============================================== */
+        /* Step 1 - register initialization */
+        /* zero initialization coil register - "100 00 000" */
+        dummyData[0] = YAS_REG_ICOILR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* zero config register - "110 00 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Step 2 - initialization coil operation */
+        dummyData[0] = YAS_REG_ICOILR | 0x11;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x01;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x12;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x02;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x13;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x03;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x14;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x04;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x15;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x05;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x16;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x06;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x17;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x07;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x10;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Step 3 - rough offset measurement */
+        /* Config register - Measurements results - "110 00 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Measurements command register - Rough offset measurement -
+           "000 00001" */
+        dummyData[0] = YAS_REG_CMDR | 0x01;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(2);              /* wait at least 1.5ms */
+
+        /* Measurement data read */
+        result =
+            yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+                                   dummyRegister, 6, rawData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        xoffset =
+            (short)((unsigned short)rawData[5] +
+                    ((unsigned short)rawData[4] & 0x7) * 256) - 5;
+        if (xoffset < 0)
+                xoffset = 0;
+        y1offset =
+            (short)((unsigned short)rawData[3] +
+                    ((unsigned short)rawData[2] & 0x7) * 256) - 5;
+        if (y1offset < 0)
+                y1offset = 0;
+        y2offset =
+            (short)((unsigned short)rawData[1] +
+                    ((unsigned short)rawData[0] & 0x7) * 256) - 5;
+        if (y2offset < 0)
+                y2offset = 0;
+
+        /* Step 4 - rough offset setting */
+        /* Set rough offset register values */
+        dummyData[0] = YAS_REG_XOFFSETR | xoffset;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_Y1OFFSETR | y1offset;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_Y2OFFSETR | y2offset;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* CAL matrix read (first read is invalid) */
+        /* Config register - CAL register read - "110 01 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x08;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* CAL data read */
+        result =
+            yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+                                   dummyRegister, 9, calData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Config register - CAL register read - "110 01 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x08;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* CAL data read */
+        result =
+            yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+                                   dummyRegister, 9, calData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Calculate coefficients of the sensitivity correction matrix */
+        a1 = 100;
+        d2 = (calData[0] & 0xFC) >> 2;  /* [71..66] 6bit */
+        a2 = (short)(d2 - 32);
+        /* [65..62] 4bit */
+        d3 = ((calData[0] & 0x03) << 2) | ((calData[1] & 0xC0) >> 6);
+        a3 = (short)(d3 - 8);
+        d4 = (calData[1] & 0x3F);       /* [61..56] 6bit */
+        a4 = (short)(d4 - 32);
+        d5 = (calData[2] & 0xFC) >> 2;  /* [55..50] 6bit */
+        a5 = (short)(d5 - 32) + 70;
+        /* [49..44] 6bit */
+        d6 = ((calData[2] & 0x03) << 4) | ((calData[3] & 0xF0) >> 4);
+        a6 = (short)(d6 - 32);
+        /* [43..38] 6bit */
+        d7 = ((calData[3] & 0x0F) << 2) | ((calData[4] & 0xC0) >> 6);
+        a7 = (short)(d7 - 32);
+        d8 = (calData[4] & 0x3F);       /* [37..32] 6bit */
+        a8 = (short)(d8 - 32);
+        d9 = (calData[5] & 0xFE) >> 1;  /* [31..25] 7bit */
+        a9 = (short)(d9 - 64) + 130;
+
+        return result;
+}
+
+static int yas529_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        unsigned char stat;
+        unsigned char rawData[6];
+        unsigned char dummyData[1] = { 0 };
+        unsigned char dummyRegister = 0;
+        int result = INV_SUCCESS;
+        short SX, SY1, SY2, SY, SZ;
+        short row1fixed, row2fixed, row3fixed;
+
+        /* Config register - Measurements results - "110 00 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Measurements command register - Normal magnetic field measurement -
+           "000 00000" */
+        dummyData[0] = YAS_REG_CMDR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(10);
+        /* Measurement data read */
+        result =
+            yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+                                   dummyRegister, 6, (unsigned char *)&rawData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        stat = rawData[0] & 0x80;
+        if (stat == 0x00) {
+                /* Extract raw data */
+                SX = (short)((unsigned short)rawData[5] +
+                             ((unsigned short)rawData[4] & 0x7) * 256);
+                SY1 =
+                    (short)((unsigned short)rawData[3] +
+                            ((unsigned short)rawData[2] & 0x7) * 256);
+                SY2 =
+                    (short)((unsigned short)rawData[1] +
+                            ((unsigned short)rawData[0] & 0x7) * 256);
+                if ((SX <= 1) || (SY1 <= 1) || (SY2 <= 1))
+                        return INV_ERROR_COMPASS_DATA_UNDERFLOW;
+                if ((SX >= 1024) || (SY1 >= 1024) || (SY2 >= 1024))
+                        return INV_ERROR_COMPASS_DATA_OVERFLOW;
+                /* Convert to XYZ axis */
+                SX = -1 * SX;
+                SY = SY2 - SY1;
+                SZ = SY1 + SY2;
+
+                /* Apply sensitivity correction matrix */
+                row1fixed = (short)((a1 * SX + a2 * SY + a3 * SZ) >> 7) * 41;
+                row2fixed = (short)((a4 * SX + a5 * SY + a6 * SZ) >> 7) * 41;
+                row3fixed = (short)((a7 * SX + a8 * SY + a9 * SZ) >> 7) * 41;
+
+                data[0] = row1fixed >> 8;
+                data[1] = row1fixed & 0xFF;
+                data[2] = row2fixed >> 8;
+                data[3] = row2fixed & 0xFF;
+                data[4] = row3fixed >> 8;
+                data[5] = row3fixed & 0xFF;
+
+                return INV_SUCCESS;
+        } else {
+                return INV_ERROR_COMPASS_DATA_NOT_READY;
+        }
+}
+
+static struct ext_slave_descr yas529_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = yas529_suspend,
+        .resume           = yas529_resume,
+        .read             = yas529_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "yas529",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_YAS529,
+        .read_reg         = 0x06,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {19660, 8000},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *yas529_get_slave_descr(void)
+{
+        return &yas529_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct yas529_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int yas529_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct yas529_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        yas529_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int yas529_mod_remove(struct i2c_client *client)
+{
+        struct yas529_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                yas529_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id yas529_mod_id[] = {
+        { "yas529", COMPASS_ID_YAS529 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, yas529_mod_id);
+
+static struct i2c_driver yas529_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = yas529_mod_probe,
+        .remove = yas529_mod_remove,
+        .id_table = yas529_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "yas529_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init yas529_mod_init(void)
+{
+        int res = i2c_add_driver(&yas529_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "yas529_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit yas529_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&yas529_mod_driver);
+}
+
+module_init(yas529_mod_init);
+module_exit(yas529_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate YAS529 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("yas529_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/yas530.c b/drivers/misc/inv_mpu/compass/yas530.c
new file mode 100644
index 00000000000..fdca05ba8e5
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/yas530.c
@@ -0,0 +1,580 @@
+/*
+        $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 COMPASSDL
+  *
+  *  @{
+  *      @file   yas530.c
+  *      @brief  Magnetometer setup and handling methods for Yamaha YAS530
+  *              compass when used in a user-space solution (no kernel driver).
+  */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include "log.h"
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define YAS530_REGADDR_DEVICE_ID          (0x80)
+#define YAS530_REGADDR_ACTUATE_INIT_COIL  (0x81)
+#define YAS530_REGADDR_MEASURE_COMMAND    (0x82)
+#define YAS530_REGADDR_CONFIG             (0x83)
+#define YAS530_REGADDR_MEASURE_INTERVAL   (0x84)
+#define YAS530_REGADDR_OFFSET_X           (0x85)
+#define YAS530_REGADDR_OFFSET_Y1          (0x86)
+#define YAS530_REGADDR_OFFSET_Y2          (0x87)
+#define YAS530_REGADDR_TEST1              (0x88)
+#define YAS530_REGADDR_TEST2              (0x89)
+#define YAS530_REGADDR_CAL                (0x90)
+#define YAS530_REGADDR_MEASURE_DATA       (0xb0)
+
+/* -------------------------------------------------------------------------- */
+static int Cx, Cy1, Cy2;
+static int /*a1, */ a2, a3, a4, a5, a6, a7, a8, a9;
+static int k;
+
+static unsigned char dx, dy1, dy2;
+static unsigned char d2, d3, d4, d5, d6, d7, d8, d9, d0;
+static unsigned char dck;
+
+/* -------------------------------------------------------------------------- */
+
+static int set_hardware_offset(void *mlsl_handle,
+                               struct ext_slave_descr *slave,
+                               struct ext_slave_platform_data *pdata,
+                               char offset_x, char offset_y1, char offset_y2)
+{
+        char data;
+        int result = INV_SUCCESS;
+
+        data = offset_x & 0x3f;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         YAS530_REGADDR_OFFSET_X, data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        data = offset_y1 & 0x3f;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         YAS530_REGADDR_OFFSET_Y1, data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        data = offset_y2 & 0x3f;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         YAS530_REGADDR_OFFSET_Y2, data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+static int set_measure_command(void *mlsl_handle,
+                               struct ext_slave_descr *slave,
+                               struct ext_slave_platform_data *pdata,
+                               int ldtc, int fors, int dlymes)
+{
+        int result = INV_SUCCESS;
+
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         YAS530_REGADDR_MEASURE_COMMAND, 0x01);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+static int measure_normal(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata,
+                          int *busy, unsigned short *t,
+                          unsigned short *x, unsigned short *y1,
+                          unsigned short *y2)
+{
+        unsigned char data[8];
+        unsigned short b, to, xo, y1o, y2o;
+        int result;
+        ktime_t sleeptime;
+        result = set_measure_command(mlsl_handle, slave, pdata, 0, 0, 0);
+        sleeptime = ktime_set(0, 2 * NSEC_PER_MSEC);
+        set_current_state(TASK_UNINTERRUPTIBLE);
+        schedule_hrtimeout(&sleeptime, HRTIMER_MODE_REL);
+
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 YAS530_REGADDR_MEASURE_DATA, 8, data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        b = (data[0] >> 7) & 0x01;
+        to = ((data[0] << 2) & 0x1fc) | ((data[1] >> 6) & 0x03);
+        xo = ((data[2] << 5) & 0xfe0) | ((data[3] >> 3) & 0x1f);
+        y1o = ((data[4] << 5) & 0xfe0) | ((data[5] >> 3) & 0x1f);
+        y2o = ((data[6] << 5) & 0xfe0) | ((data[7] >> 3) & 0x1f);
+
+        *busy = b;
+        *t = to;
+        *x = xo;
+        *y1 = y1o;
+        *y2 = y2o;
+
+        return result;
+}
+
+static int check_offset(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        char offset_x, char offset_y1, char offset_y2,
+                        int *flag_x, int *flag_y1, int *flag_y2)
+{
+        int result;
+        int busy;
+        short t, x, y1, y2;
+
+        result = set_hardware_offset(mlsl_handle, slave, pdata,
+                                     offset_x, offset_y1, offset_y2);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = measure_normal(mlsl_handle, slave, pdata,
+                                &busy, &t, &x, &y1, &y2);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        *flag_x = 0;
+        *flag_y1 = 0;
+        *flag_y2 = 0;
+
+        if (x > 2048)
+                *flag_x = 1;
+        if (y1 > 2048)
+                *flag_y1 = 1;
+        if (y2 > 2048)
+                *flag_y2 = 1;
+        if (x < 2048)
+                *flag_x = -1;
+        if (y1 < 2048)
+                *flag_y1 = -1;
+        if (y2 < 2048)
+                *flag_y2 = -1;
+
+        return result;
+}
+
+static int measure_and_set_offset(void *mlsl_handle,
+                                  struct ext_slave_descr *slave,
+                                  struct ext_slave_platform_data *pdata,
+                                  char *offset)
+{
+        int i;
+        int result = INV_SUCCESS;
+        char offset_x = 0, offset_y1 = 0, offset_y2 = 0;
+        int flag_x = 0, flag_y1 = 0, flag_y2 = 0;
+        static const int correct[5] = { 16, 8, 4, 2, 1 };
+
+        for (i = 0; i < 5; i++) {
+                result = check_offset(mlsl_handle, slave, pdata,
+                                      offset_x, offset_y1, offset_y2,
+                                      &flag_x, &flag_y1, &flag_y2);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                if (flag_x)
+                        offset_x += flag_x * correct[i];
+                if (flag_y1)
+                        offset_y1 += flag_y1 * correct[i];
+                if (flag_y2)
+                        offset_y2 += flag_y2 * correct[i];
+        }
+
+        result = set_hardware_offset(mlsl_handle, slave, pdata,
+                                     offset_x, offset_y1, offset_y2);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        offset[0] = offset_x;
+        offset[1] = offset_y1;
+        offset[2] = offset_y2;
+
+        return result;
+}
+
+static void coordinate_conversion(short x, short y1, short y2, short t,
+                                  int32_t *xo, int32_t *yo, int32_t *zo)
+{
+        int32_t sx, sy1, sy2, sy, sz;
+        int32_t hx, hy, hz;
+
+        sx = x - (Cx * t) / 100;
+        sy1 = y1 - (Cy1 * t) / 100;
+        sy2 = y2 - (Cy2 * t) / 100;
+
+        sy = sy1 - sy2;
+        sz = -sy1 - sy2;
+
+        hx = k * ((100 * sx + a2 * sy + a3 * sz) / 10);
+        hy = k * ((a4 * sx + a5 * sy + a6 * sz) / 10);
+        hz = k * ((a7 * sx + a8 * sy + a9 * sz) / 10);
+
+        *xo = hx;
+        *yo = hy;
+        *zo = hz;
+}
+
+static int yas530_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                   struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        return result;
+}
+
+static int yas530_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        unsigned char dummyData = 0x00;
+        char offset[3] = { 0, 0, 0 };
+        unsigned char data[16];
+        unsigned char read_reg[1];
+
+        /* =============================================== */
+
+        /* Step 1 - Test register initialization */
+        dummyData = 0x00;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         YAS530_REGADDR_TEST1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result =
+            inv_serial_single_write(mlsl_handle, pdata->address,
+                                    YAS530_REGADDR_TEST2, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Device ID read  */
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 YAS530_REGADDR_DEVICE_ID, 1, read_reg);
+
+        /*Step 2 Read the CAL register */
+        /* CAL data read */
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 YAS530_REGADDR_CAL, 16, data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* CAL data Second Read */
+        result = inv_serial_read(mlsl_handle, pdata->address,
+                                 YAS530_REGADDR_CAL, 16, data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /*Cal data */
+        dx = data[0];
+        dy1 = data[1];
+        dy2 = data[2];
+        d2 = (data[3] >> 2) & 0x03f;
+        d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03);
+        d4 = data[4] & 0x3f;
+        d5 = (data[5] >> 2) & 0x3f;
+        d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f);
+        d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07);
+        d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01);
+        d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01);
+        d0 = (data[9] >> 2) & 0x1f;
+        dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01);
+
+        /*Correction Data */
+        Cx = (int)dx * 6 - 768;
+        Cy1 = (int)dy1 * 6 - 768;
+        Cy2 = (int)dy2 * 6 - 768;
+        a2 = (int)d2 - 32;
+        a3 = (int)d3 - 8;
+        a4 = (int)d4 - 32;
+        a5 = (int)d5 + 38;
+        a6 = (int)d6 - 32;
+        a7 = (int)d7 - 64;
+        a8 = (int)d8 - 32;
+        a9 = (int)d9;
+        k = (int)d0 + 10;
+
+        /*Obtain the [49:47] bits */
+        dck &= 0x07;
+
+        /*Step 3 : Storing the CONFIG with the CLK value */
+        dummyData = 0x00 | (dck << 2);
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         YAS530_REGADDR_CONFIG, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /*Step 4 : Set Acquisition Interval Register */
+        dummyData = 0x00;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         YAS530_REGADDR_MEASURE_INTERVAL,
+                                         dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /*Step 5 : Reset Coil */
+        dummyData = 0x00;
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                         YAS530_REGADDR_ACTUATE_INIT_COIL,
+                                         dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Offset Measurement and Set */
+        result = measure_and_set_offset(mlsl_handle, slave, pdata, offset);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        return result;
+}
+
+static int yas530_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        int result = INV_SUCCESS;
+
+        int busy;
+        short t, x, y1, y2;
+        int32_t xyz[3];
+        short rawfixed[3];
+
+        result = measure_normal(mlsl_handle, slave, pdata,
+                                &busy, &t, &x, &y1, &y2);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        coordinate_conversion(x, y1, y2, t, &xyz[0], &xyz[1], &xyz[2]);
+
+        rawfixed[0] = (short)(xyz[0] / 100);
+        rawfixed[1] = (short)(xyz[1] / 100);
+        rawfixed[2] = (short)(xyz[2] / 100);
+
+        data[0] = rawfixed[0] >> 8;
+        data[1] = rawfixed[0] & 0xFF;
+        data[2] = rawfixed[1] >> 8;
+        data[3] = rawfixed[1] & 0xFF;
+        data[4] = rawfixed[2] >> 8;
+        data[5] = rawfixed[2] & 0xFF;
+
+        if (busy)
+                return INV_ERROR_COMPASS_DATA_NOT_READY;
+        return result;
+}
+
+static struct ext_slave_descr yas530_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = yas530_suspend,
+        .resume           = yas530_resume,
+        .read             = yas530_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "yas530",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_YAS530,
+        .read_reg         = 0x06,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {3276, 8001},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *yas530_get_slave_descr(void)
+{
+        return &yas530_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct yas530_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int yas530_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct yas530_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        yas530_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int yas530_mod_remove(struct i2c_client *client)
+{
+        struct yas530_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                yas530_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id yas530_mod_id[] = {
+        { "yas530", COMPASS_ID_YAS530 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, yas530_mod_id);
+
+static struct i2c_driver yas530_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = yas530_mod_probe,
+        .remove = yas530_mod_remove,
+        .id_table = yas530_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "yas530_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init yas530_mod_init(void)
+{
+        int res = i2c_add_driver(&yas530_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "yas530_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit yas530_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&yas530_mod_driver);
+}
+
+module_init(yas530_mod_init);
+module_exit(yas530_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate YAS530 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("yas530_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/log.h b/drivers/misc/inv_mpu/log.h
new file mode 100644
index 00000000000..5630602e3ef
--- /dev/null
+++ b/drivers/misc/inv_mpu/log.h
@@ -0,0 +1,287 @@
+/*
+        $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/>.
+        $
+ */
+
+/*
+ * This file incorporates work covered by the following copyright and
+ * permission notice:
+ *
+ * Copyright (C) 2005 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * C/C++ logging functions.  See the logging documentation for API details.
+ *
+ * We'd like these to be available from C code (in case we import some from
+ * somewhere), so this has a C interface.
+ *
+ * The output will be correct when the log file is shared between multiple
+ * threads and/or multiple processes so long as the operating system
+ * supports O_APPEND.  These calls have mutex-protected data structures
+ * and so are NOT reentrant.  Do not use MPL_LOG in a signal handler.
+ */
+#ifndef _LIBS_CUTILS_MPL_LOG_H
+#define _LIBS_CUTILS_MPL_LOG_H
+
+#include "mltypes.h"
+#include <stdarg.h>
+
+
+#include <linux/kernel.h>
+
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Normally we strip MPL_LOGV (VERBOSE messages) from release builds.
+ * You can modify this (for example with "#define MPL_LOG_NDEBUG 0"
+ * at the top of your source file) to change that behavior.
+ */
+#ifndef MPL_LOG_NDEBUG
+#ifdef NDEBUG
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+#endif
+
+#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE
+#define MPL_LOG_DEFAULT KERN_DEFAULT
+#define MPL_LOG_VERBOSE KERN_CONT
+#define MPL_LOG_DEBUG   KERN_NOTICE
+#define MPL_LOG_INFO    KERN_INFO
+#define MPL_LOG_WARN    KERN_WARNING
+#define MPL_LOG_ERROR   KERN_ERR
+#define MPL_LOG_SILENT  MPL_LOG_VERBOSE
+
+
+
+/*
+ * This is the local tag used for the following simplified
+ * logging macros.  You can change this preprocessor definition
+ * before using the other macros to change the tag.
+ */
+#ifndef MPL_LOG_TAG
+#define MPL_LOG_TAG
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Simplified macro to send a verbose log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGV
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV(fmt, ...)                                              \
+        do {                                                            \
+                if (0)                                                  \
+                        MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+        } while (0)
+#else
+#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef CONDITION
+#define CONDITION(cond)     ((cond) != 0)
+#endif
+
+#ifndef MPL_LOGV_IF
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV_IF(cond, fmt, ...)  \
+        do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0)
+#else
+#define MPL_LOGV_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                              \
+                ? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
+                : (void)0)
+#endif
+#endif
+
+/*
+ * Simplified macro to send a debug log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGD
+#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGD_IF
+#define MPL_LOGD_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                             \
+                ? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+                : (void)0)
+#endif
+
+/*
+ * Simplified macro to send an info log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGI
+#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGI_IF
+#define MPL_LOGI_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                              \
+                ? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+                : (void)0)
+#endif
+
+/*
+ * Simplified macro to send a warning log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGW
+#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGW_IF
+#define MPL_LOGW_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                             \
+                ? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+                : (void)0)
+#endif
+
+/*
+ * Simplified macro to send an error log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGE
+#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGE_IF
+#define MPL_LOGE_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                             \
+                ? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+                : (void)0)
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Log a fatal error.  If the given condition fails, this stops program
+ * execution like a normal assertion, but also generating the given message.
+ * It is NOT stripped from release builds.  Note that the condition test
+ * is -inverted- from the normal assert() semantics.
+ */
+#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                         \
+                ? ((void)android_printAssert(#cond, MPL_LOG_TAG,   \
+                                                fmt, ##__VA_ARGS__))    \
+                : (void)0)
+
+#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \
+        (((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__)))
+
+/*
+ * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that
+ * are stripped out of release builds.
+ */
+#if MPL_LOG_NDEBUG
+#define MPL_LOG_FATAL_IF(cond, fmt, ...)                                \
+        do {                                                            \
+                if (0)                                                  \
+                        MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \
+        } while (0)
+#define MPL_LOG_FATAL(fmt, ...)                                         \
+        do {                                                            \
+                if (0)                                                  \
+                        MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)        \
+        } while (0)
+#else
+#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
+        MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__)
+#define MPL_LOG_FATAL(fmt, ...) \
+        MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Assertion that generates a log message when the assertion fails.
+ * Stripped out of release builds.  Uses the current MPL_LOG_TAG.
+ */
+#define MPL_LOG_ASSERT(cond, fmt, ...)                  \
+        MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__)
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Basic log message macro.
+ *
+ * Example:
+ *  MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno);
+ *
+ * The second argument may be NULL or "" to indicate the "global" tag.
+ */
+#ifndef MPL_LOG
+#define MPL_LOG(priority, tag, fmt, ...)                \
+        MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Log macro that allows you to specify a number for the priority.
+ */
+#ifndef MPL_LOG_PRI
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+        pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Log macro that allows you to pass in a varargs ("args" is a va_list).
+ */
+#ifndef MPL_LOG_PRI_VA
+/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * ===========================================================================
+ *
+ * The stuff in the rest of this file should not be used directly.
+ */
+
+int _MLPrintLog(int priority, const char *tag, const char *fmt, ...);
+int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args);
+/* Final implementation of actual writing to a character device */
+int _MLWriteLog(const char *buf, int buflen);
+
+static inline void __print_result_location(int result,
+                                           const char *file,
+                                           const char *func, int line)
+{
+        MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result);
+}
+
+#define LOG_RESULT_LOCATION(condition) \
+        do {                                                            \
+                __print_result_location((int)(condition), __FILE__,     \
+                                        __func__, __LINE__);            \
+        } while (0)
+
+
+#endif                          /* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/drivers/misc/inv_mpu/mldl_cfg.h b/drivers/misc/inv_mpu/mldl_cfg.h
new file mode 100644
index 00000000000..2b81b89179d
--- /dev/null
+++ b/drivers/misc/inv_mpu/mldl_cfg.h
@@ -0,0 +1,384 @@
+/*
+        $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.h
+ *      @brief  The Motion Library Driver Layer Configuration header file.
+ */
+
+#ifndef __MLDL_CFG_H__
+#define __MLDL_CFG_H__
+
+#include "mltypes.h"
+#include "mlsl.h"
+#include <linux/mpu.h>
+#ifdef MPU_CURRENT_BUILD_MPU6050B1
+#include "mpu6050b1.h"
+#elif defined(MPU_CURRENT_BUILD_MPU3050)
+#include "mpu3050.h"
+#endif
+
+#include "log.h"
+
+/*************************************************************************
+ *  Sensors Bit definitions
+ *************************************************************************/
+
+#define INV_X_GYRO                      (0x0001)
+#define INV_Y_GYRO                      (0x0002)
+#define INV_Z_GYRO                      (0x0004)
+#define INV_DMP_PROCESSOR               (0x0008)
+
+#define INV_X_ACCEL                     (0x0010)
+#define INV_Y_ACCEL                     (0x0020)
+#define INV_Z_ACCEL                     (0x0040)
+
+#define INV_X_COMPASS                   (0x0080)
+#define INV_Y_COMPASS                   (0x0100)
+#define INV_Z_COMPASS                   (0x0200)
+
+#define INV_X_PRESSURE                  (0x0300)
+#define INV_Y_PRESSURE                  (0x0800)
+#define INV_Z_PRESSURE                  (0x1000)
+
+#define INV_TEMPERATURE                 (0x2000)
+#define INV_TIME                        (0x4000)
+
+#define INV_THREE_AXIS_GYRO             (0x000F)
+#define INV_THREE_AXIS_ACCEL            (0x0070)
+#define INV_THREE_AXIS_COMPASS          (0x0380)
+#define INV_THREE_AXIS_PRESSURE         (0x1C00)
+
+#define INV_FIVE_AXIS                   (0x007B)
+#define INV_SIX_AXIS_GYRO_ACCEL         (0x007F)
+#define INV_SIX_AXIS_ACCEL_COMPASS      (0x03F0)
+#define INV_NINE_AXIS                   (0x03FF)
+#define INV_ALL_SENSORS                 (0x7FFF)
+
+#define MPL_PROD_KEY(ver, rev) (ver * 100 + rev)
+
+/* -------------------------------------------------------------------------- */
+struct mpu_ram {
+        __u16 length;
+        __u8 *ram;
+};
+
+struct mpu_gyro_cfg {
+        __u8 int_config;
+        __u8 ext_sync;
+        __u8 full_scale;
+        __u8 lpf;
+        __u8 clk_src;
+        __u8 divider;
+        __u8 dmp_enable;
+        __u8 fifo_enable;
+        __u8 dmp_cfg1;
+        __u8 dmp_cfg2;
+};
+
+/* Offset registers that can be calibrated */
+struct mpu_offsets {
+        __u8    tc[GYRO_NUM_AXES];
+        __u16   gyro[GYRO_NUM_AXES];
+};
+
+/* Chip related information that can be read and verified */
+struct mpu_chip_info {
+        __u8 addr;
+        __u8 product_revision;
+        __u8 silicon_revision;
+        __u8 product_id;
+        __u16 gyro_sens_trim;
+        /* Only used for MPU6050 */
+        __u16 accel_sens_trim;
+};
+
+
+struct inv_mpu_cfg {
+        __u32 requested_sensors;
+        __u8 ignore_system_suspend;
+};
+
+/* Driver related state information */
+struct inv_mpu_state {
+#define MPU_GYRO_IS_SUSPENDED           (0x01 << EXT_SLAVE_TYPE_GYROSCOPE)
+#define MPU_ACCEL_IS_SUSPENDED          (0x01 << EXT_SLAVE_TYPE_ACCEL)
+#define MPU_COMPASS_IS_SUSPENDED        (0x01 << EXT_SLAVE_TYPE_COMPASS)
+#define MPU_PRESSURE_IS_SUSPENDED       (0x01 << EXT_SLAVE_TYPE_PRESSURE)
+#define MPU_GYRO_IS_BYPASSED            (0x10)
+#define MPU_DMP_IS_SUSPENDED            (0x20)
+#define MPU_GYRO_NEEDS_CONFIG           (0x40)
+#define MPU_DEVICE_IS_SUSPENDED         (0x80)
+        __u8 status;
+        /* 0-1 for 3050, bitfield of BIT_SLVx_DLY_EN, x = [0..4] */
+        __u8 i2c_slaves_enabled;
+};
+
+/* Platform data for the MPU */
+struct mldl_cfg {
+        struct mpu_ram                  *mpu_ram;
+        struct mpu_gyro_cfg             *mpu_gyro_cfg;
+        struct mpu_offsets              *mpu_offsets;
+        struct mpu_chip_info            *mpu_chip_info;
+
+        /* MPU Related stored status and info */
+        struct inv_mpu_cfg              *inv_mpu_cfg;
+        struct inv_mpu_state            *inv_mpu_state;
+
+        /* Slave related information */
+        struct ext_slave_descr          *slave[EXT_SLAVE_NUM_TYPES];
+        /* Platform Data */
+        struct mpu_platform_data        *pdata;
+        struct ext_slave_platform_data  *pdata_slave[EXT_SLAVE_NUM_TYPES];
+};
+
+/* -------------------------------------------------------------------------- */
+
+int inv_mpu_open(struct mldl_cfg *mldl_cfg,
+                 void *mlsl_handle,
+                 void *accel_handle,
+                 void *compass_handle,
+                 void *pressure_handle);
+int inv_mpu_close(struct mldl_cfg *mldl_cfg,
+                  void *mlsl_handle,
+                  void *accel_handle,
+                  void *compass_handle,
+                  void *pressure_handle);
+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 inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
+                    void *gyro_handle,
+                    void *accel_handle,
+                    void *compass_handle,
+                    void *pressure_handle,
+                    unsigned long sensors);
+int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg,
+                         void *mlsl_handle,
+                         const unsigned char *data,
+                         int size);
+
+/* -------------------------------------------------------------------------- */
+/* Slave Read functions */
+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);
+static inline int inv_mpu_read_accel(struct mldl_cfg *mldl_cfg,
+                                     void *gyro_handle,
+                                     void *accel_handle, unsigned char *data)
+{
+        if (!mldl_cfg) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        return inv_mpu_slave_read(
+                mldl_cfg, gyro_handle, accel_handle,
+                mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL],
+                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+                data);
+}
+
+static inline int inv_mpu_read_compass(struct mldl_cfg *mldl_cfg,
+                                       void *gyro_handle,
+                                       void *compass_handle,
+                                       unsigned char *data)
+{
+        if (!mldl_cfg) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        return inv_mpu_slave_read(
+                mldl_cfg, gyro_handle, compass_handle,
+                mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS],
+                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+                data);
+}
+
+static inline int inv_mpu_read_pressure(struct mldl_cfg *mldl_cfg,
+                                        void *gyro_handle,
+                                        void *pressure_handle,
+                                        unsigned char *data)
+{
+        if (!mldl_cfg) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        return inv_mpu_slave_read(
+                mldl_cfg, gyro_handle, pressure_handle,
+                mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE],
+                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+                data);
+}
+
+int gyro_config(void *mlsl_handle,
+                struct mldl_cfg *mldl_cfg,
+                struct ext_slave_config *data);
+
+/* Slave Config functions */
+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);
+static inline int inv_mpu_config_accel(struct mldl_cfg *mldl_cfg,
+                                       void *gyro_handle,
+                                       void *accel_handle,
+                                       struct ext_slave_config *data)
+{
+        if (!mldl_cfg) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        return inv_mpu_slave_config(
+                mldl_cfg, gyro_handle, accel_handle, data,
+                mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL],
+                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]);
+}
+
+static inline int inv_mpu_config_compass(struct mldl_cfg *mldl_cfg,
+                                         void *gyro_handle,
+                                         void *compass_handle,
+                                         struct ext_slave_config *data)
+{
+        if (!mldl_cfg) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        return inv_mpu_slave_config(
+                mldl_cfg, gyro_handle, compass_handle, data,
+                mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS],
+                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]);
+}
+
+static inline int inv_mpu_config_pressure(struct mldl_cfg *mldl_cfg,
+                                          void *gyro_handle,
+                                          void *pressure_handle,
+                                          struct ext_slave_config *data)
+{
+        if (!mldl_cfg) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        return inv_mpu_slave_config(
+                mldl_cfg, gyro_handle, pressure_handle, data,
+                mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE],
+                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_PRESSURE]);
+}
+
+int gyro_get_config(void *mlsl_handle,
+                struct mldl_cfg *mldl_cfg,
+                struct ext_slave_config *data);
+
+/* Slave get config functions */
+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);
+
+static inline int inv_mpu_get_accel_config(struct mldl_cfg *mldl_cfg,
+                                           void *gyro_handle,
+                                           void *accel_handle,
+                                           struct ext_slave_config *data)
+{
+        if (!mldl_cfg) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        return inv_mpu_get_slave_config(
+                mldl_cfg, gyro_handle, accel_handle, data,
+                mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL],
+                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]);
+}
+
+static inline int inv_mpu_get_compass_config(struct mldl_cfg *mldl_cfg,
+                                             void *gyro_handle,
+                                             void *compass_handle,
+                                             struct ext_slave_config *data)
+{
+        if (!mldl_cfg || !(mldl_cfg->pdata)) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        return inv_mpu_get_slave_config(
+                mldl_cfg, gyro_handle, compass_handle, data,
+                mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS],
+                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]);
+}
+
+static inline int inv_mpu_get_pressure_config(struct mldl_cfg *mldl_cfg,
+                                              void *gyro_handle,
+                                              void *pressure_handle,
+                                              struct ext_slave_config *data)
+{
+        if (!mldl_cfg || !(mldl_cfg->pdata)) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        return inv_mpu_get_slave_config(
+                mldl_cfg, gyro_handle, pressure_handle, data,
+                mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE],
+                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_PRESSURE]);
+}
+
+/* -------------------------------------------------------------------------- */
+
+static inline
+long inv_mpu_get_sampling_rate_hz(struct mpu_gyro_cfg *gyro_cfg)
+{
+        if (((gyro_cfg->lpf) == 0) || ((gyro_cfg->lpf) == 7))
+                return 8000L / (gyro_cfg->divider + 1);
+        else
+                return 1000L / (gyro_cfg->divider + 1);
+}
+
+static inline
+long inv_mpu_get_sampling_period_us(struct mpu_gyro_cfg *gyro_cfg)
+{
+        if (((gyro_cfg->lpf) == 0) || ((gyro_cfg->lpf) == 7))
+                return (long) (1000000L * (gyro_cfg->divider + 1)) / 8000L;
+        else
+                return (long) (1000000L * (gyro_cfg->divider + 1)) / 1000L;
+}
+
+#endif                          /* __MLDL_CFG_H__ */
+
+/**
+ * @}
+ */
diff --git a/drivers/misc/inv_mpu/mldl_print_cfg.h b/drivers/misc/inv_mpu/mldl_print_cfg.h
new file mode 100644
index 00000000000..2e1911440cc
--- /dev/null
+++ b/drivers/misc/inv_mpu/mldl_print_cfg.h
@@ -0,0 +1,38 @@
+/*
+        $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/>.
+        $
+ */
+
+/**
+ * @defgroup
+ * @brief
+ *
+ * @{
+ * @file     mldl_print_cfg.h
+ * @brief
+ *
+ *
+ */
+#ifndef __MLDL_PRINT_CFG__
+#define __MLDL_PRINT_CFG__
+
+#include "mldl_cfg.h"
+
+
+void mldl_print_cfg(struct mldl_cfg *mldl_cfg);
+
+#endif /* __MLDL_PRINT_CFG__ */
diff --git a/drivers/misc/inv_mpu/mlsl.h b/drivers/misc/inv_mpu/mlsl.h
new file mode 100644
index 00000000000..204baedc1e2
--- /dev/null
+++ b/drivers/misc/inv_mpu/mlsl.h
@@ -0,0 +1,186 @@
+/*
+        $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/>.
+        $
+ */
+
+#ifndef __MLSL_H__
+#define __MLSL_H__
+
+/**
+ *  @defgroup   MLSL
+ *  @brief      Motion Library - Serial Layer.
+ *              The Motion Library System Layer provides the Motion Library
+ *              with the communication interface to the hardware.
+ *
+ *              The communication interface is assumed to support serial
+ *              transfers in burst of variable length up to
+ *              SERIAL_MAX_TRANSFER_SIZE.
+ *              The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes.
+ *              Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will
+ *              be subdivided in smaller transfers of length <=
+ *              SERIAL_MAX_TRANSFER_SIZE.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be modified to
+ *              overcome any host processor transfer size limitation down to
+ *              1 B, the minimum.
+ *              An higher value for SERIAL_MAX_TRANSFER_SIZE will favor
+ *              performance and efficiency while requiring higher resource usage
+ *              (mostly buffering). A smaller value will increase overhead and
+ *              decrease efficiency but allows to operate with more resource
+ *              constrained processor and master serial controllers.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
+ *              mlsl.h header file and master serial controllers.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
+ *              mlsl.h header file.
+ *
+ *  @{
+ *      @file   mlsl.h
+ *      @brief  The Motion Library System Layer.
+ *
+ */
+
+#include "mltypes.h"
+#include <linux/mpu.h>
+
+
+/*
+ * NOTE : to properly support Yamaha compass reads,
+ *        the max transfer size should be at least 9 B.
+ *        Length in bytes, typically a power of 2 >= 2
+ */
+#define SERIAL_MAX_TRANSFER_SIZE 128
+
+
+/**
+ *  inv_serial_single_write() - used to write a single byte of data.
+ *  @sl_handle          pointer to the serial device used for the communication.
+ *  @slave_addr         I2C slave address of device.
+ *  @register_addr      Register address to write.
+ *  @data               Single byte of data to write.
+ *
+ *      It is called by the MPL to write a single byte of data to the MPU.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_single_write(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned char register_addr,
+        unsigned char data);
+
+/**
+ *  inv_serial_write() - used to write multiple bytes of data to registers.
+ *  @sl_handle  a file handle to the serial device used for the communication.
+ *  @slave_addr I2C slave address of device.
+ *  @register_addr      Register address to write.
+ *  @length     Length of burst of data.
+ *  @data       Pointer to block of data.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_write(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short length,
+        unsigned char const *data);
+
+/**
+ *  inv_serial_read() - used to read multiple bytes of data from registers.
+ *  @sl_handle  a file handle to the serial device used for the communication.
+ *  @slave_addr I2C slave address of device.
+ *  @register_addr      Register address to read.
+ *  @length     Length of burst of data.
+ *  @data       Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_read(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned char register_addr,
+        unsigned short length,
+        unsigned char *data);
+
+/**
+ *  inv_serial_read_mem() - used to read multiple bytes of data from the memory.
+ *          This should be sent by I2C or SPI.
+ *
+ *  @sl_handle  a file handle to the serial device used for the communication.
+ *  @slave_addr I2C slave address of device.
+ *  @mem_addr   The location in the memory to read from.
+ *  @length     Length of burst data.
+ *  @data       Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_read_mem(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short mem_addr,
+        unsigned short length,
+        unsigned char *data);
+
+/**
+ *  inv_serial_write_mem() - used to write multiple bytes of data to the memory.
+ *  @sl_handle  a file handle to the serial device used for the communication.
+ *  @slave_addr I2C slave address of device.
+ *  @mem_addr   The location in the memory to write to.
+ *  @length     Length of burst data.
+ *  @data       Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_write_mem(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short mem_addr,
+        unsigned short length,
+        unsigned char const *data);
+
+/**
+ *  inv_serial_read_fifo() - used to read multiple bytes of data from the fifo.
+ *  @sl_handle  a file handle to the serial device used for the communication.
+ *  @slave_addr I2C slave address of device.
+ *  @length     Length of burst of data.
+ *  @data       Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_read_fifo(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short length,
+        unsigned char *data);
+
+/**
+ *  inv_serial_write_fifo() - used to write multiple bytes of data to the fifo.
+ *  @sl_handle  a file handle to the serial device used for the communication.
+ *  @slave_addr I2C slave address of device.
+ *  @length     Length of burst of data.
+ *  @data       Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_write_fifo(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short length,
+        unsigned char const *data);
+
+/**
+ * @}
+ */
+#endif                          /* __MLSL_H__ */
diff --git a/drivers/misc/inv_mpu/mltypes.h b/drivers/misc/inv_mpu/mltypes.h
new file mode 100644
index 00000000000..a249f93be3e
--- /dev/null
+++ b/drivers/misc/inv_mpu/mltypes.h
@@ -0,0 +1,234 @@
+/*
+        $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/>.
+        $
+ */
+
+/**
+ *  @defgroup MLERROR
+ *  @brief  Definition of the error codes used within the MPL and
+ *          returned to the user.
+ *          Every function tries to return a meaningful error code basing
+ *          on the occuring error condition. The error code is numeric.
+ *
+ *          The available error codes and their associated values are:
+ *          - (0)               INV_SUCCESS
+ *          - (32)              INV_ERROR
+ *          - (22 / EINVAL)     INV_ERROR_INVALID_PARAMETER
+ *          - (1  / EPERM)      INV_ERROR_FEATURE_NOT_ENABLED
+ *          - (36)              INV_ERROR_FEATURE_NOT_IMPLEMENTED
+ *          - (38)              INV_ERROR_DMP_NOT_STARTED
+ *          - (39)              INV_ERROR_DMP_STARTED
+ *          - (40)              INV_ERROR_NOT_OPENED
+ *          - (41)              INV_ERROR_OPENED
+ *          - (19 / ENODEV)     INV_ERROR_INVALID_MODULE
+ *          - (12 / ENOMEM)     INV_ERROR_MEMORY_EXAUSTED
+ *          - (44)              INV_ERROR_DIVIDE_BY_ZERO
+ *          - (45)              INV_ERROR_ASSERTION_FAILURE
+ *          - (46)              INV_ERROR_FILE_OPEN
+ *          - (47)              INV_ERROR_FILE_READ
+ *          - (48)              INV_ERROR_FILE_WRITE
+ *          - (49)              INV_ERROR_INVALID_CONFIGURATION
+ *          - (52)              INV_ERROR_SERIAL_CLOSED
+ *          - (53)              INV_ERROR_SERIAL_OPEN_ERROR
+ *          - (54)              INV_ERROR_SERIAL_READ
+ *          - (55)              INV_ERROR_SERIAL_WRITE
+ *          - (56)              INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
+ *          - (57)              INV_ERROR_SM_TRANSITION
+ *          - (58)              INV_ERROR_SM_IMPROPER_STATE
+ *          - (62)              INV_ERROR_FIFO_OVERFLOW
+ *          - (63)              INV_ERROR_FIFO_FOOTER
+ *          - (64)              INV_ERROR_FIFO_READ_COUNT
+ *          - (65)              INV_ERROR_FIFO_READ_DATA
+ *          - (72)              INV_ERROR_MEMORY_SET
+ *          - (82)              INV_ERROR_LOG_MEMORY_ERROR
+ *          - (83)              INV_ERROR_LOG_OUTPUT_ERROR
+ *          - (92)              INV_ERROR_OS_BAD_PTR
+ *          - (93)              INV_ERROR_OS_BAD_HANDLE
+ *          - (94)              INV_ERROR_OS_CREATE_FAILED
+ *          - (95)              INV_ERROR_OS_LOCK_FAILED
+ *          - (102)             INV_ERROR_COMPASS_DATA_OVERFLOW
+ *          - (103)             INV_ERROR_COMPASS_DATA_UNDERFLOW
+ *          - (104)             INV_ERROR_COMPASS_DATA_NOT_READY
+ *          - (105)             INV_ERROR_COMPASS_DATA_ERROR
+ *          - (107)             INV_ERROR_CALIBRATION_LOAD
+ *          - (108)             INV_ERROR_CALIBRATION_STORE
+ *          - (109)             INV_ERROR_CALIBRATION_LEN
+ *          - (110)             INV_ERROR_CALIBRATION_CHECKSUM
+ *          - (111)             INV_ERROR_ACCEL_DATA_OVERFLOW
+ *          - (112)             INV_ERROR_ACCEL_DATA_UNDERFLOW
+ *          - (113)             INV_ERROR_ACCEL_DATA_NOT_READY
+ *          - (114)             INV_ERROR_ACCEL_DATA_ERROR
+ *
+ *          The available warning codes and their associated values are:
+ *          - (115)             INV_WARNING_MOTION_RACE
+ *          - (116)             INV_WARNING_QUAT_TRASHED
+ *
+ *  @{
+ *      @file mltypes.h
+ *  @}
+ */
+
+#ifndef MLTYPES_H
+#define MLTYPES_H
+
+#include <linux/types.h>
+#include <asm-generic/errno-base.h>
+
+
+
+
+/*---------------------------
+ *    ML Defines
+ *--------------------------*/
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+/* - ML Errors. - */
+#define ERROR_NAME(x)   (#x)
+#define ERROR_CHECK_FIRST(first, x) \
+        { if (INV_SUCCESS == first) first = x; }
+
+#define INV_SUCCESS                       (0)
+/* Generic Error code.  Proprietary Error Codes only */
+#define INV_ERROR_BASE                    (0x20)
+#define INV_ERROR                         (INV_ERROR_BASE)
+
+/* Compatibility and other generic error codes */
+#define INV_ERROR_INVALID_PARAMETER             (EINVAL)
+#define INV_ERROR_FEATURE_NOT_ENABLED           (EPERM)
+#define INV_ERROR_FEATURE_NOT_IMPLEMENTED       (INV_ERROR_BASE + 4)
+#define INV_ERROR_DMP_NOT_STARTED               (INV_ERROR_BASE + 6)
+#define INV_ERROR_DMP_STARTED                   (INV_ERROR_BASE + 7)
+#define INV_ERROR_NOT_OPENED                    (INV_ERROR_BASE + 8)
+#define INV_ERROR_OPENED                        (INV_ERROR_BASE + 9)
+#define INV_ERROR_INVALID_MODULE                (ENODEV)
+#define INV_ERROR_MEMORY_EXAUSTED               (ENOMEM)
+#define INV_ERROR_DIVIDE_BY_ZERO                (INV_ERROR_BASE + 12)
+#define INV_ERROR_ASSERTION_FAILURE             (INV_ERROR_BASE + 13)
+#define INV_ERROR_FILE_OPEN                     (INV_ERROR_BASE + 14)
+#define INV_ERROR_FILE_READ                     (INV_ERROR_BASE + 15)
+#define INV_ERROR_FILE_WRITE                    (INV_ERROR_BASE + 16)
+#define INV_ERROR_INVALID_CONFIGURATION         (INV_ERROR_BASE + 17)
+
+/* Serial Communication */
+#define INV_ERROR_SERIAL_CLOSED                 (INV_ERROR_BASE + 20)
+#define INV_ERROR_SERIAL_OPEN_ERROR             (INV_ERROR_BASE + 21)
+#define INV_ERROR_SERIAL_READ                   (INV_ERROR_BASE + 22)
+#define INV_ERROR_SERIAL_WRITE                  (INV_ERROR_BASE + 23)
+#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  (INV_ERROR_BASE + 24)
+
+/* SM = State Machine */
+#define INV_ERROR_SM_TRANSITION                 (INV_ERROR_BASE + 25)
+#define INV_ERROR_SM_IMPROPER_STATE             (INV_ERROR_BASE + 26)
+
+/* Fifo */
+#define INV_ERROR_FIFO_OVERFLOW                 (INV_ERROR_BASE + 30)
+#define INV_ERROR_FIFO_FOOTER                   (INV_ERROR_BASE + 31)
+#define INV_ERROR_FIFO_READ_COUNT               (INV_ERROR_BASE + 32)
+#define INV_ERROR_FIFO_READ_DATA                (INV_ERROR_BASE + 33)
+
+/* Memory & Registers, Set & Get */
+#define INV_ERROR_MEMORY_SET                    (INV_ERROR_BASE + 40)
+
+#define INV_ERROR_LOG_MEMORY_ERROR              (INV_ERROR_BASE + 50)
+#define INV_ERROR_LOG_OUTPUT_ERROR              (INV_ERROR_BASE + 51)
+
+/* OS interface errors */
+#define INV_ERROR_OS_BAD_PTR                    (INV_ERROR_BASE + 60)
+#define INV_ERROR_OS_BAD_HANDLE                 (INV_ERROR_BASE + 61)
+#define INV_ERROR_OS_CREATE_FAILED              (INV_ERROR_BASE + 62)
+#define INV_ERROR_OS_LOCK_FAILED                (INV_ERROR_BASE + 63)
+
+/* Compass errors */
+#define INV_ERROR_COMPASS_DATA_OVERFLOW         (INV_ERROR_BASE + 70)
+#define INV_ERROR_COMPASS_DATA_UNDERFLOW        (INV_ERROR_BASE + 71)
+#define INV_ERROR_COMPASS_DATA_NOT_READY        (INV_ERROR_BASE + 72)
+#define INV_ERROR_COMPASS_DATA_ERROR            (INV_ERROR_BASE + 73)
+
+/* Load/Store calibration */
+#define INV_ERROR_CALIBRATION_LOAD              (INV_ERROR_BASE + 75)
+#define INV_ERROR_CALIBRATION_STORE             (INV_ERROR_BASE + 76)
+#define INV_ERROR_CALIBRATION_LEN               (INV_ERROR_BASE + 77)
+#define INV_ERROR_CALIBRATION_CHECKSUM          (INV_ERROR_BASE + 78)
+
+/* Accel errors */
+#define INV_ERROR_ACCEL_DATA_OVERFLOW           (INV_ERROR_BASE + 79)
+#define INV_ERROR_ACCEL_DATA_UNDERFLOW          (INV_ERROR_BASE + 80)
+#define INV_ERROR_ACCEL_DATA_NOT_READY          (INV_ERROR_BASE + 81)
+#define INV_ERROR_ACCEL_DATA_ERROR              (INV_ERROR_BASE + 82)
+
+/* No Motion Warning States */
+#define INV_WARNING_MOTION_RACE                 (INV_ERROR_BASE + 83)
+#define INV_WARNING_QUAT_TRASHED                (INV_ERROR_BASE + 84)
+#define INV_WARNING_GYRO_MAG                    (INV_ERROR_BASE + 85)
+
+#ifdef INV_USE_LEGACY_NAMES
+#define ML_SUCCESS                        INV_SUCCESS
+#define ML_ERROR                          INV_ERROR
+#define ML_ERROR_INVALID_PARAMETER        INV_ERROR_INVALID_PARAMETER
+#define ML_ERROR_FEATURE_NOT_ENABLED      INV_ERROR_FEATURE_NOT_ENABLED
+#define ML_ERROR_FEATURE_NOT_IMPLEMENTED  INV_ERROR_FEATURE_NOT_IMPLEMENTED
+#define ML_ERROR_DMP_NOT_STARTED          INV_ERROR_DMP_NOT_STARTED
+#define ML_ERROR_DMP_STARTED              INV_ERROR_DMP_STARTED
+#define ML_ERROR_NOT_OPENED               INV_ERROR_NOT_OPENED
+#define ML_ERROR_OPENED                   INV_ERROR_OPENED
+#define ML_ERROR_INVALID_MODULE           INV_ERROR_INVALID_MODULE
+#define ML_ERROR_MEMORY_EXAUSTED          INV_ERROR_MEMORY_EXAUSTED
+#define ML_ERROR_DIVIDE_BY_ZERO           INV_ERROR_DIVIDE_BY_ZERO
+#define ML_ERROR_ASSERTION_FAILURE        INV_ERROR_ASSERTION_FAILURE
+#define ML_ERROR_FILE_OPEN                INV_ERROR_FILE_OPEN
+#define ML_ERROR_FILE_READ                INV_ERROR_FILE_READ
+#define ML_ERROR_FILE_WRITE               INV_ERROR_FILE_WRITE
+#define ML_ERROR_INVALID_CONFIGURATION    INV_ERROR_INVALID_CONFIGURATION
+#define ML_ERROR_SERIAL_CLOSED            INV_ERROR_SERIAL_CLOSED
+#define ML_ERROR_SERIAL_OPEN_ERROR        INV_ERROR_SERIAL_OPEN_ERROR
+#define ML_ERROR_SERIAL_READ              INV_ERROR_SERIAL_READ
+#define ML_ERROR_SERIAL_WRITE             INV_ERROR_SERIAL_WRITE
+#define ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  \
+        INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
+#define ML_ERROR_SM_TRANSITION            INV_ERROR_SM_TRANSITION
+#define ML_ERROR_SM_IMPROPER_STATE        INV_ERROR_SM_IMPROPER_STATE
+#define ML_ERROR_FIFO_OVERFLOW            INV_ERROR_FIFO_OVERFLOW
+#define ML_ERROR_FIFO_FOOTER              INV_ERROR_FIFO_FOOTER
+#define ML_ERROR_FIFO_READ_COUNT          INV_ERROR_FIFO_READ_COUNT
+#define ML_ERROR_FIFO_READ_DATA           INV_ERROR_FIFO_READ_DATA
+#define ML_ERROR_MEMORY_SET               INV_ERROR_MEMORY_SET
+#define ML_ERROR_LOG_MEMORY_ERROR         INV_ERROR_LOG_MEMORY_ERROR
+#define ML_ERROR_LOG_OUTPUT_ERROR         INV_ERROR_LOG_OUTPUT_ERROR
+#define ML_ERROR_OS_BAD_PTR               INV_ERROR_OS_BAD_PTR
+#define ML_ERROR_OS_BAD_HANDLE            INV_ERROR_OS_BAD_HANDLE
+#define ML_ERROR_OS_CREATE_FAILED         INV_ERROR_OS_CREATE_FAILED
+#define ML_ERROR_OS_LOCK_FAILED           INV_ERROR_OS_LOCK_FAILED
+#define ML_ERROR_COMPASS_DATA_OVERFLOW    INV_ERROR_COMPASS_DATA_OVERFLOW
+#define ML_ERROR_COMPASS_DATA_UNDERFLOW   INV_ERROR_COMPASS_DATA_UNDERFLOW
+#define ML_ERROR_COMPASS_DATA_NOT_READY   INV_ERROR_COMPASS_DATA_NOT_READY
+#define ML_ERROR_COMPASS_DATA_ERROR       INV_ERROR_COMPASS_DATA_ERROR
+#define ML_ERROR_CALIBRATION_LOAD         INV_ERROR_CALIBRATION_LOAD
+#define ML_ERROR_CALIBRATION_STORE        INV_ERROR_CALIBRATION_STORE
+#define ML_ERROR_CALIBRATION_LEN          INV_ERROR_CALIBRATION_LEN
+#define ML_ERROR_CALIBRATION_CHECKSUM     INV_ERROR_CALIBRATION_CHECKSUM
+#define ML_ERROR_ACCEL_DATA_OVERFLOW      INV_ERROR_ACCEL_DATA_OVERFLOW
+#define ML_ERROR_ACCEL_DATA_UNDERFLOW     INV_ERROR_ACCEL_DATA_UNDERFLOW
+#define ML_ERROR_ACCEL_DATA_NOT_READY     INV_ERROR_ACCEL_DATA_NOT_READY
+#define ML_ERROR_ACCEL_DATA_ERROR         INV_ERROR_ACCEL_DATA_ERROR
+#endif
+
+/* For Linux coding compliance */
+
+#endif                          /* MLTYPES_H */
diff --git a/drivers/misc/inv_mpu/mpu-dev.h b/drivers/misc/inv_mpu/mpu-dev.h
new file mode 100644
index 00000000000..b6a4fcfac58
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu-dev.h
@@ -0,0 +1,36 @@
+/*
+        $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/>.
+        $
+ */
+
+
+#ifndef __MPU_DEV_H__
+#define __MPU_DEV_H__
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/mpu.h>
+
+int inv_mpu_register_slave(struct module *slave_module,
+                        struct i2c_client *client,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_descr *(*slave_descr)(void));
+
+void inv_mpu_unregister_slave(struct i2c_client *client,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_descr *(*slave_descr)(void));
+#endif
diff --git a/drivers/misc/inv_mpu/mpu3050.h b/drivers/misc/inv_mpu/mpu3050.h
new file mode 100644
index 00000000000..02af16ed121
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050.h
@@ -0,0 +1,251 @@
+/*
+        $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/>.
+        $
+ */
+
+#ifndef __MPU_H_
+#error Do not include this file directly.  Include mpu.h instead.
+#endif
+
+#ifndef __MPU3050_H_
+#define __MPU3050_H_
+
+#include <linux/types.h>
+
+
+#define MPU_NAME "mpu3050"
+#define DEFAULT_MPU_SLAVEADDR       0x68
+
+/*==== MPU REGISTER SET ====*/
+enum mpu_register {
+        MPUREG_WHO_AM_I = 0,    /* 00 0x00 */
+        MPUREG_PRODUCT_ID,      /* 01 0x01 */
+        MPUREG_02_RSVD,         /* 02 0x02 */
+        MPUREG_03_RSVD,         /* 03 0x03 */
+        MPUREG_04_RSVD,         /* 04 0x04 */
+        MPUREG_XG_OFFS_TC,      /* 05 0x05 */
+        MPUREG_06_RSVD,         /* 06 0x06 */
+        MPUREG_07_RSVD,         /* 07 0x07 */
+        MPUREG_YG_OFFS_TC,      /* 08 0x08 */
+        MPUREG_09_RSVD,         /* 09 0x09 */
+        MPUREG_0A_RSVD,         /* 10 0x0a */
+        MPUREG_ZG_OFFS_TC,      /* 11 0x0b */
+        MPUREG_X_OFFS_USRH,     /* 12 0x0c */
+        MPUREG_X_OFFS_USRL,     /* 13 0x0d */
+        MPUREG_Y_OFFS_USRH,     /* 14 0x0e */
+        MPUREG_Y_OFFS_USRL,     /* 15 0x0f */
+        MPUREG_Z_OFFS_USRH,     /* 16 0x10 */
+        MPUREG_Z_OFFS_USRL,     /* 17 0x11 */
+        MPUREG_FIFO_EN1,        /* 18 0x12 */
+        MPUREG_FIFO_EN2,        /* 19 0x13 */
+        MPUREG_AUX_SLV_ADDR,    /* 20 0x14 */
+        MPUREG_SMPLRT_DIV,      /* 21 0x15 */
+        MPUREG_DLPF_FS_SYNC,    /* 22 0x16 */
+        MPUREG_INT_CFG,         /* 23 0x17 */
+        MPUREG_ACCEL_BURST_ADDR,/* 24 0x18 */
+        MPUREG_19_RSVD,         /* 25 0x19 */
+        MPUREG_INT_STATUS,      /* 26 0x1a */
+        MPUREG_TEMP_OUT_H,      /* 27 0x1b */
+        MPUREG_TEMP_OUT_L,      /* 28 0x1c */
+        MPUREG_GYRO_XOUT_H,     /* 29 0x1d */
+        MPUREG_GYRO_XOUT_L,     /* 30 0x1e */
+        MPUREG_GYRO_YOUT_H,     /* 31 0x1f */
+        MPUREG_GYRO_YOUT_L,     /* 32 0x20 */
+        MPUREG_GYRO_ZOUT_H,     /* 33 0x21 */
+        MPUREG_GYRO_ZOUT_L,     /* 34 0x22 */
+        MPUREG_23_RSVD,         /* 35 0x23 */
+        MPUREG_24_RSVD,         /* 36 0x24 */
+        MPUREG_25_RSVD,         /* 37 0x25 */
+        MPUREG_26_RSVD,         /* 38 0x26 */
+        MPUREG_27_RSVD,         /* 39 0x27 */
+        MPUREG_28_RSVD,         /* 40 0x28 */
+        MPUREG_29_RSVD,         /* 41 0x29 */
+        MPUREG_2A_RSVD,         /* 42 0x2a */
+        MPUREG_2B_RSVD,         /* 43 0x2b */
+        MPUREG_2C_RSVD,         /* 44 0x2c */
+        MPUREG_2D_RSVD,         /* 45 0x2d */
+        MPUREG_2E_RSVD,         /* 46 0x2e */
+        MPUREG_2F_RSVD,         /* 47 0x2f */
+        MPUREG_30_RSVD,         /* 48 0x30 */
+        MPUREG_31_RSVD,         /* 49 0x31 */
+        MPUREG_32_RSVD,         /* 50 0x32 */
+        MPUREG_33_RSVD,         /* 51 0x33 */
+        MPUREG_34_RSVD,         /* 52 0x34 */
+        MPUREG_DMP_CFG_1,       /* 53 0x35 */
+        MPUREG_DMP_CFG_2,       /* 54 0x36 */
+        MPUREG_BANK_SEL,        /* 55 0x37 */
+        MPUREG_MEM_START_ADDR,  /* 56 0x38 */
+        MPUREG_MEM_R_W,         /* 57 0x39 */
+        MPUREG_FIFO_COUNTH,     /* 58 0x3a */
+        MPUREG_FIFO_COUNTL,     /* 59 0x3b */
+        MPUREG_FIFO_R_W,        /* 60 0x3c */
+        MPUREG_USER_CTRL,       /* 61 0x3d */
+        MPUREG_PWR_MGM,         /* 62 0x3e */
+        MPUREG_3F_RSVD,         /* 63 0x3f */
+        NUM_OF_MPU_REGISTERS    /* 64 0x40 */
+};
+
+/*==== BITS FOR MPU ====*/
+
+/*---- MPU 'FIFO_EN1' register (12) ----*/
+#define BIT_TEMP_OUT                0x80
+#define BIT_GYRO_XOUT               0x40
+#define BIT_GYRO_YOUT               0x20
+#define BIT_GYRO_ZOUT               0x10
+#define BIT_ACCEL_XOUT              0x08
+#define BIT_ACCEL_YOUT              0x04
+#define BIT_ACCEL_ZOUT              0x02
+#define BIT_AUX_1OUT                0x01
+/*---- MPU 'FIFO_EN2' register (13) ----*/
+#define BIT_AUX_2OUT                0x02
+#define BIT_AUX_3OUT                0x01
+/*---- MPU 'DLPF_FS_SYNC' register (16) ----*/
+#define BITS_EXT_SYNC_NONE          0x00
+#define BITS_EXT_SYNC_TEMP          0x20
+#define BITS_EXT_SYNC_GYROX         0x40
+#define BITS_EXT_SYNC_GYROY         0x60
+#define BITS_EXT_SYNC_GYROZ         0x80
+#define BITS_EXT_SYNC_ACCELX        0xA0
+#define BITS_EXT_SYNC_ACCELY        0xC0
+#define BITS_EXT_SYNC_ACCELZ        0xE0
+#define BITS_EXT_SYNC_MASK          0xE0
+#define BITS_FS_250DPS              0x00
+#define BITS_FS_500DPS              0x08
+#define BITS_FS_1000DPS             0x10
+#define BITS_FS_2000DPS             0x18
+#define BITS_FS_MASK                0x18
+#define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
+#define BITS_DLPF_CFG_188HZ         0x01
+#define BITS_DLPF_CFG_98HZ          0x02
+#define BITS_DLPF_CFG_42HZ          0x03
+#define BITS_DLPF_CFG_20HZ          0x04
+#define BITS_DLPF_CFG_10HZ          0x05
+#define BITS_DLPF_CFG_5HZ           0x06
+#define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
+#define BITS_DLPF_CFG_MASK          0x07
+/*---- MPU 'INT_CFG' register (17) ----*/
+#define BIT_ACTL                    0x80
+#define BIT_ACTL_LOW                0x80
+#define BIT_ACTL_HIGH               0x00
+#define BIT_OPEN                    0x40
+#define BIT_OPEN_DRAIN              0x40
+#define BIT_PUSH_PULL               0x00
+#define BIT_LATCH_INT_EN            0x20
+#define BIT_INT_PULSE_WIDTH_50US    0x00
+#define BIT_INT_ANYRD_2CLEAR        0x10
+#define BIT_INT_STAT_READ_2CLEAR    0x00
+#define BIT_MPU_RDY_EN              0x04
+#define BIT_DMP_INT_EN              0x02
+#define BIT_RAW_RDY_EN              0x01
+/*---- MPU 'INT_STATUS' register (1A) ----*/
+#define BIT_INT_STATUS_FIFO_OVERLOW 0x80
+#define BIT_MPU_RDY                 0x04
+#define BIT_DMP_INT                 0x02
+#define BIT_RAW_RDY                 0x01
+/*---- MPU 'BANK_SEL' register (37) ----*/
+#define BIT_PRFTCH_EN               0x20
+#define BIT_CFG_USER_BANK           0x10
+#define BITS_MEM_SEL                0x0f
+/*---- MPU 'USER_CTRL' register (3D) ----*/
+#define BIT_DMP_EN                  0x80
+#define BIT_FIFO_EN                 0x40
+#define BIT_AUX_IF_EN               0x20
+#define BIT_AUX_RD_LENG             0x10
+#define BIT_AUX_IF_RST              0x08
+#define BIT_DMP_RST                 0x04
+#define BIT_FIFO_RST                0x02
+#define BIT_GYRO_RST                0x01
+/*---- MPU 'PWR_MGM' register (3E) ----*/
+#define BIT_H_RESET                 0x80
+#define BIT_SLEEP                   0x40
+#define BIT_STBY_XG                 0x20
+#define BIT_STBY_YG                 0x10
+#define BIT_STBY_ZG                 0x08
+#define BITS_CLKSEL                 0x07
+
+/*---- MPU Silicon Revision ----*/
+#define MPU_SILICON_REV_A4           1  /* MPU A4 Device */
+#define MPU_SILICON_REV_B1           2  /* MPU B1 Device */
+#define MPU_SILICON_REV_B4           3  /* MPU B4 Device */
+#define MPU_SILICON_REV_B6           4  /* MPU B6 Device */
+
+/*---- MPU Memory ----*/
+#define MPU_MEM_BANK_SIZE           (256)
+#define FIFO_HW_SIZE                (512)
+
+enum MPU_MEMORY_BANKS {
+        MPU_MEM_RAM_BANK_0 = 0,
+        MPU_MEM_RAM_BANK_1,
+        MPU_MEM_RAM_BANK_2,
+        MPU_MEM_RAM_BANK_3,
+        MPU_MEM_NUM_RAM_BANKS,
+        MPU_MEM_OTP_BANK_0 = MPU_MEM_NUM_RAM_BANKS,
+        /* This one is always last */
+        MPU_MEM_NUM_BANKS
+};
+
+/*---- structure containing control variables used by MLDL ----*/
+/*---- MPU clock source settings ----*/
+/*---- MPU filter selections ----*/
+enum mpu_filter {
+        MPU_FILTER_256HZ_NOLPF2 = 0,
+        MPU_FILTER_188HZ,
+        MPU_FILTER_98HZ,
+        MPU_FILTER_42HZ,
+        MPU_FILTER_20HZ,
+        MPU_FILTER_10HZ,
+        MPU_FILTER_5HZ,
+        MPU_FILTER_2100HZ_NOLPF,
+        NUM_MPU_FILTER
+};
+
+enum mpu_fullscale {
+        MPU_FS_250DPS = 0,
+        MPU_FS_500DPS,
+        MPU_FS_1000DPS,
+        MPU_FS_2000DPS,
+        NUM_MPU_FS
+};
+
+enum mpu_clock_sel {
+        MPU_CLK_SEL_INTERNAL = 0,
+        MPU_CLK_SEL_PLLGYROX,
+        MPU_CLK_SEL_PLLGYROY,
+        MPU_CLK_SEL_PLLGYROZ,
+        MPU_CLK_SEL_PLLEXT32K,
+        MPU_CLK_SEL_PLLEXT19M,
+        MPU_CLK_SEL_RESERVED,
+        MPU_CLK_SEL_STOP,
+        NUM_CLK_SEL
+};
+
+enum mpu_ext_sync {
+        MPU_EXT_SYNC_NONE = 0,
+        MPU_EXT_SYNC_TEMP,
+        MPU_EXT_SYNC_GYROX,
+        MPU_EXT_SYNC_GYROY,
+        MPU_EXT_SYNC_GYROZ,
+        MPU_EXT_SYNC_ACCELX,
+        MPU_EXT_SYNC_ACCELY,
+        MPU_EXT_SYNC_ACCELZ,
+        NUM_MPU_EXT_SYNC
+};
+
+#define DLPF_FS_SYNC_VALUE(ext_sync, full_scale, lpf) \
+        ((ext_sync << 5) | (full_scale << 3) | lpf)
+
+#endif                          /* __MPU3050_H_ */
diff --git a/drivers/misc/inv_mpu/mpu3050/Makefile b/drivers/misc/inv_mpu/mpu3050/Makefile
new file mode 100644
index 00000000000..9e573930238
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/Makefile
@@ -0,0 +1,17 @@
+
+# Kernel makefile for motions sensors
+#
+#
+
+obj-$(CONFIG_MPU_SENSORS_MPU3050) += mpu3050.o
+
+ccflags-y := -DMPU_CURRENT_BUILD_MPU3050
+
+mpu3050-objs += mldl_cfg.o
+mpu3050-objs += mldl_print_cfg.o
+mpu3050-objs += mlsl-kernel.o
+mpu3050-objs += mpu-dev.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
+EXTRA_CFLAGS += -DINV_CACHE_DMP=1
diff --git a/drivers/misc/inv_mpu/mpu3050/mldl_cfg.c b/drivers/misc/inv_mpu/mpu3050/mldl_cfg.c
new file mode 100644
index 00000000000..ccacc8ec0b5
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mldl_cfg.c
@@ -0,0 +1,1765 @@
+/*
+        $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 regval;
+
+        unsigned char reg;
+        unsigned char mask;
+
+        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);
+                /* 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;
+                mldl_cfg->inv_mpu_state->status |= MPU_DEVICE_IS_SUSPENDED;
+        } else {
+                mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED;
+                mldl_cfg->inv_mpu_state->status &= ~MPU_DEVICE_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;
+                        mldl_cfg->inv_mpu_state->status |=
+                                MPU_DEVICE_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;
+                        mldl_cfg->inv_mpu_state->status &=
+                                ~MPU_DEVICE_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;
+
+        /* Inconclusive assume it was reset */
+        return true;
+}
+
+
+int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+                         const unsigned char *data, int size)
+{
+        int bank, offset, write_size;
+        int result;
+        unsigned char read[MPU_MEM_BANK_SIZE];
+
+        if (mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED) {
+#if INV_CACHE_DMP == 1
+                memcpy(mldl_cfg->mpu_ram->ram, data, size);
+                return INV_SUCCESS;
+#else
+                LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
+                return INV_ERROR_MEMORY_SET;
+#endif
+        }
+
+        if (!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)) {
+                LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
+                return INV_ERROR_MEMORY_SET;
+        }
+        /* Write and verify memory */
+        for (bank = 0; size > 0; bank++,
+                        size -= write_size,
+                        data += write_size) {
+                if (size > MPU_MEM_BANK_SIZE)
+                        write_size = MPU_MEM_BANK_SIZE;
+                else
+                        write_size = size;
+
+                result = inv_serial_write_mem(mlsl_handle,
+                                mldl_cfg->mpu_chip_info->addr,
+                                ((bank << 8) | 0x00),
+                                write_size,
+                                data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        MPL_LOGE("Write mem error in bank %d\n", bank);
+                        return result;
+                }
+                result = inv_serial_read_mem(mlsl_handle,
+                                mldl_cfg->mpu_chip_info->addr,
+                                ((bank << 8) | 0x00),
+                                write_size,
+                                read);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        MPL_LOGE("Read mem error in bank %d\n", bank);
+                        return result;
+                }
+
+#define ML_SKIP_CHECK 20
+                for (offset = 0; offset < write_size; offset++) {
+                        /* skip the register memory locations */
+                        if (bank == 0 && offset < ML_SKIP_CHECK)
+                                continue;
+                        if (data[offset] != read[offset]) {
+                                result = INV_ERROR_SERIAL_WRITE;
+                                break;
+                        }
+                }
+                if (result != INV_SUCCESS) {
+                        LOG_RESULT_LOCATION(result);
+                        MPL_LOGE("Read data mismatch at bank %d, offset %d\n",
+                                bank, offset);
+                        return result;
+                }
+        }
+        return INV_SUCCESS;
+}
+
+static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle,
+                       unsigned long sensors)
+{
+        int result;
+        int ii;
+        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_CONFIG) &&
+            !mpu_was_reset(mldl_cfg, gyro_handle)) {
+                return INV_SUCCESS;
+        }
+
+        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 = 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;
+        }
+        result = mpu_set_clock_source(gyro_handle, mldl_cfg);
+        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 */
+#if INV_CACHE_DMP != 0
+        inv_mpu_set_firmware(mldl_cfg, gyro_handle,
+                mldl_cfg->mpu_ram->ram, mldl_cfg->mpu_ram->length);
+#endif
+
+        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;
+        }
+        mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_CONFIG;
+
+        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_CONFIG;
+        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 *gyro_handle,
+                 void *accel_handle,
+                 void *compass_handle, void *pressure_handle)
+{
+        int result;
+        void *slave_handle[EXT_SLAVE_NUM_TYPES];
+        int ii;
+
+        /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
+        ii = 0;
+        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 |
+                MPU_DEVICE_IS_SUSPENDED;
+        mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
+
+        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;
+
+        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, gyro_handle, RESET, 0, 0, 0, 0);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_get_silicon_rev(mldl_cfg, gyro_handle);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Get the factory temperature compensation offsets */
+        result = inv_serial_read(gyro_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(gyro_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(gyro_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, gyro_handle, true);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
+                        mldl_cfg->pdata->level_shifter);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+
+#if INV_CACHE_DMP != 0
+        result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, 0, SLEEP, 0, 0, 0);
+#endif
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+
+        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 *gyro_handle,
+                  void *accel_handle,
+                  void *compass_handle,
+                  void *pressure_handle)
+{
+        return 0;
+}
+
+/**
+ *  @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) ||
+                (mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG))) {
+                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;
+                }
+                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];
+        void *slave_handle[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);
+
+        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;
+
+        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(slave_handle[ii],
+                                                  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;
+}
+
+/**
+ * @}
+ */
diff --git a/drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c b/drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c
new file mode 100644
index 00000000000..e2b8d30ceba
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c
@@ -0,0 +1,137 @@
+/*
+        $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_print_cfg.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+#include <stddef.h>
+#include "mldl_cfg.h"
+#include "mlsl.h"
+#include "linux/mpu.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_print_cfg:"
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+void mldl_print_cfg(struct mldl_cfg *mldl_cfg)
+{
+        struct mpu_gyro_cfg     *mpu_gyro_cfg   = mldl_cfg->mpu_gyro_cfg;
+        struct mpu_offsets      *mpu_offsets    = mldl_cfg->mpu_offsets;
+        struct mpu_chip_info    *mpu_chip_info  = mldl_cfg->mpu_chip_info;
+        struct inv_mpu_cfg      *inv_mpu_cfg    = mldl_cfg->inv_mpu_cfg;
+        struct inv_mpu_state    *inv_mpu_state  = mldl_cfg->inv_mpu_state;
+        struct ext_slave_descr  **slave         = mldl_cfg->slave;
+        struct mpu_platform_data *pdata         = mldl_cfg->pdata;
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        /* mpu_gyro_cfg */
+        MPL_LOGV("int_config     = %02x\n", mpu_gyro_cfg->int_config);
+        MPL_LOGV("ext_sync       = %02x\n", mpu_gyro_cfg->ext_sync);
+        MPL_LOGV("full_scale     = %02x\n", mpu_gyro_cfg->full_scale);
+        MPL_LOGV("lpf            = %02x\n", mpu_gyro_cfg->lpf);
+        MPL_LOGV("clk_src        = %02x\n", mpu_gyro_cfg->clk_src);
+        MPL_LOGV("divider        = %02x\n", mpu_gyro_cfg->divider);
+        MPL_LOGV("dmp_enable     = %02x\n", mpu_gyro_cfg->dmp_enable);
+        MPL_LOGV("fifo_enable    = %02x\n", mpu_gyro_cfg->fifo_enable);
+        MPL_LOGV("dmp_cfg1       = %02x\n", mpu_gyro_cfg->dmp_cfg1);
+        MPL_LOGV("dmp_cfg2       = %02x\n", mpu_gyro_cfg->dmp_cfg2);
+        /* mpu_offsets */
+        MPL_LOGV("tc[0]      = %02x\n", mpu_offsets->tc[0]);
+        MPL_LOGV("tc[1]      = %02x\n", mpu_offsets->tc[1]);
+        MPL_LOGV("tc[2]      = %02x\n", mpu_offsets->tc[2]);
+        MPL_LOGV("gyro[0]    = %04x\n", mpu_offsets->gyro[0]);
+        MPL_LOGV("gyro[1]    = %04x\n", mpu_offsets->gyro[1]);
+        MPL_LOGV("gyro[2]    = %04x\n", mpu_offsets->gyro[2]);
+
+        /* mpu_chip_info */
+        MPL_LOGV("addr            = %02x\n", mldl_cfg->mpu_chip_info->addr);
+
+        MPL_LOGV("silicon_revision = %02x\n", mpu_chip_info->silicon_revision);
+        MPL_LOGV("product_revision = %02x\n", mpu_chip_info->product_revision);
+        MPL_LOGV("product_id       = %02x\n", mpu_chip_info->product_id);
+        MPL_LOGV("gyro_sens_trim   = %02x\n", mpu_chip_info->gyro_sens_trim);
+
+        MPL_LOGV("requested_sensors = %04x\n", inv_mpu_cfg->requested_sensors);
+        MPL_LOGV("ignore_system_suspend= %04x\n",
+                inv_mpu_cfg->ignore_system_suspend);
+        MPL_LOGV("status = %04x\n", inv_mpu_state->status);
+        MPL_LOGV("i2c_slaves_enabled= %04x\n",
+                inv_mpu_state->i2c_slaves_enabled);
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!slave[ii])
+                        continue;
+                MPL_LOGV("SLAVE %d:\n", ii);
+                MPL_LOGV("    suspend  = %02x\n", (int)slave[ii]->suspend);
+                MPL_LOGV("    resume   = %02x\n", (int)slave[ii]->resume);
+                MPL_LOGV("    read     = %02x\n", (int)slave[ii]->read);
+                MPL_LOGV("    type     = %02x\n", slave[ii]->type);
+                MPL_LOGV("    reg      = %02x\n", slave[ii]->read_reg);
+                MPL_LOGV("    len      = %02x\n", slave[ii]->read_len);
+                MPL_LOGV("    endian   = %02x\n", slave[ii]->endian);
+                MPL_LOGV("    range.mantissa= %02x\n",
+                        slave[ii]->range.mantissa);
+                MPL_LOGV("    range.fraction= %02x\n",
+                        slave[ii]->range.fraction);
+        }
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        continue;
+                MPL_LOGV("PDATA_SLAVE[%d]\n", ii);
+                MPL_LOGV("    irq        = %02x\n", pdata_slave[ii]->irq);
+                MPL_LOGV("    adapt_num  = %02x\n", pdata_slave[ii]->adapt_num);
+                MPL_LOGV("    bus        = %02x\n", pdata_slave[ii]->bus);
+                MPL_LOGV("    address    = %02x\n", pdata_slave[ii]->address);
+                MPL_LOGV("    orientation=\n"
+                        "                            %2d %2d %2d\n"
+                        "                            %2d %2d %2d\n"
+                        "                            %2d %2d %2d\n",
+                        pdata_slave[ii]->orientation[0],
+                        pdata_slave[ii]->orientation[1],
+                        pdata_slave[ii]->orientation[2],
+                        pdata_slave[ii]->orientation[3],
+                        pdata_slave[ii]->orientation[4],
+                        pdata_slave[ii]->orientation[5],
+                        pdata_slave[ii]->orientation[6],
+                        pdata_slave[ii]->orientation[7],
+                        pdata_slave[ii]->orientation[8]);
+        }
+
+        MPL_LOGV("pdata->int_config         = %02x\n", pdata->int_config);
+        MPL_LOGV("pdata->level_shifter      = %02x\n", pdata->level_shifter);
+        MPL_LOGV("pdata->orientation        =\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n",
+                 pdata->orientation[0], pdata->orientation[1],
+                 pdata->orientation[2], pdata->orientation[3],
+                 pdata->orientation[4], pdata->orientation[5],
+                 pdata->orientation[6], pdata->orientation[7],
+                 pdata->orientation[8]);
+}
diff --git a/drivers/misc/inv_mpu/mpu3050/mlsl-kernel.c b/drivers/misc/inv_mpu/mpu3050/mlsl-kernel.c
new file mode 100644
index 00000000000..19adf5182c0
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mlsl-kernel.c
@@ -0,0 +1,420 @@
+/*
+        $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/>.
+        $
+ */
+
+#include "mlsl.h"
+#include <linux/i2c.h>
+#include "log.h"
+#include "mpu3050.h"
+
+static int inv_i2c_write(struct i2c_adapter *i2c_adap,
+                            unsigned char address,
+                            unsigned int len, unsigned char const *data)
+{
+        struct i2c_msg msgs[1];
+        int res;
+
+        if (!data || !i2c_adap) {
+                LOG_RESULT_LOCATION(-EINVAL);
+                return -EINVAL;
+        }
+
+        msgs[0].addr = address;
+        msgs[0].flags = 0;      /* write */
+        msgs[0].buf = (unsigned char *)data;
+        msgs[0].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 1);
+        if (res < 1) {
+                if (res == 0)
+                        res = -EIO;
+                LOG_RESULT_LOCATION(res);
+                return res;
+        } else
+                return 0;
+}
+
+static int inv_i2c_write_register(struct i2c_adapter *i2c_adap,
+                                     unsigned char address,
+                                     unsigned char reg, unsigned char value)
+{
+        unsigned char data[2];
+
+        data[0] = reg;
+        data[1] = value;
+        return inv_i2c_write(i2c_adap, address, 2, data);
+}
+
+static int inv_i2c_read(struct i2c_adapter *i2c_adap,
+                           unsigned char address, unsigned char reg,
+                           unsigned int len, unsigned char *data)
+{
+        struct i2c_msg msgs[2];
+        int res;
+
+        if (!data || !i2c_adap) {
+                LOG_RESULT_LOCATION(-EINVAL);
+                return -EINVAL;
+        }
+
+        msgs[0].addr = address;
+        msgs[0].flags = 0;      /* write */
+        msgs[0].buf = &reg;
+        msgs[0].len = 1;
+
+        msgs[1].addr = address;
+        msgs[1].flags = I2C_M_RD;
+        msgs[1].buf = data;
+        msgs[1].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 2);
+        if (res < 2) {
+                if (res >= 0)
+                        res = -EIO;
+                LOG_RESULT_LOCATION(res);
+                return res;
+        } else
+                return 0;
+}
+
+static int mpu_memory_read(struct i2c_adapter *i2c_adap,
+                           unsigned char mpu_addr,
+                           unsigned short mem_addr,
+                           unsigned int len, unsigned char *data)
+{
+        unsigned char bank[2];
+        unsigned char addr[2];
+        unsigned char buf;
+
+        struct i2c_msg msgs[4];
+        int res;
+
+        if (!data || !i2c_adap) {
+                LOG_RESULT_LOCATION(-EINVAL);
+                return -EINVAL;
+        }
+
+        bank[0] = MPUREG_BANK_SEL;
+        bank[1] = mem_addr >> 8;
+
+        addr[0] = MPUREG_MEM_START_ADDR;
+        addr[1] = mem_addr & 0xFF;
+
+        buf = MPUREG_MEM_R_W;
+
+        /* write message */
+        msgs[0].addr = mpu_addr;
+        msgs[0].flags = 0;
+        msgs[0].buf = bank;
+        msgs[0].len = sizeof(bank);
+
+        msgs[1].addr = mpu_addr;
+        msgs[1].flags = 0;
+        msgs[1].buf = addr;
+        msgs[1].len = sizeof(addr);
+
+        msgs[2].addr = mpu_addr;
+        msgs[2].flags = 0;
+        msgs[2].buf = &buf;
+        msgs[2].len = 1;
+
+        msgs[3].addr = mpu_addr;
+        msgs[3].flags = I2C_M_RD;
+        msgs[3].buf = data;
+        msgs[3].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 4);
+        if (res != 4) {
+                if (res >= 0)
+                        res = -EIO;
+                LOG_RESULT_LOCATION(res);
+                return res;
+        } else
+                return 0;
+}
+
+static int mpu_memory_write(struct i2c_adapter *i2c_adap,
+                            unsigned char mpu_addr,
+                            unsigned short mem_addr,
+                            unsigned int len, unsigned char const *data)
+{
+        unsigned char bank[2];
+        unsigned char addr[2];
+        unsigned char buf[513];
+
+        struct i2c_msg msgs[3];
+        int res;
+
+        if (!data || !i2c_adap) {
+                LOG_RESULT_LOCATION(-EINVAL);
+                return -EINVAL;
+        }
+        if (len >= (sizeof(buf) - 1)) {
+                LOG_RESULT_LOCATION(-ENOMEM);
+                return -ENOMEM;
+        }
+
+        bank[0] = MPUREG_BANK_SEL;
+        bank[1] = mem_addr >> 8;
+
+        addr[0] = MPUREG_MEM_START_ADDR;
+        addr[1] = mem_addr & 0xFF;
+
+        buf[0] = MPUREG_MEM_R_W;
+        memcpy(buf + 1, data, len);
+
+        /* write message */
+        msgs[0].addr = mpu_addr;
+        msgs[0].flags = 0;
+        msgs[0].buf = bank;
+        msgs[0].len = sizeof(bank);
+
+        msgs[1].addr = mpu_addr;
+        msgs[1].flags = 0;
+        msgs[1].buf = addr;
+        msgs[1].len = sizeof(addr);
+
+        msgs[2].addr = mpu_addr;
+        msgs[2].flags = 0;
+        msgs[2].buf = (unsigned char *)buf;
+        msgs[2].len = len + 1;
+
+        res = i2c_transfer(i2c_adap, msgs, 3);
+        if (res != 3) {
+                if (res >= 0)
+                        res = -EIO;
+                LOG_RESULT_LOCATION(res);
+                return res;
+        } else
+                return 0;
+}
+
+int inv_serial_single_write(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned char register_addr,
+        unsigned char data)
+{
+        return inv_i2c_write_register((struct i2c_adapter *)sl_handle,
+                                      slave_addr, register_addr, data);
+}
+EXPORT_SYMBOL(inv_serial_single_write);
+
+int inv_serial_write(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short length,
+        unsigned char const *data)
+{
+        int result;
+        const unsigned short data_length = length - 1;
+        const unsigned char start_reg_addr = data[0];
+        unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
+        unsigned short bytes_written = 0;
+
+        while (bytes_written < data_length) {
+                unsigned short this_len = min(SERIAL_MAX_TRANSFER_SIZE,
+                                             data_length - bytes_written);
+                if (bytes_written == 0) {
+                        result = inv_i2c_write((struct i2c_adapter *)
+                                               sl_handle, slave_addr,
+                                               1 + this_len, data);
+                } else {
+                        /* manually increment register addr between chunks */
+                        i2c_write[0] = start_reg_addr + bytes_written;
+                        memcpy(&i2c_write[1], &data[1 + bytes_written],
+                                this_len);
+                        result = inv_i2c_write((struct i2c_adapter *)
+                                               sl_handle, slave_addr,
+                                               1 + this_len, i2c_write);
+                }
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_written += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_write);
+
+int inv_serial_read(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned char register_addr,
+        unsigned short length,
+        unsigned char *data)
+{
+        int result;
+        unsigned short bytes_read = 0;
+
+        if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR
+                && (register_addr == MPUREG_FIFO_R_W ||
+                    register_addr == MPUREG_MEM_R_W)) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        while (bytes_read < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+                result = inv_i2c_read((struct i2c_adapter *)sl_handle,
+                                      slave_addr, register_addr + bytes_read,
+                                      this_len, &data[bytes_read]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_read += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_read);
+
+int inv_serial_write_mem(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short mem_addr,
+        unsigned short length,
+        unsigned char const *data)
+{
+        int result;
+        unsigned short bytes_written = 0;
+
+        if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+                pr_err("memory read length (%d B) extends beyond its"
+                       " limits (%d) if started at location %d\n", length,
+                       MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+        while (bytes_written < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
+                result = mpu_memory_write((struct i2c_adapter *)sl_handle,
+                                          slave_addr, mem_addr + bytes_written,
+                                          this_len, &data[bytes_written]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_written += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_write_mem);
+
+int inv_serial_read_mem(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short mem_addr,
+        unsigned short length,
+        unsigned char *data)
+{
+        int result;
+        unsigned short bytes_read = 0;
+
+        if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+                printk
+                    ("memory read length (%d B) extends beyond its limits (%d) "
+                     "if started at location %d\n", length,
+                     MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+        while (bytes_read < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+                result =
+                    mpu_memory_read((struct i2c_adapter *)sl_handle,
+                                    slave_addr, mem_addr + bytes_read,
+                                    this_len, &data[bytes_read]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_read += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_read_mem);
+
+int inv_serial_write_fifo(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short length,
+        unsigned char const *data)
+{
+        int result;
+        unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
+        unsigned short bytes_written = 0;
+
+        if (length > FIFO_HW_SIZE) {
+                printk(KERN_ERR
+                       "maximum fifo write length is %d\n", FIFO_HW_SIZE);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+        while (bytes_written < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
+                i2c_write[0] = MPUREG_FIFO_R_W;
+                memcpy(&i2c_write[1], &data[bytes_written], this_len);
+                result = inv_i2c_write((struct i2c_adapter *)sl_handle,
+                                       slave_addr, this_len + 1, i2c_write);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_written += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_write_fifo);
+
+int inv_serial_read_fifo(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short length,
+        unsigned char *data)
+{
+        int result;
+        unsigned short bytes_read = 0;
+
+        if (length > FIFO_HW_SIZE) {
+                printk(KERN_ERR
+                       "maximum fifo read length is %d\n", FIFO_HW_SIZE);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+        while (bytes_read < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+                result = inv_i2c_read((struct i2c_adapter *)sl_handle,
+                                      slave_addr, MPUREG_FIFO_R_W, this_len,
+                                      &data[bytes_read]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_read += this_len;
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_read_fifo);
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/mpu3050/mpu-dev.c b/drivers/misc/inv_mpu/mpu3050/mpu-dev.c
new file mode 100644
index 00000000000..c98ed3b4a80
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mpu-dev.c
@@ -0,0 +1,1250 @@
+/*
+        $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/>.
+        $
+ */
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/pm.h>
+#include <linux/mutex.h>
+#include <linux/suspend.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "mpuirq.h"
+#include "slaveirq.h"
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#include <linux/mpu.h>
+
+
+/* Platform data for the MPU */
+struct mpu_private_data {
+        struct miscdevice dev;
+        struct i2c_client *client;
+
+        /* mldl_cfg data */
+        struct mldl_cfg mldl_cfg;
+        struct mpu_ram          mpu_ram;
+        struct mpu_gyro_cfg     mpu_gyro_cfg;
+        struct mpu_offsets      mpu_offsets;
+        struct mpu_chip_info    mpu_chip_info;
+        struct inv_mpu_cfg      inv_mpu_cfg;
+        struct inv_mpu_state    inv_mpu_state;
+
+        struct mutex mutex;
+        wait_queue_head_t mpu_event_wait;
+        struct completion completion;
+        struct timer_list timeout;
+        struct notifier_block nb;
+        struct mpuirq_data mpu_pm_event;
+        int response_timeout;   /* In seconds */
+        unsigned long event;
+        int pid;
+        struct module *slave_modules[EXT_SLAVE_NUM_TYPES];
+};
+
+struct mpu_private_data *mpu_private_data;
+
+static void mpu_pm_timeout(u_long data)
+{
+        struct mpu_private_data *mpu = (struct mpu_private_data *)data;
+        struct i2c_client *client = mpu->client;
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+        complete(&mpu->completion);
+}
+
+static int mpu_pm_notifier_callback(struct notifier_block *nb,
+                                    unsigned long event, void *unused)
+{
+        struct mpu_private_data *mpu =
+            container_of(nb, struct mpu_private_data, nb);
+        struct i2c_client *client = mpu->client;
+        struct timeval event_time;
+        dev_dbg(&client->adapter->dev, "%s: %ld\n", __func__, event);
+
+        /* Prevent the file handle from being closed before we initialize
+           the completion event */
+        mutex_lock(&mpu->mutex);
+        if (!(mpu->pid) ||
+            (event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) {
+                mutex_unlock(&mpu->mutex);
+                return NOTIFY_OK;
+        }
+
+        if (event == PM_SUSPEND_PREPARE)
+                mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE;
+        if (event == PM_POST_SUSPEND)
+                mpu->event = MPU_PM_EVENT_POST_SUSPEND;
+
+        do_gettimeofday(&event_time);
+        mpu->mpu_pm_event.interruptcount++;
+        mpu->mpu_pm_event.irqtime =
+            (((long long)event_time.tv_sec) << 32) + event_time.tv_usec;
+        mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT;
+        mpu->mpu_pm_event.data = mpu->event;
+
+        if (mpu->response_timeout > 0) {
+                mpu->timeout.expires = jiffies + mpu->response_timeout * HZ;
+                add_timer(&mpu->timeout);
+        }
+        INIT_COMPLETION(mpu->completion);
+        mutex_unlock(&mpu->mutex);
+
+        wake_up_interruptible(&mpu->mpu_event_wait);
+        wait_for_completion(&mpu->completion);
+        del_timer_sync(&mpu->timeout);
+        dev_dbg(&client->adapter->dev, "%s: %ld DONE\n", __func__, event);
+        return NOTIFY_OK;
+}
+
+static int mpu_dev_open(struct inode *inode, struct file *file)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        struct i2c_client *client = mpu->client;
+        int result;
+        int ii;
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+        dev_dbg(&client->adapter->dev, "current->pid %d\n", current->pid);
+
+        result = mutex_lock_interruptible(&mpu->mutex);
+        if (mpu->pid) {
+                mutex_unlock(&mpu->mutex);
+                return -EBUSY;
+        }
+        mpu->pid = current->pid;
+
+        /* Reset the sensors to the default */
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "%s: mutex_lock_interruptible returned %d\n",
+                        __func__, result);
+                return result;
+        }
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
+                __module_get(mpu->slave_modules[ii]);
+
+        mutex_unlock(&mpu->mutex);
+        return 0;
+}
+
+/* close function - called when the "file" /dev/mpu is closed in userspace   */
+static int mpu_release(struct inode *inode, struct file *file)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        struct i2c_client *client = mpu->client;
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        int result = 0;
+        int ii;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        mutex_lock(&mpu->mutex);
+        mldl_cfg->inv_mpu_cfg->requested_sensors = 0;
+        result = inv_mpu_suspend(mldl_cfg,
+                                slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                                slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                                slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                                slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                                INV_ALL_SENSORS);
+        mpu->pid = 0;
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
+                module_put(mpu->slave_modules[ii]);
+
+        mutex_unlock(&mpu->mutex);
+        complete(&mpu->completion);
+        dev_dbg(&client->adapter->dev, "mpu_release\n");
+
+        return result;
+}
+
+/* read function called when from /dev/mpu is read.  Read from the FIFO */
+static ssize_t mpu_read(struct file *file,
+                        char __user *buf, size_t count, loff_t *offset)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        struct i2c_client *client = mpu->client;
+        size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long);
+        int err;
+
+        if (!mpu->event && (!(file->f_flags & O_NONBLOCK)))
+                wait_event_interruptible(mpu->mpu_event_wait, mpu->event);
+
+        if (!mpu->event || !buf
+            || count < sizeof(mpu->mpu_pm_event))
+                return 0;
+
+        err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event));
+        if (err) {
+                dev_err(&client->adapter->dev,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        mpu->event = 0;
+        return len;
+}
+
+static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        int mask = 0;
+
+        poll_wait(file, &mpu->mpu_event_wait, poll);
+        if (mpu->event)
+                mask |= POLLIN | POLLRDNORM;
+        return mask;
+}
+
+static int mpu_dev_ioctl_get_ext_slave_platform_data(
+        struct i2c_client *client,
+        struct ext_slave_platform_data __user *arg)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct ext_slave_platform_data *pdata_slave;
+        struct ext_slave_platform_data local_pdata_slave;
+
+        if (copy_from_user(&local_pdata_slave, arg, sizeof(local_pdata_slave)))
+                return -EFAULT;
+
+        if (local_pdata_slave.type >= EXT_SLAVE_NUM_TYPES)
+                return -EINVAL;
+
+        pdata_slave = mpu->mldl_cfg.pdata_slave[local_pdata_slave.type];
+        /* All but private data and irq_data */
+        if (!pdata_slave)
+                return -ENODEV;
+        if (copy_to_user(arg, pdata_slave, sizeof(*pdata_slave)))
+                return -EFAULT;
+        return 0;
+}
+
+static int mpu_dev_ioctl_get_mpu_platform_data(
+        struct i2c_client *client,
+        struct mpu_platform_data __user *arg)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct mpu_platform_data *pdata = mpu->mldl_cfg.pdata;
+
+        if (copy_to_user(arg, pdata, sizeof(*pdata)))
+                return -EFAULT;
+        return 0;
+}
+
+static int mpu_dev_ioctl_get_ext_slave_descr(
+        struct i2c_client *client,
+        struct ext_slave_descr __user *arg)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct ext_slave_descr *slave;
+        struct ext_slave_descr local_slave;
+
+        if (copy_from_user(&local_slave, arg, sizeof(local_slave)))
+                return -EFAULT;
+
+        if (local_slave.type >= EXT_SLAVE_NUM_TYPES)
+                return -EINVAL;
+
+        slave = mpu->mldl_cfg.slave[local_slave.type];
+        /* All but private data and irq_data */
+        if (!slave)
+                return -ENODEV;
+        if (copy_to_user(arg, slave, sizeof(*slave)))
+                return -EFAULT;
+        return 0;
+}
+
+
+/**
+ * slave_config() - Pass a requested slave configuration to the slave sensor
+ *
+ * @adapter the adaptor to use to communicate with the slave
+ * @mldl_cfg the mldl configuration structuer
+ * @slave pointer to the slave descriptor
+ * @usr_config The configuration to pass to the slave sensor
+ *
+ * returns 0 or non-zero error code
+ */
+static int inv_mpu_config(struct mldl_cfg *mldl_cfg,
+                        void *gyro_adapter,
+                        struct ext_slave_config __user *usr_config)
+{
+        int retval = 0;
+        struct ext_slave_config config;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        if (config.len && config.data) {
+                void *data;
+                data = kmalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data, config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = gyro_config(gyro_adapter, mldl_cfg, &config);
+        kfree(config.data);
+        return retval;
+}
+
+static int inv_mpu_get_config(struct mldl_cfg *mldl_cfg,
+                            void *gyro_adapter,
+                            struct ext_slave_config __user *usr_config)
+{
+        int retval = 0;
+        struct ext_slave_config config;
+        void *user_data;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        user_data = config.data;
+        if (config.len && config.data) {
+                void *data;
+                data = kmalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data, config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = gyro_get_config(gyro_adapter, mldl_cfg, &config);
+        if (!retval)
+                retval = copy_to_user((unsigned char __user *)user_data,
+                                config.data, config.len);
+        kfree(config.data);
+        return retval;
+}
+
+static int slave_config(struct mldl_cfg *mldl_cfg,
+                        void *gyro_adapter,
+                        void *slave_adapter,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_config __user *usr_config)
+{
+        int retval = 0;
+        struct ext_slave_config config;
+        if ((!slave) || (!slave->config))
+                return -ENODEV;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        if (config.len && config.data) {
+                void *data;
+                data = kmalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data, config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = inv_mpu_slave_config(mldl_cfg, gyro_adapter, slave_adapter,
+                                      &config, slave, pdata);
+        kfree(config.data);
+        return retval;
+}
+
+static int slave_get_config(struct mldl_cfg *mldl_cfg,
+                            void *gyro_adapter,
+                            void *slave_adapter,
+                            struct ext_slave_descr *slave,
+                            struct ext_slave_platform_data *pdata,
+                            struct ext_slave_config __user *usr_config)
+{
+        int retval = 0;
+        struct ext_slave_config config;
+        void *user_data;
+        if (!(slave) || !(slave->get_config))
+                return -ENODEV;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        user_data = config.data;
+        if (config.len && config.data) {
+                void *data;
+                data = kmalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data, config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = inv_mpu_get_slave_config(mldl_cfg, gyro_adapter,
+                                          slave_adapter, &config, slave, pdata);
+        if (retval) {
+                kfree(config.data);
+                return retval;
+        }
+        retval = copy_to_user((unsigned char __user *)user_data,
+                              config.data, config.len);
+        kfree(config.data);
+        return retval;
+}
+
+static int inv_slave_read(struct mldl_cfg *mldl_cfg,
+                          void *gyro_adapter,
+                          void *slave_adapter,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata,
+                          void __user *usr_data)
+{
+        int retval;
+        unsigned char *data;
+        data = kzalloc(slave->read_len, GFP_KERNEL);
+        if (!data)
+                return -EFAULT;
+
+        retval = inv_mpu_slave_read(mldl_cfg, gyro_adapter, slave_adapter,
+                                    slave, pdata, data);
+
+        if ((!retval) &&
+            (copy_to_user((unsigned char __user *)usr_data,
+                          data, slave->read_len)))
+                retval = -EFAULT;
+
+        kfree(data);
+        return retval;
+}
+
+static int mpu_handle_mlsl(void *sl_handle,
+                           unsigned char addr,
+                           unsigned int cmd,
+                           struct mpu_read_write __user *usr_msg)
+{
+        int retval = 0;
+        struct mpu_read_write msg;
+        unsigned char *user_data;
+        retval = copy_from_user(&msg, usr_msg, sizeof(msg));
+        if (retval)
+                return -EFAULT;
+
+        user_data = msg.data;
+        if (msg.length && msg.data) {
+                unsigned char *data;
+                data = kmalloc(msg.length, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)msg.data, msg.length);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                msg.data = data;
+        } else {
+                return -EPERM;
+        }
+
+        switch (cmd) {
+        case MPU_READ:
+                retval = inv_serial_read(sl_handle, addr,
+                                         msg.address, msg.length, msg.data);
+                break;
+        case MPU_WRITE:
+                retval = inv_serial_write(sl_handle, addr,
+                                          msg.length, msg.data);
+                break;
+        case MPU_READ_MEM:
+                retval = inv_serial_read_mem(sl_handle, addr,
+                                             msg.address, msg.length, msg.data);
+                break;
+        case MPU_WRITE_MEM:
+                retval = inv_serial_write_mem(sl_handle, addr,
+                                              msg.address, msg.length,
+                                              msg.data);
+                break;
+        case MPU_READ_FIFO:
+                retval = inv_serial_read_fifo(sl_handle, addr,
+                                              msg.length, msg.data);
+                break;
+        case MPU_WRITE_FIFO:
+                retval = inv_serial_write_fifo(sl_handle, addr,
+                                               msg.length, msg.data);
+                break;
+
+        };
+        if (retval) {
+                dev_err(&((struct i2c_adapter *)sl_handle)->dev,
+                        "%s: i2c %d error %d\n",
+                        __func__, cmd, retval);
+                kfree(msg.data);
+                return retval;
+        }
+        retval = copy_to_user((unsigned char __user *)user_data,
+                              msg.data, msg.length);
+        kfree(msg.data);
+        return retval;
+}
+
+/* ioctl - I/O control */
+static long mpu_dev_ioctl(struct file *file,
+                          unsigned int cmd, unsigned long arg)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        struct i2c_client *client = mpu->client;
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        int retval = 0;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_descr **slave = mldl_cfg->slave;
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        retval = mutex_lock_interruptible(&mpu->mutex);
+        if (retval) {
+                dev_err(&client->adapter->dev,
+                        "%s: mutex_lock_interruptible returned %d\n",
+                        __func__, retval);
+                return retval;
+        }
+
+        switch (cmd) {
+        case MPU_GET_EXT_SLAVE_PLATFORM_DATA:
+                retval = mpu_dev_ioctl_get_ext_slave_platform_data(
+                        client,
+                        (struct ext_slave_platform_data __user *)arg);
+                break;
+        case MPU_GET_MPU_PLATFORM_DATA:
+                retval = mpu_dev_ioctl_get_mpu_platform_data(
+                        client,
+                        (struct mpu_platform_data __user *)arg);
+                break;
+        case MPU_GET_EXT_SLAVE_DESCR:
+                retval = mpu_dev_ioctl_get_ext_slave_descr(
+                        client,
+                        (struct ext_slave_descr __user *)arg);
+                break;
+        case MPU_READ:
+        case MPU_WRITE:
+        case MPU_READ_MEM:
+        case MPU_WRITE_MEM:
+        case MPU_READ_FIFO:
+        case MPU_WRITE_FIFO:
+                retval = mpu_handle_mlsl(
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        mldl_cfg->mpu_chip_info->addr, cmd,
+                        (struct mpu_read_write __user *)arg);
+                break;
+        case MPU_CONFIG_GYRO:
+                retval = inv_mpu_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_CONFIG_ACCEL:
+                retval = slave_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave[EXT_SLAVE_TYPE_ACCEL],
+                        pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_CONFIG_COMPASS:
+                retval = slave_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave[EXT_SLAVE_TYPE_COMPASS],
+                        pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_CONFIG_PRESSURE:
+                retval = slave_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        slave[EXT_SLAVE_TYPE_PRESSURE],
+                        pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_GET_CONFIG_GYRO:
+                retval = inv_mpu_get_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_GET_CONFIG_ACCEL:
+                retval = slave_get_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave[EXT_SLAVE_TYPE_ACCEL],
+                        pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_GET_CONFIG_COMPASS:
+                retval = slave_get_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave[EXT_SLAVE_TYPE_COMPASS],
+                        pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_GET_CONFIG_PRESSURE:
+                retval = slave_get_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        slave[EXT_SLAVE_TYPE_PRESSURE],
+                        pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_SUSPEND:
+                retval = inv_mpu_suspend(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        arg);
+                break;
+        case MPU_RESUME:
+                retval = inv_mpu_resume(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        arg);
+                break;
+        case MPU_PM_EVENT_HANDLED:
+                dev_dbg(&client->adapter->dev, "%s: %d\n", __func__, cmd);
+                complete(&mpu->completion);
+                break;
+        case MPU_READ_ACCEL:
+                retval = inv_slave_read(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave[EXT_SLAVE_TYPE_ACCEL],
+                        pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+                        (unsigned char __user *)arg);
+                break;
+        case MPU_READ_COMPASS:
+                retval = inv_slave_read(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave[EXT_SLAVE_TYPE_COMPASS],
+                        pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+                        (unsigned char __user *)arg);
+                break;
+        case MPU_READ_PRESSURE:
+                retval = inv_slave_read(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        slave[EXT_SLAVE_TYPE_PRESSURE],
+                        pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+                        (unsigned char __user *)arg);
+                break;
+        case MPU_GET_REQUESTED_SENSORS:
+                if (copy_to_user(
+                           (__u32 __user *)arg,
+                           &mldl_cfg->inv_mpu_cfg->requested_sensors,
+                           sizeof(mldl_cfg->inv_mpu_cfg->requested_sensors)))
+                        retval = -EFAULT;
+                break;
+        case MPU_SET_REQUESTED_SENSORS:
+                mldl_cfg->inv_mpu_cfg->requested_sensors = arg;
+                break;
+        case MPU_GET_IGNORE_SYSTEM_SUSPEND:
+                if (copy_to_user(
+                        (unsigned char __user *)arg,
+                        &mldl_cfg->inv_mpu_cfg->ignore_system_suspend,
+                        sizeof(mldl_cfg->inv_mpu_cfg->ignore_system_suspend)))
+                        retval = -EFAULT;
+                break;
+        case MPU_SET_IGNORE_SYSTEM_SUSPEND:
+                mldl_cfg->inv_mpu_cfg->ignore_system_suspend = arg;
+                break;
+        case MPU_GET_MLDL_STATUS:
+                if (copy_to_user(
+                        (unsigned char __user *)arg,
+                        &mldl_cfg->inv_mpu_state->status,
+                        sizeof(mldl_cfg->inv_mpu_state->status)))
+                        retval = -EFAULT;
+                break;
+        case MPU_GET_I2C_SLAVES_ENABLED:
+                if (copy_to_user(
+                        (unsigned char __user *)arg,
+                        &mldl_cfg->inv_mpu_state->i2c_slaves_enabled,
+                        sizeof(mldl_cfg->inv_mpu_state->i2c_slaves_enabled)))
+                        retval = -EFAULT;
+                break;
+        default:
+                dev_err(&client->adapter->dev,
+                        "%s: Unknown cmd %x, arg %lu\n",
+                        __func__, cmd, arg);
+                retval = -EINVAL;
+        };
+
+        mutex_unlock(&mpu->mutex);
+        dev_dbg(&client->adapter->dev, "%s: %08x, %08lx, %d\n",
+                __func__, cmd, arg, retval);
+
+        if (retval > 0)
+                retval = -retval;
+
+        return retval;
+}
+
+void mpu_shutdown(struct i2c_client *client)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        mutex_lock(&mpu->mutex);
+        (void)inv_mpu_suspend(mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        INV_ALL_SENSORS);
+        mutex_unlock(&mpu->mutex);
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+}
+
+int mpu_dev_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        mutex_lock(&mpu->mutex);
+        if (!mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
+                dev_dbg(&client->adapter->dev,
+                        "%s: suspending on event %d\n", __func__, mesg.event);
+                (void)inv_mpu_suspend(mldl_cfg,
+                                slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                                slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                                slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                                slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                                INV_ALL_SENSORS);
+        } else {
+                dev_dbg(&client->adapter->dev,
+                        "%s: Already suspended %d\n", __func__, mesg.event);
+        }
+        mutex_unlock(&mpu->mutex);
+        return 0;
+}
+
+int mpu_dev_resume(struct i2c_client *client)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        mutex_lock(&mpu->mutex);
+        if (mpu->pid && !mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
+                (void)inv_mpu_resume(mldl_cfg,
+                                slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                                slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                                slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                                slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                                mldl_cfg->inv_mpu_cfg->requested_sensors);
+                dev_dbg(&client->adapter->dev,
+                        "%s for pid %d\n", __func__, mpu->pid);
+        }
+        mutex_unlock(&mpu->mutex);
+        return 0;
+}
+
+/* define which file operations are supported */
+static const struct file_operations mpu_fops = {
+        .owner = THIS_MODULE,
+        .read = mpu_read,
+        .poll = mpu_poll,
+        .unlocked_ioctl = mpu_dev_ioctl,
+        .open = mpu_dev_open,
+        .release = mpu_release,
+};
+
+int inv_mpu_register_slave(struct module *slave_module,
+                        struct i2c_client *slave_client,
+                        struct ext_slave_platform_data *slave_pdata,
+                        struct ext_slave_descr *(*get_slave_descr)(void))
+{
+        struct mpu_private_data *mpu = mpu_private_data;
+        struct mldl_cfg *mldl_cfg;
+        struct ext_slave_descr *slave_descr;
+        struct ext_slave_platform_data **pdata_slave;
+        char *irq_name = NULL;
+        int result = 0;
+
+        if (!slave_client || !slave_pdata || !get_slave_descr)
+                return -EINVAL;
+
+        if (!mpu) {
+                dev_err(&slave_client->adapter->dev,
+                        "%s: Null mpu_private_data\n", __func__);
+                return -EINVAL;
+        }
+        mldl_cfg    = &mpu->mldl_cfg;
+        pdata_slave = mldl_cfg->pdata_slave;
+        slave_descr = get_slave_descr();
+
+        if (!slave_descr) {
+                dev_err(&slave_client->adapter->dev,
+                        "%s: Null ext_slave_descr\n", __func__);
+                return -EINVAL;
+        }
+
+        mutex_lock(&mpu->mutex);
+        if (mpu->pid) {
+                mutex_unlock(&mpu->mutex);
+                return -EBUSY;
+        }
+
+        if (pdata_slave[slave_descr->type]) {
+                result = -EBUSY;
+                goto out_unlock_mutex;
+        }
+
+        slave_pdata->address    = slave_client->addr;
+        slave_pdata->irq        = slave_client->irq;
+        slave_pdata->adapt_num  = i2c_adapter_id(slave_client->adapter);
+
+        dev_info(&slave_client->adapter->dev,
+                "%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n",
+                __func__,
+                slave_descr->name,
+                slave_descr->type,
+                slave_pdata->address,
+                slave_pdata->irq,
+                slave_pdata->adapt_num);
+
+        switch (slave_descr->type) {
+        case EXT_SLAVE_TYPE_ACCEL:
+                irq_name = "accelirq";
+                break;
+        case EXT_SLAVE_TYPE_COMPASS:
+                irq_name = "compassirq";
+                break;
+        case EXT_SLAVE_TYPE_PRESSURE:
+                irq_name = "pressureirq";
+                break;
+        default:
+                irq_name = "none";
+        };
+        if (slave_descr->init) {
+                result = slave_descr->init(slave_client->adapter,
+                                        slave_descr,
+                                        slave_pdata);
+                if (result) {
+                        dev_err(&slave_client->adapter->dev,
+                                "%s init failed %d\n",
+                                slave_descr->name, result);
+                        goto out_unlock_mutex;
+                }
+        }
+
+        pdata_slave[slave_descr->type] = slave_pdata;
+        mpu->slave_modules[slave_descr->type] = slave_module;
+        mldl_cfg->slave[slave_descr->type] = slave_descr;
+
+        goto out_unlock_mutex;
+
+out_unlock_mutex:
+        mutex_unlock(&mpu->mutex);
+
+        if (!result && irq_name && (slave_pdata->irq > 0)) {
+                int warn_result;
+                dev_info(&slave_client->adapter->dev,
+                        "Installing %s irq using %d\n",
+                        irq_name,
+                        slave_pdata->irq);
+                warn_result = slaveirq_init(slave_client->adapter,
+                                        slave_pdata, irq_name);
+                if (result)
+                        dev_WARN(&slave_client->adapter->dev,
+                                "%s irq assigned error: %d\n",
+                                slave_descr->name, warn_result);
+        } else {
+                dev_info(&slave_client->adapter->dev,
+                        "%s irq not assigned: %d %d %d\n",
+                        slave_descr->name,
+                        result, (int)irq_name, slave_pdata->irq);
+        }
+
+        return result;
+}
+EXPORT_SYMBOL(inv_mpu_register_slave);
+
+void inv_mpu_unregister_slave(struct i2c_client *slave_client,
+                        struct ext_slave_platform_data *slave_pdata,
+                        struct ext_slave_descr *(*get_slave_descr)(void))
+{
+        struct mpu_private_data *mpu = mpu_private_data;
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct ext_slave_descr *slave_descr;
+        int result;
+
+        dev_info(&slave_client->adapter->dev, "%s\n", __func__);
+
+        if (!slave_client || !slave_pdata || !get_slave_descr)
+                return;
+
+        if (slave_pdata->irq)
+                slaveirq_exit(slave_pdata);
+
+        slave_descr = get_slave_descr();
+        if (!slave_descr)
+                return;
+
+        mutex_lock(&mpu->mutex);
+
+        if (slave_descr->exit) {
+                result = slave_descr->exit(slave_client->adapter,
+                                        slave_descr,
+                                        slave_pdata);
+                if (result)
+                        dev_err(&slave_client->adapter->dev,
+                                "Accel exit failed %d\n", result);
+        }
+        mldl_cfg->slave[slave_descr->type] = NULL;
+        mldl_cfg->pdata_slave[slave_descr->type] = NULL;
+        mpu->slave_modules[slave_descr->type] = NULL;
+
+        mutex_unlock(&mpu->mutex);
+
+}
+EXPORT_SYMBOL(inv_mpu_unregister_slave);
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static const struct i2c_device_id mpu_id[] = {
+        {"mpu3050", 0},
+        {"mpu6050", 0},
+        {"mpu6050_no_accel", 0},
+        {}
+};
+MODULE_DEVICE_TABLE(i2c, mpu_id);
+
+int mpu_probe(struct i2c_client *client, const struct i2c_device_id *devid)
+{
+        struct mpu_platform_data *pdata;
+        struct mpu_private_data *mpu;
+        struct mldl_cfg *mldl_cfg;
+        int res = 0;
+        int ii = 0;
+        unsigned long irq_flags;
+
+        dev_info(&client->adapter->dev, "%s: %d\n", __func__, ii++);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                res = -ENODEV;
+                goto out_check_functionality_failed;
+        }
+
+        mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL);
+        if (!mpu) {
+                res = -ENOMEM;
+                goto out_alloc_data_failed;
+        }
+        mldl_cfg = &mpu->mldl_cfg;
+        mldl_cfg->mpu_ram       = &mpu->mpu_ram;
+        mldl_cfg->mpu_gyro_cfg  = &mpu->mpu_gyro_cfg;
+        mldl_cfg->mpu_offsets   = &mpu->mpu_offsets;
+        mldl_cfg->mpu_chip_info = &mpu->mpu_chip_info;
+        mldl_cfg->inv_mpu_cfg   = &mpu->inv_mpu_cfg;
+        mldl_cfg->inv_mpu_state = &mpu->inv_mpu_state;
+
+        mldl_cfg->mpu_ram->length = MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE;
+        mldl_cfg->mpu_ram->ram = kzalloc(mldl_cfg->mpu_ram->length, GFP_KERNEL);
+        if (!mldl_cfg->mpu_ram->ram) {
+                res = -ENOMEM;
+                goto out_alloc_ram_failed;
+        }
+        mpu_private_data = mpu;
+        i2c_set_clientdata(client, mpu);
+        mpu->client = client;
+
+        init_waitqueue_head(&mpu->mpu_event_wait);
+        mutex_init(&mpu->mutex);
+        init_completion(&mpu->completion);
+
+        mpu->response_timeout = 60;     /* Seconds */
+        mpu->timeout.function = mpu_pm_timeout;
+        mpu->timeout.data = (u_long) mpu;
+        init_timer(&mpu->timeout);
+
+        mpu->nb.notifier_call = mpu_pm_notifier_callback;
+        mpu->nb.priority = 0;
+        res = register_pm_notifier(&mpu->nb);
+        if (res) {
+                dev_err(&client->adapter->dev,
+                        "Unable to register pm_notifier %d\n", res);
+                goto out_register_pm_notifier_failed;
+        }
+
+        pdata = (struct mpu_platform_data *)client->dev.platform_data;
+        if (!pdata) {
+                dev_WARN(&client->adapter->dev,
+                         "Missing platform data for mpu\n");
+        }
+        mldl_cfg->pdata = pdata;
+
+        mldl_cfg->mpu_chip_info->addr = client->addr;
+        res = inv_mpu_open(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
+
+        if (res) {
+                dev_err(&client->adapter->dev,
+                        "Unable to open %s %d\n", MPU_NAME, res);
+                res = -ENODEV;
+                goto out_whoami_failed;
+        }
+
+        mpu->dev.minor = MISC_DYNAMIC_MINOR;
+        mpu->dev.name = "mpu";
+        mpu->dev.fops = &mpu_fops;
+        res = misc_register(&mpu->dev);
+        if (res < 0) {
+                dev_err(&client->adapter->dev,
+                        "ERROR: misc_register returned %d\n", res);
+                goto out_misc_register_failed;
+        }
+
+        if (client->irq) {
+                dev_info(&client->adapter->dev,
+                         "Installing irq using %d\n", client->irq);
+                if (BIT_ACTL_LOW == ((mldl_cfg->pdata->int_config) & BIT_ACTL))
+                        irq_flags = IRQF_TRIGGER_FALLING;
+                else
+                        irq_flags = IRQF_TRIGGER_RISING;
+                res = mpuirq_init(client, mldl_cfg, irq_flags);
+
+                if (res)
+                        goto out_mpuirq_failed;
+        } else {
+                dev_WARN(&client->adapter->dev,
+                         "Missing %s IRQ\n", MPU_NAME);
+        }
+        return res;
+
+out_mpuirq_failed:
+        misc_deregister(&mpu->dev);
+out_misc_register_failed:
+        inv_mpu_close(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
+out_whoami_failed:
+        unregister_pm_notifier(&mpu->nb);
+out_register_pm_notifier_failed:
+        kfree(mldl_cfg->mpu_ram->ram);
+        mpu_private_data = NULL;
+out_alloc_ram_failed:
+        kfree(mpu);
+out_alloc_data_failed:
+out_check_functionality_failed:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, res);
+        return res;
+
+}
+
+static int mpu_remove(struct i2c_client *client)
+{
+        struct mpu_private_data *mpu = i2c_get_clientdata(client);
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_close(mldl_cfg,
+                slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                slave_adapter[EXT_SLAVE_TYPE_PRESSURE]);
+
+
+        if (client->irq)
+                mpuirq_exit();
+
+        misc_deregister(&mpu->dev);
+
+        unregister_pm_notifier(&mpu->nb);
+
+        kfree(mpu->mldl_cfg.mpu_ram->ram);
+        kfree(mpu);
+
+        return 0;
+}
+
+static struct i2c_driver mpu_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = mpu_probe,
+        .remove = mpu_remove,
+        .id_table = mpu_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = MPU_NAME,
+                   },
+        .address_list = normal_i2c,
+        .shutdown = mpu_shutdown,       /* optional */
+        .suspend = mpu_dev_suspend,     /* optional */
+        .resume = mpu_dev_resume,       /* optional */
+
+};
+
+static int __init mpu_init(void)
+{
+        int res = i2c_add_driver(&mpu_driver);
+        pr_info("%s: Probe name %s\n", __func__, MPU_NAME);
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit mpu_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&mpu_driver);
+}
+
+module_init(mpu_init);
+module_exit(mpu_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("User space character device interface for MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS(MPU_NAME);
diff --git a/drivers/misc/inv_mpu/mpu6050/Makefile b/drivers/misc/inv_mpu/mpu6050/Makefile
new file mode 100644
index 00000000000..a93aa97a699
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/Makefile
@@ -0,0 +1,18 @@
+
+# Kernel makefile for motions sensors
+#
+#
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050B1) += mpu6050b1.o
+
+ccflags-y := -DMPU_CURRENT_BUILD_MPU6050B1
+
+mpu6050b1-objs += mldl_cfg.o
+mpu6050b1-objs += mldl_print_cfg.o
+mpu6050b1-objs += mlsl-kernel.o
+mpu6050b1-objs += mpu-dev.o
+mpu6050b1-objs += ../accel/mpu6050.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
+EXTRA_CFLAGS += -DINV_CACHE_DMP=1
diff --git a/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c
new file mode 100644
index 00000000000..22af0c20098
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c
@@ -0,0 +1,1916 @@
+/*
+        $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 "mpu6050b1.h"
+
+#include "mlsl.h"
+#include "mldl_print_cfg.h"
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_cfg:"
+
+/* -------------------------------------------------------------------------- */
+
+#define SLEEP   0
+#define WAKE_UP 7
+#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;
+}
+
+/**
+ *  @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 mpu6050b1_set_i2c_bypass(struct mldl_cfg *mldl_cfg,
+                                    void *mlsl_handle, unsigned char enable)
+{
+        unsigned char reg;
+        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, &reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        if (!enable) {
+                /* setting int_config with the property flag BIT_BYPASS_EN
+                   should be done by the setup functions */
+                result = inv_serial_single_write(
+                        mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                        MPUREG_INT_PIN_CFG,
+                        (mldl_cfg->pdata->int_config & ~(BIT_BYPASS_EN)));
+                if (!(reg & BIT_I2C_MST_EN)) {
+                        result =
+                            inv_serial_single_write(
+                                    mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                                    MPUREG_USER_CTRL,
+                                    (reg | BIT_I2C_MST_EN));
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                }
+        } else if (enable) {
+                if (reg & BIT_AUX_IF_EN) {
+                        result =
+                            inv_serial_single_write(
+                                    mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                                    MPUREG_USER_CTRL,
+                                    (reg & (~BIT_I2C_MST_EN)));
+                        if (result) {
+                                LOG_RESULT_LOCATION(result);
+                                return result;
+                        }
+                        /*****************************************
+                         * To avoid hanging the bus we must sleep until all
+                         * slave transactions have been completed.
+                         *  24 bytes max slave reads
+                         *  +1 byte possible extra write
+                         *  +4 max slave address
+                         * ---
+                         *  33 Maximum bytes
+                         *  x9 Approximate bits per byte
+                         * ---
+                         * 297 bits.
+                         * 2.97 ms minimum @ 100kbps
+                         * 0.75 ms minimum @ 400kbps.
+                         *****************************************/
+                        msleep(3);
+                }
+                result = inv_serial_single_write(
+                        mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                        MPUREG_INT_PIN_CFG,
+                        (mldl_cfg->pdata->int_config | BIT_BYPASS_EN));
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+        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 mpu6050b1_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 */
+
+/* produces an unique identifier for each device based on the
+   combination of product version and product revision */
+struct prod_rev_map_t {
+        unsigned short mpl_product_key;
+        unsigned char silicon_rev;
+        unsigned short gyro_trim;
+        unsigned short accel_trim;
+};
+
+/* NOTE: product entries are in chronological order */
+static struct prod_rev_map_t prod_rev_map[] = {
+        /* prod_ver = 0 */
+        {MPL_PROD_KEY(0,  1), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0,  2), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0,  3), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0,  4), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0,  5), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0,  6), MPU_SILICON_REV_A2, 131, 16384},  /* (A2/C2-1) */
+        /* prod_ver = 1, forced to 0 for MPU6050 A2 */
+        {MPL_PROD_KEY(0,  7), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0,  8), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0,  9), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0, 10), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0, 11), MPU_SILICON_REV_A2, 131, 16384},  /* (A2/D2-1) */
+        {MPL_PROD_KEY(0, 12), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0, 13), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0, 14), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0, 15), MPU_SILICON_REV_A2, 131, 16384},
+        {MPL_PROD_KEY(0, 27), MPU_SILICON_REV_A2, 131, 16384},  /* (A2/D4)   */
+        /* prod_ver = 1 */
+        {MPL_PROD_KEY(1, 16), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/D2-1) */
+        {MPL_PROD_KEY(1, 17), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/D2-2) */
+        {MPL_PROD_KEY(1, 18), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/D2-3) */
+        {MPL_PROD_KEY(1, 19), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/D2-4) */
+        {MPL_PROD_KEY(1, 20), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/D2-5) */
+        {MPL_PROD_KEY(1, 28), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/D4)   */
+        {MPL_PROD_KEY(1,  1), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/E1-1) */
+        {MPL_PROD_KEY(1,  2), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/E1-2) */
+        {MPL_PROD_KEY(1,  3), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/E1-3) */
+        {MPL_PROD_KEY(1,  4), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/E1-4) */
+        {MPL_PROD_KEY(1,  5), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/E1-5) */
+        {MPL_PROD_KEY(1,  6), MPU_SILICON_REV_B1, 131, 16384},  /* (B1/E1-6) */
+        /* prod_ver = 2 */
+        {MPL_PROD_KEY(2,  7), MPU_SILICON_REV_B1, 131, 16384},  /* (B2/E1-1) */
+        {MPL_PROD_KEY(2,  8), MPU_SILICON_REV_B1, 131, 16384},  /* (B2/E1-2) */
+        {MPL_PROD_KEY(2,  9), MPU_SILICON_REV_B1, 131, 16384},  /* (B2/E1-3) */
+        {MPL_PROD_KEY(2, 10), MPU_SILICON_REV_B1, 131, 16384},  /* (B2/E1-4) */
+        {MPL_PROD_KEY(2, 11), MPU_SILICON_REV_B1, 131, 16384},  /* (B2/E1-5) */
+        {MPL_PROD_KEY(2, 12), MPU_SILICON_REV_B1, 131, 16384},  /* (B2/E1-6) */
+        {MPL_PROD_KEY(2, 29), MPU_SILICON_REV_B1, 131, 16384},  /* (B2/D4)   */
+        /* prod_ver = 3 */
+        {MPL_PROD_KEY(3, 30), MPU_SILICON_REV_B1, 131, 16384},  /* (B2/E2)   */
+        /* prod_ver = 4 */
+        {MPL_PROD_KEY(4, 31), MPU_SILICON_REV_B1, 131,  8192},  /* (B2/F1)   */
+        {MPL_PROD_KEY(4,  1), MPU_SILICON_REV_B1, 131,  8192},  /* (B3/F1)   */
+        {MPL_PROD_KEY(4,  3), MPU_SILICON_REV_B1, 131,  8192},  /* (B4/F1)   */
+        /* prod_ver = 5 */
+        {MPL_PROD_KEY(6, 19), MPU_SILICON_REV_B1, 131, 16384},  /* (B5/E2)   */
+        /* prod_ver = 7 */
+        {MPL_PROD_KEY(7, 19), MPU_SILICON_REV_B1, 131, 16384},  /* (B5/E2)   */
+        /* prod_ver = 8 */
+        {MPL_PROD_KEY(8, 19), MPU_SILICON_REV_B1, 131, 16384},  /* (B5/E2)   */
+        /* prod_ver = 9 */
+        {MPL_PROD_KEY(9, 19), MPU_SILICON_REV_B1, 131, 16384},  /* (B5/E2)   */
+        /* prod_ver = 10 */
+        {MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384}  /* (B5/E2)   */
+};
+
+/**
+ *  @internal
+ *  @brief  Inverse lookup of the index of an MPL product key .
+ *  @param  key
+ *              the MPL product indentifier also referred to as 'key'.
+ *  @return the index position of the key in the array, -1 if not found.
+ */
+short index_of_key(unsigned short key)
+{
+        int i;
+        for (i = 0; i < NUM_OF_PROD_REVS; i++)
+                if (prod_rev_map[i].mpl_product_key == key)
+                        return (short)i;
+        return -1;
+}
+
+/**
+ *  @internal
+ *  @brief  Get the product revision and version for MPU6050 and
+ *          extract all per-part specific information.
+ *          The product version number is read from the PRODUCT_ID register in
+ *          user space register map.
+ *          The product revision number is in read from OTP bank 0, ADDR6[7:2].
+ *          These 2 numbers, combined, provide an unique key to be used to
+ *          retrieve some per-device information such as the silicon revision
+ *          and the gyro and accel sensitivity trim values.
+ *
+ *  @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_mpu6050(
+                struct mldl_cfg *mldl_cfg, void *mlsl_handle)
+{
+        int result;
+        unsigned char prod_ver = 0x00, prod_rev = 0x00;
+        unsigned char bank =
+            (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
+        unsigned short memAddr = ((bank << 8) | 0x06);
+        unsigned short key;
+        short index;
+        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, &prod_ver);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        prod_ver &= 0xF;
+
+        result = inv_serial_read_mem(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                                     memAddr, 1, &prod_rev);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        prod_rev >>= 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;
+        }
+
+        key = MPL_PROD_KEY(prod_ver, prod_rev);
+        if (key == 0) {
+                MPL_LOGE("Product id read as 0 "
+                         "indicates device is either "
+                         "incompatible or an MPU3050\n");
+                return INV_ERROR_INVALID_MODULE;
+        }
+        index = index_of_key(key);
+        if (index == -1 || index >= NUM_OF_PROD_REVS) {
+                MPL_LOGE("Unsupported product key %d in MPL\n", key);
+                return INV_ERROR_INVALID_MODULE;
+        }
+        /* check MPL is compiled for this device */
+        if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1) {
+                MPL_LOGE("MPL compiled for MPU6050B1 support "
+                         "but device is not MPU6050B1 (%d)\n", key);
+                return INV_ERROR_INVALID_MODULE;
+        }
+
+        mpu_chip_info->product_id = prod_ver;
+        mpu_chip_info->product_revision = prod_rev;
+        mpu_chip_info->silicon_revision = prod_rev_map[index].silicon_rev;
+        mpu_chip_info->gyro_sens_trim = prod_rev_map[index].gyro_trim;
+        mpu_chip_info->accel_sens_trim = prod_rev_map[index].accel_trim;
+
+        return result;
+}
+#define inv_get_silicon_rev inv_get_silicon_rev_mpu6050
+
+
+/**
+ *  @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 regval;
+
+        result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                                 MPUREG_YG_OFFS_TC, 1, &regval);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        if (enable)
+                regval |= BIT_I2C_MST_VDDIO;
+
+        result = inv_serial_single_write(
+                mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                MPUREG_YG_OFFS_TC, regval);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        return INV_SUCCESS;
+}
+
+
+/**
+ * @internal
+ * @brief MPU6050 B1 power management functions.
+ * @param mldl_cfg
+ *          a pointer to the internal mldl_cfg data structure.
+ * @param mlsl_handle
+ *          a file handle to the serial device used to communicate
+ *          with the MPU6050 B1 device.
+ * @param reset
+ *          1 to reset hardware.
+ * @param sensors
+ *          Bitfield of sensors to leave on
+ *
+ * @return 0 on success, a non-zero error code on error.
+ */
+static int mpu60xx_pwr_mgmt(struct mldl_cfg *mldl_cfg,
+                                    void *mlsl_handle,
+                                    unsigned int reset, unsigned long sensors)
+{
+        unsigned char pwr_mgmt[2];
+        unsigned char pwr_mgmt_prev[2];
+        int result;
+        int sleep = !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
+                                | INV_DMP_PROCESSOR));
+
+        if (reset) {
+                MPL_LOGI("Reset MPU6050 B1\n");
+                result = inv_serial_single_write(
+                        mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                        MPUREG_PWR_MGMT_1, BIT_H_RESET);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
+                msleep(15);
+        }
+
+        /* NOTE : reading both PWR_MGMT_1 and PWR_MGMT_2 for efficiency because
+                  they are accessible even when the device is powered off */
+        result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                                 MPUREG_PWR_MGMT_1, 2, pwr_mgmt_prev);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        pwr_mgmt[0] = pwr_mgmt_prev[0];
+        pwr_mgmt[1] = pwr_mgmt_prev[1];
+
+        if (sleep) {
+                mldl_cfg->inv_mpu_state->status |= MPU_DEVICE_IS_SUSPENDED;
+                pwr_mgmt[0] |= BIT_SLEEP;
+        } else {
+                mldl_cfg->inv_mpu_state->status &= ~MPU_DEVICE_IS_SUSPENDED;
+                pwr_mgmt[0] &= ~BIT_SLEEP;
+        }
+        if (pwr_mgmt[0] != pwr_mgmt_prev[0]) {
+                result = inv_serial_single_write(
+                        mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                        MPUREG_PWR_MGMT_1, pwr_mgmt[0]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        pwr_mgmt[1] &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
+        if (!(sensors & INV_X_GYRO))
+                pwr_mgmt[1] |= BIT_STBY_XG;
+        if (!(sensors & INV_Y_GYRO))
+                pwr_mgmt[1] |= BIT_STBY_YG;
+        if (!(sensors & INV_Z_GYRO))
+                pwr_mgmt[1] |= BIT_STBY_ZG;
+
+        if (pwr_mgmt[1] != pwr_mgmt_prev[1]) {
+                result = inv_serial_single_write(
+                        mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+                        MPUREG_PWR_MGMT_2, pwr_mgmt[1]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        if ((pwr_mgmt[1] & (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
+            (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) {
+                mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_SUSPENDED;
+        } else {
+                mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED;
+        }
+
+        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;
+        }
+
+        /* ERRATA:
+           workaroud to switch from any MPU_CLK_SEL_PLLGYROx to
+           MPU_CLK_SEL_INTERNAL and XGyro is powered up:
+           1) Select INT_OSC
+           2) PD XGyro
+           3) PU XGyro
+         */
+        if ((cur_clk_src == MPU_CLK_SEL_PLLGYROX
+                 || cur_clk_src == MPU_CLK_SEL_PLLGYROY
+                 || cur_clk_src == MPU_CLK_SEL_PLLGYROZ)
+            && mldl_cfg->mpu_gyro_cfg->clk_src == MPU_CLK_SEL_INTERNAL
+            && mldl_cfg->inv_mpu_cfg->requested_sensors & INV_X_GYRO) {
+                unsigned char first_result = INV_SUCCESS;
+                mldl_cfg->inv_mpu_cfg->requested_sensors &= ~INV_X_GYRO;
+                result = mpu60xx_pwr_mgmt(
+                        mldl_cfg, gyro_handle,
+                        false, mldl_cfg->inv_mpu_cfg->requested_sensors);
+                ERROR_CHECK_FIRST(first_result, result);
+                mldl_cfg->inv_mpu_cfg->requested_sensors |= INV_X_GYRO;
+                result = mpu60xx_pwr_mgmt(
+                        mldl_cfg, gyro_handle,
+                        false, mldl_cfg->inv_mpu_cfg->requested_sensors);
+                ERROR_CHECK_FIRST(first_result, result);
+                result = first_result;
+        }
+        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_mpu60xx(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;
+        /* Slave values */
+        unsigned char slave_reg;
+        unsigned char slave_len;
+        unsigned char slave_endian;
+        unsigned char slave_address;
+        /* Which MPU6050 registers to use */
+        unsigned char addr_reg;
+        unsigned char reg_reg;
+        unsigned char ctrl_reg;
+        /* Which MPU6050 registers to use for the trigger */
+        unsigned char addr_trig_reg;
+        unsigned char reg_trig_reg;
+        unsigned char ctrl_trig_reg;
+
+        unsigned char bits_slave_delay = 0;
+        /* Divide down rate for the Slave, from the mpu rate */
+        unsigned char d0_trig_reg;
+        unsigned char delay_ctrl_orig;
+        unsigned char delay_ctrl;
+        long divider;
+
+        if (NULL == slave || NULL == slave_pdata) {
+                slave_reg = 0;
+                slave_len = 0;
+                slave_endian = 0;
+                slave_address = 0;
+        } else {
+                slave_reg = slave->read_reg;
+                slave_len = slave->read_len;
+                slave_endian = slave->endian;
+                slave_address = slave_pdata->address;
+                slave_address |= BIT_I2C_READ;
+        }
+
+        switch (slave_id) {
+        case EXT_SLAVE_TYPE_ACCEL:
+                addr_reg = MPUREG_I2C_SLV1_ADDR;
+                reg_reg  = MPUREG_I2C_SLV1_REG;
+                ctrl_reg = MPUREG_I2C_SLV1_CTRL;
+                addr_trig_reg = 0;
+                reg_trig_reg  = 0;
+                ctrl_trig_reg = 0;
+                bits_slave_delay = BIT_SLV1_DLY_EN;
+                break;
+        case EXT_SLAVE_TYPE_COMPASS:
+                addr_reg = MPUREG_I2C_SLV0_ADDR;
+                reg_reg  = MPUREG_I2C_SLV0_REG;
+                ctrl_reg = MPUREG_I2C_SLV0_CTRL;
+                addr_trig_reg = MPUREG_I2C_SLV2_ADDR;
+                reg_trig_reg  = MPUREG_I2C_SLV2_REG;
+                ctrl_trig_reg = MPUREG_I2C_SLV2_CTRL;
+                d0_trig_reg   = MPUREG_I2C_SLV2_DO;
+                bits_slave_delay = BIT_SLV2_DLY_EN | BIT_SLV0_DLY_EN;
+                break;
+        case EXT_SLAVE_TYPE_PRESSURE:
+                addr_reg = MPUREG_I2C_SLV3_ADDR;
+                reg_reg  = MPUREG_I2C_SLV3_REG;
+                ctrl_reg = MPUREG_I2C_SLV3_CTRL;
+                addr_trig_reg = MPUREG_I2C_SLV4_ADDR;
+                reg_trig_reg  = MPUREG_I2C_SLV4_REG;
+                ctrl_trig_reg = MPUREG_I2C_SLV4_CTRL;
+                bits_slave_delay = BIT_SLV4_DLY_EN | BIT_SLV3_DLY_EN;
+                break;
+        default:
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        };
+
+        /* return if this slave has already been set */
+        if ((slave_address &&
+             ((mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay)
+                    == bits_slave_delay)) ||
+            (!slave_address &&
+             (mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay) ==
+                    0))
+                return 0;
+
+        result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
+
+        /* Address */
+        result = inv_serial_single_write(gyro_handle,
+                                         mldl_cfg->mpu_chip_info->addr,
+                                         addr_reg, slave_address);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Register */
+        result = inv_serial_single_write(gyro_handle,
+                                         mldl_cfg->mpu_chip_info->addr,
+                                         reg_reg, slave_reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* 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;
+                return INV_ERROR_INVALID_CONFIGURATION;
+        }
+        reg = slave_len;
+        if (slave_endian == EXT_SLAVE_LITTLE_ENDIAN) {
+                reg |= BIT_SLV_BYTE_SW;
+                if (slave_reg & 1)
+                        reg |= BIT_SLV_GRP;
+        }
+        if (slave_address)
+                reg |= BIT_SLV_ENABLE;
+
+        result = inv_serial_single_write(gyro_handle,
+                                         mldl_cfg->mpu_chip_info->addr,
+                                         ctrl_reg, reg);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Trigger */
+        if (addr_trig_reg) {
+                /* If slave address is 0 this clears the trigger */
+                result = inv_serial_single_write(gyro_handle,
+                                                 mldl_cfg->mpu_chip_info->addr,
+                                                 addr_trig_reg,
+                                                 slave_address & ~BIT_I2C_READ);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        if (slave && slave->trigger && reg_trig_reg) {
+                result = inv_serial_single_write(gyro_handle,
+                                                 mldl_cfg->mpu_chip_info->addr,
+                                                 reg_trig_reg,
+                                                 slave->trigger->reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(gyro_handle,
+                                                 mldl_cfg->mpu_chip_info->addr,
+                                                 ctrl_trig_reg,
+                                                 BIT_SLV_ENABLE | 0x01);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                result = inv_serial_single_write(gyro_handle,
+                                                 mldl_cfg->mpu_chip_info->addr,
+                                                 d0_trig_reg,
+                                                 slave->trigger->value);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        } else if (ctrl_trig_reg) {
+                result = inv_serial_single_write(gyro_handle,
+                                                 mldl_cfg->mpu_chip_info->addr,
+                                                 ctrl_trig_reg, 0x00);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        /* Data rate */
+        if (slave) {
+                struct ext_slave_config config;
+                long data;
+                config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
+                config.len = sizeof(long);
+                config.apply = false;
+                config.data = &data;
+                if (!(slave->get_config))
+                        return INV_ERROR_INVALID_CONFIGURATION;
+
+                result = slave->get_config(NULL, slave, slave_pdata, &config);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                MPL_LOGI("Slave %d ODR: %ld Hz\n", slave_id, data / 1000);
+                divider = ((1000 * inv_mpu_get_sampling_rate_hz(
+                                    mldl_cfg->mpu_gyro_cfg))
+                        / data) - 1;
+        } else {
+                divider = 0;
+        }
+
+        result = inv_serial_read(gyro_handle,
+                                mldl_cfg->mpu_chip_info->addr,
+                                MPUREG_I2C_MST_DELAY_CTRL,
+                                1, &delay_ctrl_orig);
+        delay_ctrl = delay_ctrl_orig;
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        if (divider > 0 && divider <= MASK_I2C_MST_DLY) {
+                result = inv_serial_read(gyro_handle,
+                                         mldl_cfg->mpu_chip_info->addr,
+                                         MPUREG_I2C_SLV4_CTRL, 1, &reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                if ((reg & MASK_I2C_MST_DLY) &&
+                        ((long)(reg & MASK_I2C_MST_DLY) !=
+                                (divider & MASK_I2C_MST_DLY))) {
+                        MPL_LOGW("Changing slave divider: %ld to %ld\n",
+                                 (long)(reg & MASK_I2C_MST_DLY),
+                                 (divider & MASK_I2C_MST_DLY));
+
+                }
+                reg |= (unsigned char)(divider & MASK_I2C_MST_DLY);
+                result = inv_serial_single_write(gyro_handle,
+                                                 mldl_cfg->mpu_chip_info->addr,
+                                                 MPUREG_I2C_SLV4_CTRL,
+                                                 reg);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+
+                delay_ctrl |= bits_slave_delay;
+        } else {
+                delay_ctrl &= ~(bits_slave_delay);
+        }
+        if (delay_ctrl != delay_ctrl_orig) {
+                result = inv_serial_single_write(
+                        gyro_handle, mldl_cfg->mpu_chip_info->addr,
+                        MPUREG_I2C_MST_DELAY_CTRL,
+                        delay_ctrl);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+        }
+
+        if (slave_address)
+                mldl_cfg->inv_mpu_state->i2c_slaves_enabled |=
+                        bits_slave_delay;
+        else
+                mldl_cfg->inv_mpu_state->i2c_slaves_enabled &=
+                        ~bits_slave_delay;
+
+        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_mpu60xx(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;
+
+        /* Inconclusive assume it was reset */
+        return true;
+}
+
+
+int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+                         const unsigned char *data, int size)
+{
+        int bank, offset, write_size;
+        int result;
+        unsigned char read[MPU_MEM_BANK_SIZE];
+
+        if (mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED) {
+#if INV_CACHE_DMP == 1
+                memcpy(mldl_cfg->mpu_ram->ram, data, size);
+                return INV_SUCCESS;
+#else
+                LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
+                return INV_ERROR_MEMORY_SET;
+#endif
+        }
+
+        if (!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)) {
+                LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
+                return INV_ERROR_MEMORY_SET;
+        }
+        /* Write and verify memory */
+        for (bank = 0; size > 0; bank++,
+                        size -= write_size,
+                        data += write_size) {
+                if (size > MPU_MEM_BANK_SIZE)
+                        write_size = MPU_MEM_BANK_SIZE;
+                else
+                        write_size = size;
+
+                result = inv_serial_write_mem(mlsl_handle,
+                                mldl_cfg->mpu_chip_info->addr,
+                                ((bank << 8) | 0x00),
+                                write_size,
+                                data);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        MPL_LOGE("Write mem error in bank %d\n", bank);
+                        return result;
+                }
+                result = inv_serial_read_mem(mlsl_handle,
+                                mldl_cfg->mpu_chip_info->addr,
+                                ((bank << 8) | 0x00),
+                                write_size,
+                                read);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        MPL_LOGE("Read mem error in bank %d\n", bank);
+                        return result;
+                }
+
+#define ML_SKIP_CHECK 38
+                for (offset = 0; offset < write_size; offset++) {
+                        /* skip the register memory locations */
+                        if (bank == 0 && offset < ML_SKIP_CHECK)
+                                continue;
+                        if (data[offset] != read[offset]) {
+                                result = INV_ERROR_SERIAL_WRITE;
+                                break;
+                        }
+                }
+                if (result != INV_SUCCESS) {
+                        LOG_RESULT_LOCATION(result);
+                        MPL_LOGE("Read data mismatch at bank %d, offset %d\n",
+                                bank, offset);
+                        return result;
+                }
+        }
+        return INV_SUCCESS;
+}
+
+static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle,
+                       unsigned long sensors)
+{
+        int result;
+        int ii;
+        unsigned char reg;
+        unsigned char regs[7];
+
+        /* Wake up the part */
+        result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, sensors);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Always set the INT_ENABLE and DIVIDER as the Accel Only mode for 6050
+           can set these too */
+        result = inv_serial_single_write(
+                gyro_handle, mldl_cfg->mpu_chip_info->addr,
+                MPUREG_INT_ENABLE, (mldl_cfg->mpu_gyro_cfg->int_config));
+        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;
+        }
+
+        if (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG) &&
+            !mpu_was_reset(mldl_cfg, gyro_handle)) {
+                return INV_SUCCESS;
+        }
+
+        /* Configure the MPU */
+        result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+        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;
+        }
+
+        reg = MPUREG_GYRO_CONFIG_VALUE(0, 0, 0,
+                                       mldl_cfg->mpu_gyro_cfg->full_scale);
+        result = inv_serial_single_write(
+                gyro_handle, mldl_cfg->mpu_chip_info->addr,
+                MPUREG_GYRO_CONFIG, reg);
+        reg = MPUREG_CONFIG_VALUE(mldl_cfg->mpu_gyro_cfg->ext_sync,
+                                  mldl_cfg->mpu_gyro_cfg->lpf);
+        result = inv_serial_single_write(
+                gyro_handle, mldl_cfg->mpu_chip_info->addr,
+                MPUREG_CONFIG, 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 */
+#if INV_CACHE_DMP != 0
+        inv_mpu_set_firmware(mldl_cfg, gyro_handle,
+                mldl_cfg->mpu_ram->ram, mldl_cfg->mpu_ram->length);
+#endif
+
+        result = inv_serial_single_write(
+                gyro_handle, mldl_cfg->mpu_chip_info->addr,
+                MPUREG_XG_OFFS_TC,
+                ((mldl_cfg->mpu_offsets->tc[0] << 1) & BITS_XG_OFFS_TC));
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        regs[0] = ((mldl_cfg->mpu_offsets->tc[1] << 1) & BITS_YG_OFFS_TC);
+        result = inv_serial_single_write(
+                gyro_handle, mldl_cfg->mpu_chip_info->addr,
+                MPUREG_YG_OFFS_TC, regs[0]);
+        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] << 1) & BITS_ZG_OFFS_TC));
+        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;
+        }
+        mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_CONFIG;
+
+        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_CONFIG;
+        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 *gyro_handle,
+                 void *accel_handle,
+                 void *compass_handle, void *pressure_handle)
+{
+        int result;
+        void *slave_handle[EXT_SLAVE_NUM_TYPES];
+        int ii;
+
+        /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
+        ii = 0;
+        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 |
+                MPU_DEVICE_IS_SUSPENDED;
+        mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
+
+        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;
+
+        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 = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, true,
+                                  INV_THREE_AXIS_GYRO);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        result = inv_get_silicon_rev(mldl_cfg, gyro_handle);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Get the factory temperature compensation offsets */
+        result = inv_serial_read(gyro_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(gyro_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(gyro_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, gyro_handle, true);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
+                        mldl_cfg->pdata->level_shifter);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        for (ii = 0; ii < GYRO_NUM_AXES; ii++) {
+                mldl_cfg->mpu_offsets->tc[ii] =
+                    (mldl_cfg->mpu_offsets->tc[ii] & BITS_XG_OFFS_TC) >> 1;
+        }
+
+#if INV_CACHE_DMP != 0
+        result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, 0);
+#endif
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+
+        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 *gyro_handle,
+                  void *accel_handle,
+                  void *compass_handle,
+                  void *pressure_handle)
+{
+        return 0;
+}
+
+/**
+ *  @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) ||
+                (mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG))) {
+                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;
+                }
+                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];
+        void *slave_handle[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);
+
+        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;
+
+        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 = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false,
+                                        ((~sensors) & INV_ALL_SENSORS));
+                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(slave_handle[ii],
+                                                  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;
+}
+
+/**
+ * @}
+ */
diff --git a/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c b/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c
new file mode 100644
index 00000000000..7379a170761
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c
@@ -0,0 +1,138 @@
+/*
+        $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_print_cfg.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+#include <stddef.h>
+#include "mldl_cfg.h"
+#include "mlsl.h"
+#include "linux/mpu.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_print_cfg:"
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+void mldl_print_cfg(struct mldl_cfg *mldl_cfg)
+{
+        struct mpu_gyro_cfg     *mpu_gyro_cfg   = mldl_cfg->mpu_gyro_cfg;
+        struct mpu_offsets      *mpu_offsets    = mldl_cfg->mpu_offsets;
+        struct mpu_chip_info    *mpu_chip_info  = mldl_cfg->mpu_chip_info;
+        struct inv_mpu_cfg      *inv_mpu_cfg    = mldl_cfg->inv_mpu_cfg;
+        struct inv_mpu_state    *inv_mpu_state  = mldl_cfg->inv_mpu_state;
+        struct ext_slave_descr  **slave         = mldl_cfg->slave;
+        struct mpu_platform_data *pdata         = mldl_cfg->pdata;
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        /* mpu_gyro_cfg */
+        MPL_LOGV("int_config     = %02x\n", mpu_gyro_cfg->int_config);
+        MPL_LOGV("ext_sync       = %02x\n", mpu_gyro_cfg->ext_sync);
+        MPL_LOGV("full_scale     = %02x\n", mpu_gyro_cfg->full_scale);
+        MPL_LOGV("lpf            = %02x\n", mpu_gyro_cfg->lpf);
+        MPL_LOGV("clk_src        = %02x\n", mpu_gyro_cfg->clk_src);
+        MPL_LOGV("divider        = %02x\n", mpu_gyro_cfg->divider);
+        MPL_LOGV("dmp_enable     = %02x\n", mpu_gyro_cfg->dmp_enable);
+        MPL_LOGV("fifo_enable    = %02x\n", mpu_gyro_cfg->fifo_enable);
+        MPL_LOGV("dmp_cfg1       = %02x\n", mpu_gyro_cfg->dmp_cfg1);
+        MPL_LOGV("dmp_cfg2       = %02x\n", mpu_gyro_cfg->dmp_cfg2);
+        /* mpu_offsets */
+        MPL_LOGV("tc[0]      = %02x\n", mpu_offsets->tc[0]);
+        MPL_LOGV("tc[1]      = %02x\n", mpu_offsets->tc[1]);
+        MPL_LOGV("tc[2]      = %02x\n", mpu_offsets->tc[2]);
+        MPL_LOGV("gyro[0]    = %04x\n", mpu_offsets->gyro[0]);
+        MPL_LOGV("gyro[1]    = %04x\n", mpu_offsets->gyro[1]);
+        MPL_LOGV("gyro[2]    = %04x\n", mpu_offsets->gyro[2]);
+
+        /* mpu_chip_info */
+        MPL_LOGV("addr            = %02x\n", mldl_cfg->mpu_chip_info->addr);
+
+        MPL_LOGV("silicon_revision = %02x\n", mpu_chip_info->silicon_revision);
+        MPL_LOGV("product_revision = %02x\n", mpu_chip_info->product_revision);
+        MPL_LOGV("product_id       = %02x\n", mpu_chip_info->product_id);
+        MPL_LOGV("gyro_sens_trim   = %02x\n", mpu_chip_info->gyro_sens_trim);
+        MPL_LOGV("accel_sens_trim  = %02x\n", mpu_chip_info->accel_sens_trim);
+
+        MPL_LOGV("requested_sensors = %04x\n", inv_mpu_cfg->requested_sensors);
+        MPL_LOGV("ignore_system_suspend= %04x\n",
+                inv_mpu_cfg->ignore_system_suspend);
+        MPL_LOGV("status = %04x\n", inv_mpu_state->status);
+        MPL_LOGV("i2c_slaves_enabled= %04x\n",
+                inv_mpu_state->i2c_slaves_enabled);
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!slave[ii])
+                        continue;
+                MPL_LOGV("SLAVE %d:\n", ii);
+                MPL_LOGV("    suspend  = %02x\n", (int)slave[ii]->suspend);
+                MPL_LOGV("    resume   = %02x\n", (int)slave[ii]->resume);
+                MPL_LOGV("    read     = %02x\n", (int)slave[ii]->read);
+                MPL_LOGV("    type     = %02x\n", slave[ii]->type);
+                MPL_LOGV("    reg      = %02x\n", slave[ii]->read_reg);
+                MPL_LOGV("    len      = %02x\n", slave[ii]->read_len);
+                MPL_LOGV("    endian   = %02x\n", slave[ii]->endian);
+                MPL_LOGV("    range.mantissa= %02x\n",
+                        slave[ii]->range.mantissa);
+                MPL_LOGV("    range.fraction= %02x\n",
+                        slave[ii]->range.fraction);
+        }
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        continue;
+                MPL_LOGV("PDATA_SLAVE[%d]\n", ii);
+                MPL_LOGV("    irq        = %02x\n", pdata_slave[ii]->irq);
+                MPL_LOGV("    adapt_num  = %02x\n", pdata_slave[ii]->adapt_num);
+                MPL_LOGV("    bus        = %02x\n", pdata_slave[ii]->bus);
+                MPL_LOGV("    address    = %02x\n", pdata_slave[ii]->address);
+                MPL_LOGV("    orientation=\n"
+                        "                            %2d %2d %2d\n"
+                        "                            %2d %2d %2d\n"
+                        "                            %2d %2d %2d\n",
+                        pdata_slave[ii]->orientation[0],
+                        pdata_slave[ii]->orientation[1],
+                        pdata_slave[ii]->orientation[2],
+                        pdata_slave[ii]->orientation[3],
+                        pdata_slave[ii]->orientation[4],
+                        pdata_slave[ii]->orientation[5],
+                        pdata_slave[ii]->orientation[6],
+                        pdata_slave[ii]->orientation[7],
+                        pdata_slave[ii]->orientation[8]);
+        }
+
+        MPL_LOGV("pdata->int_config         = %02x\n", pdata->int_config);
+        MPL_LOGV("pdata->level_shifter      = %02x\n", pdata->level_shifter);
+        MPL_LOGV("pdata->orientation        =\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n",
+                 pdata->orientation[0], pdata->orientation[1],
+                 pdata->orientation[2], pdata->orientation[3],
+                 pdata->orientation[4], pdata->orientation[5],
+                 pdata->orientation[6], pdata->orientation[7],
+                 pdata->orientation[8]);
+}
diff --git a/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c b/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c
new file mode 100644
index 00000000000..f1c228f48fe
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c
@@ -0,0 +1,420 @@
+/*
+        $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/>.
+        $
+ */
+
+#include "mlsl.h"
+#include <linux/i2c.h>
+#include "log.h"
+#include "mpu6050b1.h"
+
+static int inv_i2c_write(struct i2c_adapter *i2c_adap,
+                            unsigned char address,
+                            unsigned int len, unsigned char const *data)
+{
+        struct i2c_msg msgs[1];
+        int res;
+
+        if (!data || !i2c_adap) {
+                LOG_RESULT_LOCATION(-EINVAL);
+                return -EINVAL;
+        }
+
+        msgs[0].addr = address;
+        msgs[0].flags = 0;      /* write */
+        msgs[0].buf = (unsigned char *)data;
+        msgs[0].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 1);
+        if (res < 1) {
+                if (res == 0)
+                        res = -EIO;
+                LOG_RESULT_LOCATION(res);
+                return res;
+        } else
+                return 0;
+}
+
+static int inv_i2c_write_register(struct i2c_adapter *i2c_adap,
+                                     unsigned char address,
+                                     unsigned char reg, unsigned char value)
+{
+        unsigned char data[2];
+
+        data[0] = reg;
+        data[1] = value;
+        return inv_i2c_write(i2c_adap, address, 2, data);
+}
+
+static int inv_i2c_read(struct i2c_adapter *i2c_adap,
+                           unsigned char address, unsigned char reg,
+                           unsigned int len, unsigned char *data)
+{
+        struct i2c_msg msgs[2];
+        int res;
+
+        if (!data || !i2c_adap) {
+                LOG_RESULT_LOCATION(-EINVAL);
+                return -EINVAL;
+        }
+
+        msgs[0].addr = address;
+        msgs[0].flags = 0;      /* write */
+        msgs[0].buf = &reg;
+        msgs[0].len = 1;
+
+        msgs[1].addr = address;
+        msgs[1].flags = I2C_M_RD;
+        msgs[1].buf = data;
+        msgs[1].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 2);
+        if (res < 2) {
+                if (res >= 0)
+                        res = -EIO;
+                LOG_RESULT_LOCATION(res);
+                return res;
+        } else
+                return 0;
+}
+
+static int mpu_memory_read(struct i2c_adapter *i2c_adap,
+                           unsigned char mpu_addr,
+                           unsigned short mem_addr,
+                           unsigned int len, unsigned char *data)
+{
+        unsigned char bank[2];
+        unsigned char addr[2];
+        unsigned char buf;
+
+        struct i2c_msg msgs[4];
+        int res;
+
+        if (!data || !i2c_adap) {
+                LOG_RESULT_LOCATION(-EINVAL);
+                return -EINVAL;
+        }
+
+        bank[0] = MPUREG_BANK_SEL;
+        bank[1] = mem_addr >> 8;
+
+        addr[0] = MPUREG_MEM_START_ADDR;
+        addr[1] = mem_addr & 0xFF;
+
+        buf = MPUREG_MEM_R_W;
+
+        /* write message */
+        msgs[0].addr = mpu_addr;
+        msgs[0].flags = 0;
+        msgs[0].buf = bank;
+        msgs[0].len = sizeof(bank);
+
+        msgs[1].addr = mpu_addr;
+        msgs[1].flags = 0;
+        msgs[1].buf = addr;
+        msgs[1].len = sizeof(addr);
+
+        msgs[2].addr = mpu_addr;
+        msgs[2].flags = 0;
+        msgs[2].buf = &buf;
+        msgs[2].len = 1;
+
+        msgs[3].addr = mpu_addr;
+        msgs[3].flags = I2C_M_RD;
+        msgs[3].buf = data;
+        msgs[3].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 4);
+        if (res != 4) {
+                if (res >= 0)
+                        res = -EIO;
+                LOG_RESULT_LOCATION(res);
+                return res;
+        } else
+                return 0;
+}
+
+static int mpu_memory_write(struct i2c_adapter *i2c_adap,
+                            unsigned char mpu_addr,
+                            unsigned short mem_addr,
+                            unsigned int len, unsigned char const *data)
+{
+        unsigned char bank[2];
+        unsigned char addr[2];
+        unsigned char buf[513];
+
+        struct i2c_msg msgs[3];
+        int res;
+
+        if (!data || !i2c_adap) {
+                LOG_RESULT_LOCATION(-EINVAL);
+                return -EINVAL;
+        }
+        if (len >= (sizeof(buf) - 1)) {
+                LOG_RESULT_LOCATION(-ENOMEM);
+                return -ENOMEM;
+        }
+
+        bank[0] = MPUREG_BANK_SEL;
+        bank[1] = mem_addr >> 8;
+
+        addr[0] = MPUREG_MEM_START_ADDR;
+        addr[1] = mem_addr & 0xFF;
+
+        buf[0] = MPUREG_MEM_R_W;
+        memcpy(buf + 1, data, len);
+
+        /* write message */
+        msgs[0].addr = mpu_addr;
+        msgs[0].flags = 0;
+        msgs[0].buf = bank;
+        msgs[0].len = sizeof(bank);
+
+        msgs[1].addr = mpu_addr;
+        msgs[1].flags = 0;
+        msgs[1].buf = addr;
+        msgs[1].len = sizeof(addr);
+
+        msgs[2].addr = mpu_addr;
+        msgs[2].flags = 0;
+        msgs[2].buf = (unsigned char *)buf;
+        msgs[2].len = len + 1;
+
+        res = i2c_transfer(i2c_adap, msgs, 3);
+        if (res != 3) {
+                if (res >= 0)
+                        res = -EIO;
+                LOG_RESULT_LOCATION(res);
+                return res;
+        } else
+                return 0;
+}
+
+int inv_serial_single_write(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned char register_addr,
+        unsigned char data)
+{
+        return inv_i2c_write_register((struct i2c_adapter *)sl_handle,
+                                      slave_addr, register_addr, data);
+}
+EXPORT_SYMBOL(inv_serial_single_write);
+
+int inv_serial_write(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short length,
+        unsigned char const *data)
+{
+        int result;
+        const unsigned short data_length = length - 1;
+        const unsigned char start_reg_addr = data[0];
+        unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
+        unsigned short bytes_written = 0;
+
+        while (bytes_written < data_length) {
+                unsigned short this_len = min(SERIAL_MAX_TRANSFER_SIZE,
+                                             data_length - bytes_written);
+                if (bytes_written == 0) {
+                        result = inv_i2c_write((struct i2c_adapter *)
+                                               sl_handle, slave_addr,
+                                               1 + this_len, data);
+                } else {
+                        /* manually increment register addr between chunks */
+                        i2c_write[0] = start_reg_addr + bytes_written;
+                        memcpy(&i2c_write[1], &data[1 + bytes_written],
+                                this_len);
+                        result = inv_i2c_write((struct i2c_adapter *)
+                                               sl_handle, slave_addr,
+                                               1 + this_len, i2c_write);
+                }
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_written += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_write);
+
+int inv_serial_read(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned char register_addr,
+        unsigned short length,
+        unsigned char *data)
+{
+        int result;
+        unsigned short bytes_read = 0;
+
+        if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR
+                && (register_addr == MPUREG_FIFO_R_W ||
+                    register_addr == MPUREG_MEM_R_W)) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+
+        while (bytes_read < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+                result = inv_i2c_read((struct i2c_adapter *)sl_handle,
+                                      slave_addr, register_addr + bytes_read,
+                                      this_len, &data[bytes_read]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_read += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_read);
+
+int inv_serial_write_mem(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short mem_addr,
+        unsigned short length,
+        unsigned char const *data)
+{
+        int result;
+        unsigned short bytes_written = 0;
+
+        if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+                pr_err("memory read length (%d B) extends beyond its"
+                       " limits (%d) if started at location %d\n", length,
+                       MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+        while (bytes_written < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
+                result = mpu_memory_write((struct i2c_adapter *)sl_handle,
+                                          slave_addr, mem_addr + bytes_written,
+                                          this_len, &data[bytes_written]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_written += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_write_mem);
+
+int inv_serial_read_mem(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short mem_addr,
+        unsigned short length,
+        unsigned char *data)
+{
+        int result;
+        unsigned short bytes_read = 0;
+
+        if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+                printk
+                    ("memory read length (%d B) extends beyond its limits (%d) "
+                     "if started at location %d\n", length,
+                     MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+        while (bytes_read < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+                result =
+                    mpu_memory_read((struct i2c_adapter *)sl_handle,
+                                    slave_addr, mem_addr + bytes_read,
+                                    this_len, &data[bytes_read]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_read += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_read_mem);
+
+int inv_serial_write_fifo(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short length,
+        unsigned char const *data)
+{
+        int result;
+        unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
+        unsigned short bytes_written = 0;
+
+        if (length > FIFO_HW_SIZE) {
+                printk(KERN_ERR
+                       "maximum fifo write length is %d\n", FIFO_HW_SIZE);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+        while (bytes_written < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
+                i2c_write[0] = MPUREG_FIFO_R_W;
+                memcpy(&i2c_write[1], &data[bytes_written], this_len);
+                result = inv_i2c_write((struct i2c_adapter *)sl_handle,
+                                       slave_addr, this_len + 1, i2c_write);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_written += this_len;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_write_fifo);
+
+int inv_serial_read_fifo(
+        void *sl_handle,
+        unsigned char slave_addr,
+        unsigned short length,
+        unsigned char *data)
+{
+        int result;
+        unsigned short bytes_read = 0;
+
+        if (length > FIFO_HW_SIZE) {
+                printk(KERN_ERR
+                       "maximum fifo read length is %d\n", FIFO_HW_SIZE);
+                return INV_ERROR_INVALID_PARAMETER;
+        }
+        while (bytes_read < length) {
+                unsigned short this_len =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+                result = inv_i2c_read((struct i2c_adapter *)sl_handle,
+                                      slave_addr, MPUREG_FIFO_R_W, this_len,
+                                      &data[bytes_read]);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                }
+                bytes_read += this_len;
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL(inv_serial_read_fifo);
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/mpu6050/mpu-dev.c b/drivers/misc/inv_mpu/mpu6050/mpu-dev.c
new file mode 100644
index 00000000000..e171dc2a7ef
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mpu-dev.c
@@ -0,0 +1,1309 @@
+/*
+        $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/>.
+        $
+ */
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/pm.h>
+#include <linux/mutex.h>
+#include <linux/suspend.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "mpuirq.h"
+#include "slaveirq.h"
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#include <linux/mpu.h>
+
+#include "accel/mpu6050.h"
+
+/* Platform data for the MPU */
+struct mpu_private_data {
+        struct miscdevice dev;
+        struct i2c_client *client;
+
+        /* mldl_cfg data */
+        struct mldl_cfg mldl_cfg;
+        struct mpu_ram          mpu_ram;
+        struct mpu_gyro_cfg     mpu_gyro_cfg;
+        struct mpu_offsets      mpu_offsets;
+        struct mpu_chip_info    mpu_chip_info;
+        struct inv_mpu_cfg      inv_mpu_cfg;
+        struct inv_mpu_state    inv_mpu_state;
+
+        struct mutex mutex;
+        wait_queue_head_t mpu_event_wait;
+        struct completion completion;
+        struct timer_list timeout;
+        struct notifier_block nb;
+        struct mpuirq_data mpu_pm_event;
+        int response_timeout;   /* In seconds */
+        unsigned long event;
+        int pid;
+        struct module *slave_modules[EXT_SLAVE_NUM_TYPES];
+};
+
+struct mpu_private_data *mpu_private_data;
+
+static void mpu_pm_timeout(u_long data)
+{
+        struct mpu_private_data *mpu = (struct mpu_private_data *)data;
+        struct i2c_client *client = mpu->client;
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+        complete(&mpu->completion);
+}
+
+static int mpu_pm_notifier_callback(struct notifier_block *nb,
+                                    unsigned long event, void *unused)
+{
+        struct mpu_private_data *mpu =
+            container_of(nb, struct mpu_private_data, nb);
+        struct i2c_client *client = mpu->client;
+        struct timeval event_time;
+        dev_dbg(&client->adapter->dev, "%s: %ld\n", __func__, event);
+
+        /* Prevent the file handle from being closed before we initialize
+           the completion event */
+        mutex_lock(&mpu->mutex);
+        if (!(mpu->pid) ||
+            (event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) {
+                mutex_unlock(&mpu->mutex);
+                return NOTIFY_OK;
+        }
+
+        if (event == PM_SUSPEND_PREPARE)
+                mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE;
+        if (event == PM_POST_SUSPEND)
+                mpu->event = MPU_PM_EVENT_POST_SUSPEND;
+
+        do_gettimeofday(&event_time);
+        mpu->mpu_pm_event.interruptcount++;
+        mpu->mpu_pm_event.irqtime =
+            (((long long)event_time.tv_sec) << 32) + event_time.tv_usec;
+        mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT;
+        mpu->mpu_pm_event.data = mpu->event;
+
+        if (mpu->response_timeout > 0) {
+                mpu->timeout.expires = jiffies + mpu->response_timeout * HZ;
+                add_timer(&mpu->timeout);
+        }
+        INIT_COMPLETION(mpu->completion);
+        mutex_unlock(&mpu->mutex);
+
+        wake_up_interruptible(&mpu->mpu_event_wait);
+        wait_for_completion(&mpu->completion);
+        del_timer_sync(&mpu->timeout);
+        dev_dbg(&client->adapter->dev, "%s: %ld DONE\n", __func__, event);
+        return NOTIFY_OK;
+}
+
+static int mpu_dev_open(struct inode *inode, struct file *file)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        struct i2c_client *client = mpu->client;
+        int result;
+        int ii;
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+        dev_dbg(&client->adapter->dev, "current->pid %d\n", current->pid);
+
+        result = mutex_lock_interruptible(&mpu->mutex);
+        if (mpu->pid) {
+                mutex_unlock(&mpu->mutex);
+                return -EBUSY;
+        }
+        mpu->pid = current->pid;
+
+        /* Reset the sensors to the default */
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "%s: mutex_lock_interruptible returned %d\n",
+                        __func__, result);
+                return result;
+        }
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
+                __module_get(mpu->slave_modules[ii]);
+
+        mutex_unlock(&mpu->mutex);
+        return 0;
+}
+
+/* close function - called when the "file" /dev/mpu is closed in userspace   */
+static int mpu_release(struct inode *inode, struct file *file)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        struct i2c_client *client = mpu->client;
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        int result = 0;
+        int ii;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        mutex_lock(&mpu->mutex);
+        mldl_cfg->inv_mpu_cfg->requested_sensors = 0;
+        result = inv_mpu_suspend(mldl_cfg,
+                                slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                                slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                                slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                                slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                                INV_ALL_SENSORS);
+        mpu->pid = 0;
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
+                module_put(mpu->slave_modules[ii]);
+
+        mutex_unlock(&mpu->mutex);
+        complete(&mpu->completion);
+        dev_dbg(&client->adapter->dev, "mpu_release\n");
+
+        return result;
+}
+
+/* read function called when from /dev/mpu is read.  Read from the FIFO */
+static ssize_t mpu_read(struct file *file,
+                        char __user *buf, size_t count, loff_t *offset)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        struct i2c_client *client = mpu->client;
+        size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long);
+        int err;
+
+        if (!mpu->event && (!(file->f_flags & O_NONBLOCK)))
+                wait_event_interruptible(mpu->mpu_event_wait, mpu->event);
+
+        if (!mpu->event || !buf
+            || count < sizeof(mpu->mpu_pm_event))
+                return 0;
+
+        err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event));
+        if (err) {
+                dev_err(&client->adapter->dev,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        mpu->event = 0;
+        return len;
+}
+
+static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        int mask = 0;
+
+        poll_wait(file, &mpu->mpu_event_wait, poll);
+        if (mpu->event)
+                mask |= POLLIN | POLLRDNORM;
+        return mask;
+}
+
+static int mpu_dev_ioctl_get_ext_slave_platform_data(
+        struct i2c_client *client,
+        struct ext_slave_platform_data __user *arg)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct ext_slave_platform_data *pdata_slave;
+        struct ext_slave_platform_data local_pdata_slave;
+
+        if (copy_from_user(&local_pdata_slave, arg, sizeof(local_pdata_slave)))
+                return -EFAULT;
+
+        if (local_pdata_slave.type >= EXT_SLAVE_NUM_TYPES)
+                return -EINVAL;
+
+        pdata_slave = mpu->mldl_cfg.pdata_slave[local_pdata_slave.type];
+        /* All but private data and irq_data */
+        if (!pdata_slave)
+                return -ENODEV;
+        if (copy_to_user(arg, pdata_slave, sizeof(*pdata_slave)))
+                return -EFAULT;
+        return 0;
+}
+
+static int mpu_dev_ioctl_get_mpu_platform_data(
+        struct i2c_client *client,
+        struct mpu_platform_data __user *arg)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct mpu_platform_data *pdata = mpu->mldl_cfg.pdata;
+
+        if (copy_to_user(arg, pdata, sizeof(*pdata)))
+                return -EFAULT;
+        return 0;
+}
+
+static int mpu_dev_ioctl_get_ext_slave_descr(
+        struct i2c_client *client,
+        struct ext_slave_descr __user *arg)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct ext_slave_descr *slave;
+        struct ext_slave_descr local_slave;
+
+        if (copy_from_user(&local_slave, arg, sizeof(local_slave)))
+                return -EFAULT;
+
+        if (local_slave.type >= EXT_SLAVE_NUM_TYPES)
+                return -EINVAL;
+
+        slave = mpu->mldl_cfg.slave[local_slave.type];
+        /* All but private data and irq_data */
+        if (!slave)
+                return -ENODEV;
+        if (copy_to_user(arg, slave, sizeof(*slave)))
+                return -EFAULT;
+        return 0;
+}
+
+
+/**
+ * slave_config() - Pass a requested slave configuration to the slave sensor
+ *
+ * @adapter the adaptor to use to communicate with the slave
+ * @mldl_cfg the mldl configuration structuer
+ * @slave pointer to the slave descriptor
+ * @usr_config The configuration to pass to the slave sensor
+ *
+ * returns 0 or non-zero error code
+ */
+static int inv_mpu_config(struct mldl_cfg *mldl_cfg,
+                        void *gyro_adapter,
+                        struct ext_slave_config __user *usr_config)
+{
+        int retval = 0;
+        struct ext_slave_config config;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        if (config.len && config.data) {
+                void *data;
+                data = kmalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data, config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = gyro_config(gyro_adapter, mldl_cfg, &config);
+        kfree(config.data);
+        return retval;
+}
+
+static int inv_mpu_get_config(struct mldl_cfg *mldl_cfg,
+                            void *gyro_adapter,
+                            struct ext_slave_config __user *usr_config)
+{
+        int retval = 0;
+        struct ext_slave_config config;
+        void *user_data;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        user_data = config.data;
+        if (config.len && config.data) {
+                void *data;
+                data = kmalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data, config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = gyro_get_config(gyro_adapter, mldl_cfg, &config);
+        if (!retval)
+                retval = copy_to_user((unsigned char __user *)user_data,
+                                config.data, config.len);
+        kfree(config.data);
+        return retval;
+}
+
+static int slave_config(struct mldl_cfg *mldl_cfg,
+                        void *gyro_adapter,
+                        void *slave_adapter,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_config __user *usr_config)
+{
+        int retval = 0;
+        struct ext_slave_config config;
+        if ((!slave) || (!slave->config))
+                return -ENODEV;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        if (config.len && config.data) {
+                void *data;
+                data = kmalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data, config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = inv_mpu_slave_config(mldl_cfg, gyro_adapter, slave_adapter,
+                                      &config, slave, pdata);
+        kfree(config.data);
+        return retval;
+}
+
+static int slave_get_config(struct mldl_cfg *mldl_cfg,
+                            void *gyro_adapter,
+                            void *slave_adapter,
+                            struct ext_slave_descr *slave,
+                            struct ext_slave_platform_data *pdata,
+                            struct ext_slave_config __user *usr_config)
+{
+        int retval = 0;
+        struct ext_slave_config config;
+        void *user_data;
+        if (!(slave) || !(slave->get_config))
+                return -ENODEV;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        user_data = config.data;
+        if (config.len && config.data) {
+                void *data;
+                data = kmalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data, config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = inv_mpu_get_slave_config(mldl_cfg, gyro_adapter,
+                                          slave_adapter, &config, slave, pdata);
+        if (retval) {
+                kfree(config.data);
+                return retval;
+        }
+        retval = copy_to_user((unsigned char __user *)user_data,
+                              config.data, config.len);
+        kfree(config.data);
+        return retval;
+}
+
+static int inv_slave_read(struct mldl_cfg *mldl_cfg,
+                          void *gyro_adapter,
+                          void *slave_adapter,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata,
+                          void __user *usr_data)
+{
+        int retval;
+        unsigned char *data;
+        data = kzalloc(slave->read_len, GFP_KERNEL);
+        if (!data)
+                return -EFAULT;
+
+        retval = inv_mpu_slave_read(mldl_cfg, gyro_adapter, slave_adapter,
+                                    slave, pdata, data);
+
+        if ((!retval) &&
+            (copy_to_user((unsigned char __user *)usr_data,
+                          data, slave->read_len)))
+                retval = -EFAULT;
+
+        kfree(data);
+        return retval;
+}
+
+static int mpu_handle_mlsl(void *sl_handle,
+                           unsigned char addr,
+                           unsigned int cmd,
+                           struct mpu_read_write __user *usr_msg)
+{
+        int retval = 0;
+        struct mpu_read_write msg;
+        unsigned char *user_data;
+        retval = copy_from_user(&msg, usr_msg, sizeof(msg));
+        if (retval)
+                return -EFAULT;
+
+        user_data = msg.data;
+        if (msg.length && msg.data) {
+                unsigned char *data;
+                data = kmalloc(msg.length, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                retval = copy_from_user(data,
+                                        (void __user *)msg.data, msg.length);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                msg.data = data;
+        } else {
+                return -EPERM;
+        }
+
+        switch (cmd) {
+        case MPU_READ:
+                retval = inv_serial_read(sl_handle, addr,
+                                         msg.address, msg.length, msg.data);
+                break;
+        case MPU_WRITE:
+                retval = inv_serial_write(sl_handle, addr,
+                                          msg.length, msg.data);
+                break;
+        case MPU_READ_MEM:
+                retval = inv_serial_read_mem(sl_handle, addr,
+                                             msg.address, msg.length, msg.data);
+                break;
+        case MPU_WRITE_MEM:
+                retval = inv_serial_write_mem(sl_handle, addr,
+                                              msg.address, msg.length,
+                                              msg.data);
+                break;
+        case MPU_READ_FIFO:
+                retval = inv_serial_read_fifo(sl_handle, addr,
+                                              msg.length, msg.data);
+                break;
+        case MPU_WRITE_FIFO:
+                retval = inv_serial_write_fifo(sl_handle, addr,
+                                               msg.length, msg.data);
+                break;
+
+        };
+        if (retval) {
+                dev_err(&((struct i2c_adapter *)sl_handle)->dev,
+                        "%s: i2c %d error %d\n",
+                        __func__, cmd, retval);
+                kfree(msg.data);
+                return retval;
+        }
+        retval = copy_to_user((unsigned char __user *)user_data,
+                              msg.data, msg.length);
+        kfree(msg.data);
+        return retval;
+}
+
+/* ioctl - I/O control */
+static long mpu_dev_ioctl(struct file *file,
+                          unsigned int cmd, unsigned long arg)
+{
+        struct mpu_private_data *mpu =
+            container_of(file->private_data, struct mpu_private_data, dev);
+        struct i2c_client *client = mpu->client;
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        int retval = 0;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_descr **slave = mldl_cfg->slave;
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        retval = mutex_lock_interruptible(&mpu->mutex);
+        if (retval) {
+                dev_err(&client->adapter->dev,
+                        "%s: mutex_lock_interruptible returned %d\n",
+                        __func__, retval);
+                return retval;
+        }
+
+        switch (cmd) {
+        case MPU_GET_EXT_SLAVE_PLATFORM_DATA:
+                retval = mpu_dev_ioctl_get_ext_slave_platform_data(
+                        client,
+                        (struct ext_slave_platform_data __user *)arg);
+                break;
+        case MPU_GET_MPU_PLATFORM_DATA:
+                retval = mpu_dev_ioctl_get_mpu_platform_data(
+                        client,
+                        (struct mpu_platform_data __user *)arg);
+                break;
+        case MPU_GET_EXT_SLAVE_DESCR:
+                retval = mpu_dev_ioctl_get_ext_slave_descr(
+                        client,
+                        (struct ext_slave_descr __user *)arg);
+                break;
+        case MPU_READ:
+        case MPU_WRITE:
+        case MPU_READ_MEM:
+        case MPU_WRITE_MEM:
+        case MPU_READ_FIFO:
+        case MPU_WRITE_FIFO:
+                retval = mpu_handle_mlsl(
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        mldl_cfg->mpu_chip_info->addr, cmd,
+                        (struct mpu_read_write __user *)arg);
+                break;
+        case MPU_CONFIG_GYRO:
+                retval = inv_mpu_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_CONFIG_ACCEL:
+                retval = slave_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave[EXT_SLAVE_TYPE_ACCEL],
+                        pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_CONFIG_COMPASS:
+                retval = slave_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave[EXT_SLAVE_TYPE_COMPASS],
+                        pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_CONFIG_PRESSURE:
+                retval = slave_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        slave[EXT_SLAVE_TYPE_PRESSURE],
+                        pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_GET_CONFIG_GYRO:
+                retval = inv_mpu_get_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_GET_CONFIG_ACCEL:
+                retval = slave_get_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave[EXT_SLAVE_TYPE_ACCEL],
+                        pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_GET_CONFIG_COMPASS:
+                retval = slave_get_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave[EXT_SLAVE_TYPE_COMPASS],
+                        pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_GET_CONFIG_PRESSURE:
+                retval = slave_get_config(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        slave[EXT_SLAVE_TYPE_PRESSURE],
+                        pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+                        (struct ext_slave_config __user *)arg);
+                break;
+        case MPU_SUSPEND:
+                retval = inv_mpu_suspend(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        arg);
+                break;
+        case MPU_RESUME:
+                retval = inv_mpu_resume(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        arg);
+                break;
+        case MPU_PM_EVENT_HANDLED:
+                dev_dbg(&client->adapter->dev, "%s: %d\n", __func__, cmd);
+                complete(&mpu->completion);
+                break;
+        case MPU_READ_ACCEL:
+                retval = inv_slave_read(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave[EXT_SLAVE_TYPE_ACCEL],
+                        pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+                        (unsigned char __user *)arg);
+                break;
+        case MPU_READ_COMPASS:
+                retval = inv_slave_read(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave[EXT_SLAVE_TYPE_COMPASS],
+                        pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+                        (unsigned char __user *)arg);
+                break;
+        case MPU_READ_PRESSURE:
+                retval = inv_slave_read(
+                        mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        slave[EXT_SLAVE_TYPE_PRESSURE],
+                        pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+                        (unsigned char __user *)arg);
+                break;
+        case MPU_GET_REQUESTED_SENSORS:
+                if (copy_to_user(
+                           (__u32 __user *)arg,
+                           &mldl_cfg->inv_mpu_cfg->requested_sensors,
+                           sizeof(mldl_cfg->inv_mpu_cfg->requested_sensors)))
+                        retval = -EFAULT;
+                break;
+        case MPU_SET_REQUESTED_SENSORS:
+                mldl_cfg->inv_mpu_cfg->requested_sensors = arg;
+                break;
+        case MPU_GET_IGNORE_SYSTEM_SUSPEND:
+                if (copy_to_user(
+                        (unsigned char __user *)arg,
+                        &mldl_cfg->inv_mpu_cfg->ignore_system_suspend,
+                        sizeof(mldl_cfg->inv_mpu_cfg->ignore_system_suspend)))
+                        retval = -EFAULT;
+                break;
+        case MPU_SET_IGNORE_SYSTEM_SUSPEND:
+                mldl_cfg->inv_mpu_cfg->ignore_system_suspend = arg;
+                break;
+        case MPU_GET_MLDL_STATUS:
+                if (copy_to_user(
+                        (unsigned char __user *)arg,
+                        &mldl_cfg->inv_mpu_state->status,
+                        sizeof(mldl_cfg->inv_mpu_state->status)))
+                        retval = -EFAULT;
+                break;
+        case MPU_GET_I2C_SLAVES_ENABLED:
+                if (copy_to_user(
+                        (unsigned char __user *)arg,
+                        &mldl_cfg->inv_mpu_state->i2c_slaves_enabled,
+                        sizeof(mldl_cfg->inv_mpu_state->i2c_slaves_enabled)))
+                        retval = -EFAULT;
+                break;
+        default:
+                dev_err(&client->adapter->dev,
+                        "%s: Unknown cmd %x, arg %lu\n",
+                        __func__, cmd, arg);
+                retval = -EINVAL;
+        };
+
+        mutex_unlock(&mpu->mutex);
+        dev_dbg(&client->adapter->dev, "%s: %08x, %08lx, %d\n",
+                __func__, cmd, arg, retval);
+
+        if (retval > 0)
+                retval = -retval;
+
+        return retval;
+}
+
+void mpu_shutdown(struct i2c_client *client)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        mutex_lock(&mpu->mutex);
+        (void)inv_mpu_suspend(mldl_cfg,
+                        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                        slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                        slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                        slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                        INV_ALL_SENSORS);
+        mutex_unlock(&mpu->mutex);
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+}
+
+int mpu_dev_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        mutex_lock(&mpu->mutex);
+        if (!mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
+                dev_dbg(&client->adapter->dev,
+                        "%s: suspending on event %d\n", __func__, mesg.event);
+                (void)inv_mpu_suspend(mldl_cfg,
+                                slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                                slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                                slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                                slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                                INV_ALL_SENSORS);
+        } else {
+                dev_dbg(&client->adapter->dev,
+                        "%s: Already suspended %d\n", __func__, mesg.event);
+        }
+        mutex_unlock(&mpu->mutex);
+        return 0;
+}
+
+int mpu_dev_resume(struct i2c_client *client)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *)i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+        mutex_lock(&mpu->mutex);
+        if (mpu->pid && !mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
+                (void)inv_mpu_resume(mldl_cfg,
+                                slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                                slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                                slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                                slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+                                mldl_cfg->inv_mpu_cfg->requested_sensors);
+                dev_dbg(&client->adapter->dev,
+                        "%s for pid %d\n", __func__, mpu->pid);
+        }
+        mutex_unlock(&mpu->mutex);
+        return 0;
+}
+
+/* define which file operations are supported */
+static const struct file_operations mpu_fops = {
+        .owner = THIS_MODULE,
+        .read = mpu_read,
+        .poll = mpu_poll,
+        .unlocked_ioctl = mpu_dev_ioctl,
+        .open = mpu_dev_open,
+        .release = mpu_release,
+};
+
+int inv_mpu_register_slave(struct module *slave_module,
+                        struct i2c_client *slave_client,
+                        struct ext_slave_platform_data *slave_pdata,
+                        struct ext_slave_descr *(*get_slave_descr)(void))
+{
+        struct mpu_private_data *mpu = mpu_private_data;
+        struct mldl_cfg *mldl_cfg;
+        struct ext_slave_descr *slave_descr;
+        struct ext_slave_platform_data **pdata_slave;
+        char *irq_name = NULL;
+        int result = 0;
+
+        if (!slave_client || !slave_pdata || !get_slave_descr)
+                return -EINVAL;
+
+        if (!mpu) {
+                dev_err(&slave_client->adapter->dev,
+                        "%s: Null mpu_private_data\n", __func__);
+                return -EINVAL;
+        }
+        mldl_cfg    = &mpu->mldl_cfg;
+        pdata_slave = mldl_cfg->pdata_slave;
+        slave_descr = get_slave_descr();
+
+        if (!slave_descr) {
+                dev_err(&slave_client->adapter->dev,
+                        "%s: Null ext_slave_descr\n", __func__);
+                return -EINVAL;
+        }
+
+        mutex_lock(&mpu->mutex);
+        if (mpu->pid) {
+                mutex_unlock(&mpu->mutex);
+                return -EBUSY;
+        }
+
+        if (pdata_slave[slave_descr->type]) {
+                result = -EBUSY;
+                goto out_unlock_mutex;
+        }
+
+        slave_pdata->address    = slave_client->addr;
+        slave_pdata->irq        = slave_client->irq;
+        slave_pdata->adapt_num  = i2c_adapter_id(slave_client->adapter);
+
+        dev_info(&slave_client->adapter->dev,
+                "%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n",
+                __func__,
+                slave_descr->name,
+                slave_descr->type,
+                slave_pdata->address,
+                slave_pdata->irq,
+                slave_pdata->adapt_num);
+
+        switch (slave_descr->type) {
+        case EXT_SLAVE_TYPE_ACCEL:
+                irq_name = "accelirq";
+                break;
+        case EXT_SLAVE_TYPE_COMPASS:
+                irq_name = "compassirq";
+                break;
+        case EXT_SLAVE_TYPE_PRESSURE:
+                irq_name = "pressureirq";
+                break;
+        default:
+                irq_name = "none";
+        };
+        if (slave_descr->init) {
+                result = slave_descr->init(slave_client->adapter,
+                                        slave_descr,
+                                        slave_pdata);
+                if (result) {
+                        dev_err(&slave_client->adapter->dev,
+                                "%s init failed %d\n",
+                                slave_descr->name, result);
+                        goto out_unlock_mutex;
+                }
+        }
+
+        if (slave_descr->type == EXT_SLAVE_TYPE_ACCEL &&
+            slave_descr->id == ACCEL_ID_MPU6050 &&
+            slave_descr->config) {
+                /* pass a reference to the mldl_cfg data
+                   structure to the mpu6050 accel "class" */
+                struct ext_slave_config config;
+                config.key = MPU_SLAVE_CONFIG_INTERNAL_REFERENCE;
+                config.len = sizeof(struct mldl_cfg *);
+                config.apply = true;
+                config.data = mldl_cfg;
+                result = slave_descr->config(
+                        slave_client->adapter, slave_descr,
+                        slave_pdata, &config);
+                if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        goto out_slavedescr_exit;
+                }
+        }
+        pdata_slave[slave_descr->type] = slave_pdata;
+        mpu->slave_modules[slave_descr->type] = slave_module;
+        mldl_cfg->slave[slave_descr->type] = slave_descr;
+
+        goto out_unlock_mutex;
+
+out_slavedescr_exit:
+        if (slave_descr->exit)
+                slave_descr->exit(slave_client->adapter,
+                                  slave_descr, slave_pdata);
+out_unlock_mutex:
+        mutex_unlock(&mpu->mutex);
+
+        if (!result && irq_name && (slave_pdata->irq > 0)) {
+                int warn_result;
+                dev_info(&slave_client->adapter->dev,
+                        "Installing %s irq using %d\n",
+                        irq_name,
+                        slave_pdata->irq);
+                warn_result = slaveirq_init(slave_client->adapter,
+                                        slave_pdata, irq_name);
+                if (result)
+                        dev_WARN(&slave_client->adapter->dev,
+                                "%s irq assigned error: %d\n",
+                                slave_descr->name, warn_result);
+        } else {
+                dev_WARN(&slave_client->adapter->dev,
+                        "%s irq not assigned: %d %d %d\n",
+                        slave_descr->name,
+                        result, (int)irq_name, slave_pdata->irq);
+        }
+
+        return result;
+}
+EXPORT_SYMBOL(inv_mpu_register_slave);
+
+void inv_mpu_unregister_slave(struct i2c_client *slave_client,
+                        struct ext_slave_platform_data *slave_pdata,
+                        struct ext_slave_descr *(*get_slave_descr)(void))
+{
+        struct mpu_private_data *mpu = mpu_private_data;
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct ext_slave_descr *slave_descr;
+        int result;
+
+        dev_info(&slave_client->adapter->dev, "%s\n", __func__);
+
+        if (!slave_client || !slave_pdata || !get_slave_descr)
+                return;
+
+        if (slave_pdata->irq)
+                slaveirq_exit(slave_pdata);
+
+        slave_descr = get_slave_descr();
+        if (!slave_descr)
+                return;
+
+        mutex_lock(&mpu->mutex);
+
+        if (slave_descr->exit) {
+                result = slave_descr->exit(slave_client->adapter,
+                                        slave_descr,
+                                        slave_pdata);
+                if (result)
+                        dev_err(&slave_client->adapter->dev,
+                                "Accel exit failed %d\n", result);
+        }
+        mldl_cfg->slave[slave_descr->type] = NULL;
+        mldl_cfg->pdata_slave[slave_descr->type] = NULL;
+        mpu->slave_modules[slave_descr->type] = NULL;
+
+        mutex_unlock(&mpu->mutex);
+
+}
+EXPORT_SYMBOL(inv_mpu_unregister_slave);
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static const struct i2c_device_id mpu_id[] = {
+        {"mpu3050", 0},
+        {"mpu6050", 0},
+        {"mpu6050_no_accel", 0},
+        {}
+};
+MODULE_DEVICE_TABLE(i2c, mpu_id);
+
+int mpu_probe(struct i2c_client *client, const struct i2c_device_id *devid)
+{
+        struct mpu_platform_data *pdata;
+        struct mpu_private_data *mpu;
+        struct mldl_cfg *mldl_cfg;
+        int res = 0;
+        int ii = 0;
+        unsigned long irq_flags;
+
+        dev_info(&client->adapter->dev, "%s: %d\n", __func__, ii++);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                res = -ENODEV;
+                goto out_check_functionality_failed;
+        }
+
+        mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL);
+        if (!mpu) {
+                res = -ENOMEM;
+                goto out_alloc_data_failed;
+        }
+        mldl_cfg = &mpu->mldl_cfg;
+        mldl_cfg->mpu_ram       = &mpu->mpu_ram;
+        mldl_cfg->mpu_gyro_cfg  = &mpu->mpu_gyro_cfg;
+        mldl_cfg->mpu_offsets   = &mpu->mpu_offsets;
+        mldl_cfg->mpu_chip_info = &mpu->mpu_chip_info;
+        mldl_cfg->inv_mpu_cfg   = &mpu->inv_mpu_cfg;
+        mldl_cfg->inv_mpu_state = &mpu->inv_mpu_state;
+
+        mldl_cfg->mpu_ram->length = MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE;
+        mldl_cfg->mpu_ram->ram = kzalloc(mldl_cfg->mpu_ram->length, GFP_KERNEL);
+        if (!mldl_cfg->mpu_ram->ram) {
+                res = -ENOMEM;
+                goto out_alloc_ram_failed;
+        }
+        mpu_private_data = mpu;
+        i2c_set_clientdata(client, mpu);
+        mpu->client = client;
+
+        init_waitqueue_head(&mpu->mpu_event_wait);
+        mutex_init(&mpu->mutex);
+        init_completion(&mpu->completion);
+
+        mpu->response_timeout = 60;     /* Seconds */
+        mpu->timeout.function = mpu_pm_timeout;
+        mpu->timeout.data = (u_long) mpu;
+        init_timer(&mpu->timeout);
+
+        mpu->nb.notifier_call = mpu_pm_notifier_callback;
+        mpu->nb.priority = 0;
+        res = register_pm_notifier(&mpu->nb);
+        if (res) {
+                dev_err(&client->adapter->dev,
+                        "Unable to register pm_notifier %d\n", res);
+                goto out_register_pm_notifier_failed;
+        }
+
+        pdata = (struct mpu_platform_data *)client->dev.platform_data;
+        if (!pdata) {
+                dev_WARN(&client->adapter->dev,
+                         "Missing platform data for mpu\n");
+        }
+        mldl_cfg->pdata = pdata;
+
+        mldl_cfg->mpu_chip_info->addr = client->addr;
+        res = inv_mpu_open(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
+
+        if (res) {
+                dev_err(&client->adapter->dev,
+                        "Unable to open %s %d\n", MPU_NAME, res);
+                res = -ENODEV;
+                goto out_whoami_failed;
+        }
+
+        mpu->dev.minor = MISC_DYNAMIC_MINOR;
+        mpu->dev.name = "mpu";
+        mpu->dev.fops = &mpu_fops;
+        res = misc_register(&mpu->dev);
+        if (res < 0) {
+                dev_err(&client->adapter->dev,
+                        "ERROR: misc_register returned %d\n", res);
+                goto out_misc_register_failed;
+        }
+
+        if (client->irq) {
+                dev_info(&client->adapter->dev,
+                         "Installing irq using %d\n", client->irq);
+                if (BIT_ACTL_LOW == ((mldl_cfg->pdata->int_config) & BIT_ACTL))
+                        irq_flags = IRQF_TRIGGER_FALLING;
+                else
+                        irq_flags = IRQF_TRIGGER_RISING;
+                res = mpuirq_init(client, mldl_cfg, irq_flags);
+
+                if (res)
+                        goto out_mpuirq_failed;
+        } else {
+                dev_WARN(&client->adapter->dev,
+                         "Missing %s IRQ\n", MPU_NAME);
+        }
+        if (!strcmp(mpu_id[1].name, devid->name)) {
+                /* Special case to re-use the inv_mpu_register_slave */
+                struct ext_slave_platform_data *slave_pdata;
+                slave_pdata = kzalloc(sizeof(*slave_pdata), GFP_KERNEL);
+                if (!slave_pdata) {
+                        res = -ENOMEM;
+                        goto out_slave_pdata_kzalloc_failed;
+                }
+                slave_pdata->bus = EXT_SLAVE_BUS_PRIMARY;
+                for (ii = 0; ii < 9; ii++)
+                        slave_pdata->orientation[ii] = pdata->orientation[ii];
+                res = inv_mpu_register_slave(
+                        NULL, client,
+                        slave_pdata,
+                        mpu6050_get_slave_descr);
+                if (res) {
+                        /* if inv_mpu_register_slave fails there are no pointer
+                           references to the memory allocated to slave_pdata */
+                        kfree(slave_pdata);
+                        goto out_slave_pdata_kzalloc_failed;
+                }
+        }
+        return res;
+
+out_slave_pdata_kzalloc_failed:
+        if (client->irq)
+                mpuirq_exit();
+out_mpuirq_failed:
+        misc_deregister(&mpu->dev);
+out_misc_register_failed:
+        inv_mpu_close(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
+out_whoami_failed:
+        unregister_pm_notifier(&mpu->nb);
+out_register_pm_notifier_failed:
+        kfree(mldl_cfg->mpu_ram->ram);
+        mpu_private_data = NULL;
+out_alloc_ram_failed:
+        kfree(mpu);
+out_alloc_data_failed:
+out_check_functionality_failed:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, res);
+        return res;
+
+}
+
+static int mpu_remove(struct i2c_client *client)
+{
+        struct mpu_private_data *mpu = i2c_get_clientdata(client);
+        struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+        int ii;
+
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+                if (!pdata_slave[ii])
+                        slave_adapter[ii] = NULL;
+                else
+                        slave_adapter[ii] =
+                                i2c_get_adapter(pdata_slave[ii]->adapt_num);
+        }
+
+        slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_close(mldl_cfg,
+                slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+                slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+                slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+                slave_adapter[EXT_SLAVE_TYPE_PRESSURE]);
+
+        if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL] &&
+                (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id ==
+                        ACCEL_ID_MPU6050)) {
+                struct ext_slave_platform_data *slave_pdata =
+                        mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL];
+                inv_mpu_unregister_slave(
+                        client,
+                        mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+                        mpu6050_get_slave_descr);
+                kfree(slave_pdata);
+        }
+
+        if (client->irq)
+                mpuirq_exit();
+
+        misc_deregister(&mpu->dev);
+
+        unregister_pm_notifier(&mpu->nb);
+
+        kfree(mpu->mldl_cfg.mpu_ram->ram);
+        kfree(mpu);
+
+        return 0;
+}
+
+static struct i2c_driver mpu_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = mpu_probe,
+        .remove = mpu_remove,
+        .id_table = mpu_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = MPU_NAME,
+                   },
+        .address_list = normal_i2c,
+        .shutdown = mpu_shutdown,       /* optional */
+        .suspend = mpu_dev_suspend,     /* optional */
+        .resume = mpu_dev_resume,       /* optional */
+
+};
+
+static int __init mpu_init(void)
+{
+        int res = i2c_add_driver(&mpu_driver);
+        pr_info("%s: Probe name %s\n", __func__, MPU_NAME);
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit mpu_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&mpu_driver);
+}
+
+module_init(mpu_init);
+module_exit(mpu_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("User space character device interface for MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS(MPU_NAME);
diff --git a/drivers/misc/inv_mpu/mpu6050b1.h b/drivers/misc/inv_mpu/mpu6050b1.h
new file mode 100644
index 00000000000..686f6e5d86a
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050b1.h
@@ -0,0 +1,435 @@
+/*
+        $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/>.
+        $
+ */
+
+/**
+ * @defgroup
+ * @brief
+ *
+ * @{
+ *      @file     mpu6050.h
+ *      @brief
+ */
+
+#ifndef __MPU_H_
+#error Do not include this file directly.  Include mpu.h instead.
+#endif
+
+#ifndef __MPU6050B1_H_
+#define __MPU6050B1_H_
+
+
+#define MPU_NAME "mpu6050B1"
+#define DEFAULT_MPU_SLAVEADDR           0x68
+
+/*==== MPU6050B1 REGISTER SET ====*/
+enum {
+        MPUREG_XG_OFFS_TC = 0,                  /* 0x00,   0 */
+        MPUREG_YG_OFFS_TC,                      /* 0x01,   1 */
+        MPUREG_ZG_OFFS_TC,                      /* 0x02,   2 */
+        MPUREG_X_FINE_GAIN,                     /* 0x03,   3 */
+        MPUREG_Y_FINE_GAIN,                     /* 0x04,   4 */
+        MPUREG_Z_FINE_GAIN,                     /* 0x05,   5 */
+        MPUREG_XA_OFFS_H,                       /* 0x06,   6 */
+        MPUREG_XA_OFFS_L,                       /* 0x07,   7 */
+        MPUREG_YA_OFFS_H,                       /* 0x08,   8 */
+        MPUREG_YA_OFFS_L,                       /* 0x09,   9 */
+        MPUREG_ZA_OFFS_H,                       /* 0x0a,  10 */
+        MPUREG_ZA_OFFS_L,                       /* 0x0B,  11 */
+        MPUREG_PRODUCT_ID,                      /* 0x0c,  12 */
+        MPUREG_0D_RSVD,                         /* 0x0d,  13 */
+        MPUREG_0E_RSVD,                         /* 0x0e,  14 */
+        MPUREG_0F_RSVD,                         /* 0x0f,  15 */
+        MPUREG_10_RSVD,                         /* 0x00,  16 */
+        MPUREG_11_RSVD,                         /* 0x11,  17 */
+        MPUREG_12_RSVD,                         /* 0x12,  18 */
+        MPUREG_XG_OFFS_USRH,                    /* 0x13,  19 */
+        MPUREG_XG_OFFS_USRL,                    /* 0x14,  20 */
+        MPUREG_YG_OFFS_USRH,                    /* 0x15,  21 */
+        MPUREG_YG_OFFS_USRL,                    /* 0x16,  22 */
+        MPUREG_ZG_OFFS_USRH,                    /* 0x17,  23 */
+        MPUREG_ZG_OFFS_USRL,                    /* 0x18,  24 */
+        MPUREG_SMPLRT_DIV,                      /* 0x19,  25 */
+        MPUREG_CONFIG,                          /* 0x1A,  26 */
+        MPUREG_GYRO_CONFIG,                     /* 0x1b,  27 */
+        MPUREG_ACCEL_CONFIG,                    /* 0x1c,  28 */
+        MPUREG_ACCEL_FF_THR,                    /* 0x1d,  29 */
+        MPUREG_ACCEL_FF_DUR,                    /* 0x1e,  30 */
+        MPUREG_ACCEL_MOT_THR,                   /* 0x1f,  31 */
+        MPUREG_ACCEL_MOT_DUR,                   /* 0x20,  32 */
+        MPUREG_ACCEL_ZRMOT_THR,                 /* 0x21,  33 */
+        MPUREG_ACCEL_ZRMOT_DUR,                 /* 0x22,  34 */
+        MPUREG_FIFO_EN,                         /* 0x23,  35 */
+        MPUREG_I2C_MST_CTRL,                    /* 0x24,  36 */
+        MPUREG_I2C_SLV0_ADDR,                   /* 0x25,  37 */
+        MPUREG_I2C_SLV0_REG,                    /* 0x26,  38 */
+        MPUREG_I2C_SLV0_CTRL,                   /* 0x27,  39 */
+        MPUREG_I2C_SLV1_ADDR,                   /* 0x28,  40 */
+        MPUREG_I2C_SLV1_REG,                    /* 0x29,  41 */
+        MPUREG_I2C_SLV1_CTRL,                   /* 0x2a,  42 */
+        MPUREG_I2C_SLV2_ADDR,                   /* 0x2B,  43 */
+        MPUREG_I2C_SLV2_REG,                    /* 0x2c,  44 */
+        MPUREG_I2C_SLV2_CTRL,                   /* 0x2d,  45 */
+        MPUREG_I2C_SLV3_ADDR,                   /* 0x2E,  46 */
+        MPUREG_I2C_SLV3_REG,                    /* 0x2f,  47 */
+        MPUREG_I2C_SLV3_CTRL,                   /* 0x30,  48 */
+        MPUREG_I2C_SLV4_ADDR,                   /* 0x31,  49 */
+        MPUREG_I2C_SLV4_REG,                    /* 0x32,  50 */
+        MPUREG_I2C_SLV4_DO,                     /* 0x33,  51 */
+        MPUREG_I2C_SLV4_CTRL,                   /* 0x34,  52 */
+        MPUREG_I2C_SLV4_DI,                     /* 0x35,  53 */
+        MPUREG_I2C_MST_STATUS,                  /* 0x36,  54 */
+        MPUREG_INT_PIN_CFG,                     /* 0x37,  55 */
+        MPUREG_INT_ENABLE,                      /* 0x38,  56 */
+        MPUREG_DMP_INT_STATUS,                  /* 0x39,  57 */
+        MPUREG_INT_STATUS,                      /* 0x3A,  58 */
+        MPUREG_ACCEL_XOUT_H,                    /* 0x3B,  59 */
+        MPUREG_ACCEL_XOUT_L,                    /* 0x3c,  60 */
+        MPUREG_ACCEL_YOUT_H,                    /* 0x3d,  61 */
+        MPUREG_ACCEL_YOUT_L,                    /* 0x3e,  62 */
+        MPUREG_ACCEL_ZOUT_H,                    /* 0x3f,  63 */
+        MPUREG_ACCEL_ZOUT_L,                    /* 0x40,  64 */
+        MPUREG_TEMP_OUT_H,                      /* 0x41,  65 */
+        MPUREG_TEMP_OUT_L,                      /* 0x42,  66 */
+        MPUREG_GYRO_XOUT_H,                     /* 0x43,  67 */
+        MPUREG_GYRO_XOUT_L,                     /* 0x44,  68 */
+        MPUREG_GYRO_YOUT_H,                     /* 0x45,  69 */
+        MPUREG_GYRO_YOUT_L,                     /* 0x46,  70 */
+        MPUREG_GYRO_ZOUT_H,                     /* 0x47,  71 */
+        MPUREG_GYRO_ZOUT_L,                     /* 0x48,  72 */
+        MPUREG_EXT_SLV_SENS_DATA_00,            /* 0x49,  73 */
+        MPUREG_EXT_SLV_SENS_DATA_01,            /* 0x4a,  74 */
+        MPUREG_EXT_SLV_SENS_DATA_02,            /* 0x4b,  75 */
+        MPUREG_EXT_SLV_SENS_DATA_03,            /* 0x4c,  76 */
+        MPUREG_EXT_SLV_SENS_DATA_04,            /* 0x4d,  77 */
+        MPUREG_EXT_SLV_SENS_DATA_05,            /* 0x4e,  78 */
+        MPUREG_EXT_SLV_SENS_DATA_06,            /* 0x4F,  79 */
+        MPUREG_EXT_SLV_SENS_DATA_07,            /* 0x50,  80 */
+        MPUREG_EXT_SLV_SENS_DATA_08,            /* 0x51,  81 */
+        MPUREG_EXT_SLV_SENS_DATA_09,            /* 0x52,  82 */
+        MPUREG_EXT_SLV_SENS_DATA_10,            /* 0x53,  83 */
+        MPUREG_EXT_SLV_SENS_DATA_11,            /* 0x54,  84 */
+        MPUREG_EXT_SLV_SENS_DATA_12,            /* 0x55,  85 */
+        MPUREG_EXT_SLV_SENS_DATA_13,            /* 0x56,  86 */
+        MPUREG_EXT_SLV_SENS_DATA_14,            /* 0x57,  87 */
+        MPUREG_EXT_SLV_SENS_DATA_15,            /* 0x58,  88 */
+        MPUREG_EXT_SLV_SENS_DATA_16,            /* 0x59,  89 */
+        MPUREG_EXT_SLV_SENS_DATA_17,            /* 0x5a,  90 */
+        MPUREG_EXT_SLV_SENS_DATA_18,            /* 0x5B,  91 */
+        MPUREG_EXT_SLV_SENS_DATA_19,            /* 0x5c,  92 */
+        MPUREG_EXT_SLV_SENS_DATA_20,            /* 0x5d,  93 */
+        MPUREG_EXT_SLV_SENS_DATA_21,            /* 0x5e,  94 */
+        MPUREG_EXT_SLV_SENS_DATA_22,            /* 0x5f,  95 */
+        MPUREG_EXT_SLV_SENS_DATA_23,            /* 0x60,  96 */
+        MPUREG_ACCEL_INTEL_STATUS,              /* 0x61,  97 */
+        MPUREG_62_RSVD,                         /* 0x62,  98 */
+        MPUREG_I2C_SLV0_DO,                     /* 0x63,  99 */
+        MPUREG_I2C_SLV1_DO,                     /* 0x64, 100 */
+        MPUREG_I2C_SLV2_DO,                     /* 0x65, 101 */
+        MPUREG_I2C_SLV3_DO,                     /* 0x66, 102 */
+        MPUREG_I2C_MST_DELAY_CTRL,              /* 0x67, 103 */
+        MPUREG_SIGNAL_PATH_RESET,               /* 0x68, 104 */
+        MPUREG_ACCEL_INTEL_CTRL,                /* 0x69, 105 */
+        MPUREG_USER_CTRL,                       /* 0x6A, 106 */
+        MPUREG_PWR_MGMT_1,                      /* 0x6B, 107 */
+        MPUREG_PWR_MGMT_2,                      /* 0x6C, 108 */
+        MPUREG_BANK_SEL,                        /* 0x6D, 109 */
+        MPUREG_MEM_START_ADDR,                  /* 0x6E, 100 */
+        MPUREG_MEM_R_W,                         /* 0x6F, 111 */
+        MPUREG_DMP_CFG_1,                       /* 0x70, 112 */
+        MPUREG_DMP_CFG_2,                       /* 0x71, 113 */
+        MPUREG_FIFO_COUNTH,                     /* 0x72, 114 */
+        MPUREG_FIFO_COUNTL,                     /* 0x73, 115 */
+        MPUREG_FIFO_R_W,                        /* 0x74, 116 */
+        MPUREG_WHOAMI,                          /* 0x75, 117 */
+
+        NUM_OF_MPU_REGISTERS                    /* = 0x76, 118 */
+};
+
+/*==== MPU6050B1 MEMORY ====*/
+enum MPU_MEMORY_BANKS {
+        MEM_RAM_BANK_0 = 0,
+        MEM_RAM_BANK_1,
+        MEM_RAM_BANK_2,
+        MEM_RAM_BANK_3,
+        MEM_RAM_BANK_4,
+        MEM_RAM_BANK_5,
+        MEM_RAM_BANK_6,
+        MEM_RAM_BANK_7,
+        MEM_RAM_BANK_8,
+        MEM_RAM_BANK_9,
+        MEM_RAM_BANK_10,
+        MEM_RAM_BANK_11,
+        MPU_MEM_NUM_RAM_BANKS,
+        MPU_MEM_OTP_BANK_0 = 16
+};
+
+
+/*==== MPU6050B1 parameters ====*/
+
+#define NUM_REGS                (NUM_OF_MPU_REGISTERS)
+#define START_SENS_REGS         (0x3B)
+#define NUM_SENS_REGS           (0x60 - START_SENS_REGS + 1)
+
+/*---- MPU Memory ----*/
+#define NUM_BANKS               (MPU_MEM_NUM_RAM_BANKS)
+#define BANK_SIZE               (256)
+#define MEM_SIZE                (NUM_BANKS * BANK_SIZE)
+#define MPU_MEM_BANK_SIZE       (BANK_SIZE)     /*alternative name */
+
+#define FIFO_HW_SIZE            (1024)
+
+#define NUM_EXT_SLAVES          (4)
+
+
+/*==== BITS FOR MPU6050B1 ====*/
+/*---- MPU6050B1 'XG_OFFS_TC' register (0, 1, 2) ----*/
+#define BIT_PU_SLEEP_MODE                       0x80
+#define BITS_XG_OFFS_TC                         0x7E
+#define BIT_OTP_BNK_VLD                         0x01
+
+#define BIT_I2C_MST_VDDIO                       0x80
+#define BITS_YG_OFFS_TC                         0x7E
+#define BITS_ZG_OFFS_TC                         0x7E
+/*---- MPU6050B1 'FIFO_EN' register (23) ----*/
+#define BIT_TEMP_OUT                            0x80
+#define BIT_GYRO_XOUT                           0x40
+#define BIT_GYRO_YOUT                           0x20
+#define BIT_GYRO_ZOUT                           0x10
+#define BIT_ACCEL                               0x08
+#define BIT_SLV_2                               0x04
+#define BIT_SLV_1                               0x02
+#define BIT_SLV_0                               0x01
+/*---- MPU6050B1 'CONFIG' register (1A) ----*/
+/*NONE                                          0xC0 */
+#define BITS_EXT_SYNC_SET                       0x38
+#define BITS_DLPF_CFG                           0x07
+/*---- MPU6050B1 'GYRO_CONFIG' register (1B) ----*/
+/* voluntarily modified label from BITS_FS_SEL to
+ * BITS_GYRO_FS_SEL to avoid confusion with MPU
+ */
+#define BITS_GYRO_FS_SEL                        0x18
+/*NONE                                          0x07 */
+/*---- MPU6050B1 'ACCEL_CONFIG' register (1C) ----*/
+#define BITS_ACCEL_FS_SEL                       0x18
+#define BITS_ACCEL_HPF                          0x07
+/*---- MPU6050B1 'I2C_MST_CTRL' register (24) ----*/
+#define BIT_MULT_MST_EN                         0x80
+#define BIT_WAIT_FOR_ES                         0x40
+#define BIT_SLV_3_FIFO_EN                       0x20
+#define BIT_I2C_MST_PSR                         0x10
+#define BITS_I2C_MST_CLK                        0x0F
+/*---- MPU6050B1 'I2C_SLV?_ADDR' register (27,2A,2D,30) ----*/
+#define BIT_I2C_READ                            0x80
+#define BIT_I2C_WRITE                           0x00
+#define BITS_I2C_ADDR                           0x7F
+/*---- MPU6050B1 'I2C_SLV?_CTRL' register (27,2A,2D,30) ----*/
+#define BIT_SLV_ENABLE                          0x80
+#define BIT_SLV_BYTE_SW                         0x40
+#define BIT_SLV_REG_DIS                         0x20
+#define BIT_SLV_GRP                             0x10
+#define BITS_SLV_LENG                           0x0F
+/*---- MPU6050B1 'I2C_SLV4_ADDR' register (31) ----*/
+#define BIT_I2C_SLV4_RNW                        0x80
+/*---- MPU6050B1 'I2C_SLV4_CTRL' register (34) ----*/
+#define BIT_I2C_SLV4_EN                         0x80
+#define BIT_SLV4_DONE_INT_EN                    0x40
+#define BIT_SLV4_REG_DIS                        0x20
+#define MASK_I2C_MST_DLY                        0x1F
+/*---- MPU6050B1 'I2C_MST_STATUS' register (36) ----*/
+#define BIT_PASS_THROUGH                        0x80
+#define BIT_I2C_SLV4_DONE                       0x40
+#define BIT_I2C_LOST_ARB                        0x20
+#define BIT_I2C_SLV4_NACK                       0x10
+#define BIT_I2C_SLV3_NACK                       0x08
+#define BIT_I2C_SLV2_NACK                       0x04
+#define BIT_I2C_SLV1_NACK                       0x02
+#define BIT_I2C_SLV0_NACK                       0x01
+/*---- MPU6050B1 'INT_PIN_CFG' register (37) ----*/
+#define BIT_ACTL                                0x80
+#define BIT_ACTL_LOW                            0x80
+#define BIT_ACTL_HIGH                           0x00
+#define BIT_OPEN                                0x40
+#define BIT_LATCH_INT_EN                        0x20
+#define BIT_INT_ANYRD_2CLEAR                    0x10
+#define BIT_ACTL_FSYNC                          0x08
+#define BIT_FSYNC_INT_EN                        0x04
+#define BIT_BYPASS_EN                           0x02
+#define BIT_CLKOUT_EN                           0x01
+/*---- MPU6050B1 'INT_ENABLE' register (38) ----*/
+#define BIT_FF_EN                               0x80
+#define BIT_MOT_EN                              0x40
+#define BIT_ZMOT_EN                             0x20
+#define BIT_FIFO_OVERFLOW_EN                    0x10
+#define BIT_I2C_MST_INT_EN                      0x08
+#define BIT_PLL_RDY_EN                          0x04
+#define BIT_DMP_INT_EN                          0x02
+#define BIT_RAW_RDY_EN                          0x01
+/*---- MPU6050B1 'DMP_INT_STATUS' register (39) ----*/
+/*NONE                                          0x80 */
+/*NONE                                          0x40 */
+#define BIT_DMP_INT_5                           0x20
+#define BIT_DMP_INT_4                           0x10
+#define BIT_DMP_INT_3                           0x08
+#define BIT_DMP_INT_2                           0x04
+#define BIT_DMP_INT_1                           0x02
+#define BIT_DMP_INT_0                           0x01
+/*---- MPU6050B1 'INT_STATUS' register (3A) ----*/
+#define BIT_FF_INT                              0x80
+#define BIT_MOT_INT                             0x40
+#define BIT_ZMOT_INT                            0x20
+#define BIT_FIFO_OVERFLOW_INT                   0x10
+#define BIT_I2C_MST_INT                         0x08
+#define BIT_PLL_RDY_INT                         0x04
+#define BIT_DMP_INT                             0x02
+#define BIT_RAW_DATA_RDY_INT                    0x01
+/*---- MPU6050B1 'MPUREG_I2C_MST_DELAY_CTRL' register (0x67) ----*/
+#define BIT_DELAY_ES_SHADOW                     0x80
+#define BIT_SLV4_DLY_EN                         0x10
+#define BIT_SLV3_DLY_EN                         0x08
+#define BIT_SLV2_DLY_EN                         0x04
+#define BIT_SLV1_DLY_EN                         0x02
+#define BIT_SLV0_DLY_EN                         0x01
+/*---- MPU6050B1 'BANK_SEL' register (6D) ----*/
+#define BIT_PRFTCH_EN                           0x40
+#define BIT_CFG_USER_BANK                       0x20
+#define BITS_MEM_SEL                            0x1f
+/*---- MPU6050B1 'USER_CTRL' register (6A) ----*/
+#define BIT_DMP_EN                              0x80
+#define BIT_FIFO_EN                             0x40
+#define BIT_I2C_MST_EN                          0x20
+#define BIT_I2C_IF_DIS                          0x10
+#define BIT_DMP_RST                             0x08
+#define BIT_FIFO_RST                            0x04
+#define BIT_I2C_MST_RST                         0x02
+#define BIT_SIG_COND_RST                        0x01
+/*---- MPU6050B1 'PWR_MGMT_1' register (6B) ----*/
+#define BIT_H_RESET                             0x80
+#define BIT_SLEEP                               0x40
+#define BIT_CYCLE                               0x20
+#define BIT_PD_PTAT                             0x08
+#define BITS_CLKSEL                             0x07
+/*---- MPU6050B1 'PWR_MGMT_2' register (6C) ----*/
+#define BITS_LPA_WAKE_CTRL                      0xC0
+#define BITS_LPA_WAKE_1HZ                       0x00
+#define BITS_LPA_WAKE_2HZ                       0x40
+#define BITS_LPA_WAKE_10HZ                      0x80
+#define BITS_LPA_WAKE_40HZ                      0xC0
+#define BIT_STBY_XA                             0x20
+#define BIT_STBY_YA                             0x10
+#define BIT_STBY_ZA                             0x08
+#define BIT_STBY_XG                             0x04
+#define BIT_STBY_YG                             0x02
+#define BIT_STBY_ZG                             0x01
+
+#define ACCEL_MOT_THR_LSB (32) /* mg */
+#define ACCEL_MOT_DUR_LSB (1)
+#define ACCEL_ZRMOT_THR_LSB_CONVERSION(mg) ((mg * 1000) / 255)
+#define ACCEL_ZRMOT_DUR_LSB (64)
+
+/*----------------------------------------------------------------------------*/
+/*---- Alternative names to take care of conflicts with current mpu3050.h ----*/
+/*----------------------------------------------------------------------------*/
+
+/*-- registers --*/
+#define MPUREG_DLPF_FS_SYNC     MPUREG_CONFIG                   /* 0x1A */
+
+#define MPUREG_PWR_MGM          MPUREG_PWR_MGMT_1               /* 0x6B */
+#define MPUREG_FIFO_EN1         MPUREG_FIFO_EN                  /* 0x23 */
+#define MPUREG_INT_CFG          MPUREG_INT_ENABLE               /* 0x38 */
+#define MPUREG_X_OFFS_USRH      MPUREG_XG_OFFS_USRH             /* 0x13 */
+#define MPUREG_WHO_AM_I         MPUREG_WHOAMI                   /* 0x75 */
+#define MPUREG_23_RSVD          MPUREG_EXT_SLV_SENS_DATA_00     /* 0x49 */
+
+/*-- bits --*/
+/* 'USER_CTRL' register */
+#define BIT_AUX_IF_EN           BIT_I2C_MST_EN
+#define BIT_AUX_RD_LENG         BIT_I2C_MST_EN
+#define BIT_IME_IF_RST          BIT_I2C_MST_RST
+#define BIT_GYRO_RST            BIT_SIG_COND_RST
+/* 'INT_ENABLE' register */
+#define BIT_RAW_RDY             BIT_RAW_DATA_RDY_INT
+#define BIT_MPU_RDY_EN          BIT_PLL_RDY_EN
+/* 'INT_STATUS' register */
+#define BIT_INT_STATUS_FIFO_OVERLOW BIT_FIFO_OVERFLOW_INT
+
+/*---- MPU6050 Silicon Revisions ----*/
+#define MPU_SILICON_REV_A2              1       /* MPU6050A2 Device */
+#define MPU_SILICON_REV_B1              2       /* MPU6050B1 Device */
+
+/*---- MPU6050 notable product revisions ----*/
+#define MPU_PRODUCT_KEY_B1_E1_5         105
+#define MPU_PRODUCT_KEY_B2_F1           431
+
+/*---- structure containing control variables used by MLDL ----*/
+/*---- MPU clock source settings ----*/
+/*---- MPU filter selections ----*/
+enum mpu_filter {
+        MPU_FILTER_256HZ_NOLPF2 = 0,
+        MPU_FILTER_188HZ,
+        MPU_FILTER_98HZ,
+        MPU_FILTER_42HZ,
+        MPU_FILTER_20HZ,
+        MPU_FILTER_10HZ,
+        MPU_FILTER_5HZ,
+        MPU_FILTER_2100HZ_NOLPF,
+        NUM_MPU_FILTER
+};
+
+enum mpu_fullscale {
+        MPU_FS_250DPS = 0,
+        MPU_FS_500DPS,
+        MPU_FS_1000DPS,
+        MPU_FS_2000DPS,
+        NUM_MPU_FS
+};
+
+enum mpu_clock_sel {
+        MPU_CLK_SEL_INTERNAL = 0,
+        MPU_CLK_SEL_PLLGYROX,
+        MPU_CLK_SEL_PLLGYROY,
+        MPU_CLK_SEL_PLLGYROZ,
+        MPU_CLK_SEL_PLLEXT32K,
+        MPU_CLK_SEL_PLLEXT19M,
+        MPU_CLK_SEL_RESERVED,
+        MPU_CLK_SEL_STOP,
+        NUM_CLK_SEL
+};
+
+enum mpu_ext_sync {
+        MPU_EXT_SYNC_NONE = 0,
+        MPU_EXT_SYNC_TEMP,
+        MPU_EXT_SYNC_GYROX,
+        MPU_EXT_SYNC_GYROY,
+        MPU_EXT_SYNC_GYROZ,
+        MPU_EXT_SYNC_ACCELX,
+        MPU_EXT_SYNC_ACCELY,
+        MPU_EXT_SYNC_ACCELZ,
+        NUM_MPU_EXT_SYNC
+};
+
+#define MPUREG_CONFIG_VALUE(ext_sync, lpf) \
+        ((ext_sync << 3) | lpf)
+
+#define MPUREG_GYRO_CONFIG_VALUE(x_st, y_st, z_st, full_scale)  \
+        ((x_st ? 0x80 : 0) |                            \
+         (y_st ? 0x70 : 0) |                            \
+         (z_st ? 0x60 : 0) |                            \
+         (full_scale << 3))
+
+#endif                          /* __MPU6050_H_ */
diff --git a/drivers/misc/inv_mpu/mpuirq.c b/drivers/misc/inv_mpu/mpuirq.c
new file mode 100644
index 00000000000..d8b721e4346
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpuirq.c
@@ -0,0 +1,254 @@
+/*
+        $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/>.
+        $
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include <linux/mpu.h>
+#include "mpuirq.h"
+#include "mldl_cfg.h"
+
+#define MPUIRQ_NAME "mpuirq"
+
+/* function which gets accel data and sends it to MPU */
+
+DECLARE_WAIT_QUEUE_HEAD(mpuirq_wait);
+
+struct mpuirq_dev_data {
+        struct i2c_client *mpu_client;
+        struct miscdevice *dev;
+        int irq;
+        int pid;
+        int accel_divider;
+        int data_ready;
+        int timeout;
+};
+
+static struct mpuirq_dev_data mpuirq_dev_data;
+static struct mpuirq_data mpuirq_data;
+static char *interface = MPUIRQ_NAME;
+
+static int mpuirq_open(struct inode *inode, struct file *file)
+{
+        dev_dbg(mpuirq_dev_data.dev->this_device,
+                "%s current->pid %d\n", __func__, current->pid);
+        mpuirq_dev_data.pid = current->pid;
+        file->private_data = &mpuirq_dev_data;
+        return 0;
+}
+
+/* close function - called when the "file" /dev/mpuirq is closed in userspace */
+static int mpuirq_release(struct inode *inode, struct file *file)
+{
+        dev_dbg(mpuirq_dev_data.dev->this_device, "mpuirq_release\n");
+        return 0;
+}
+
+/* read function called when from /dev/mpuirq is read */
+static ssize_t mpuirq_read(struct file *file,
+                           char *buf, size_t count, loff_t *ppos)
+{
+        int len, err;
+        struct mpuirq_dev_data *p_mpuirq_dev_data = file->private_data;
+
+        if (!mpuirq_dev_data.data_ready &&
+            mpuirq_dev_data.timeout && (!(file->f_flags & O_NONBLOCK))) {
+                wait_event_interruptible_timeout(mpuirq_wait,
+                                                 mpuirq_dev_data.data_ready,
+                                                 mpuirq_dev_data.timeout);
+        }
+
+        if (mpuirq_dev_data.data_ready && NULL != buf
+            && count >= sizeof(mpuirq_data)) {
+                err = copy_to_user(buf, &mpuirq_data, sizeof(mpuirq_data));
+                mpuirq_data.data_type = 0;
+        } else {
+                return 0;
+        }
+        if (err != 0) {
+                dev_err(p_mpuirq_dev_data->dev->this_device,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        mpuirq_dev_data.data_ready = 0;
+        len = sizeof(mpuirq_data);
+        return len;
+}
+
+unsigned int mpuirq_poll(struct file *file, struct poll_table_struct *poll)
+{
+        int mask = 0;
+
+        poll_wait(file, &mpuirq_wait, poll);
+        if (mpuirq_dev_data.data_ready)
+                mask |= POLLIN | POLLRDNORM;
+        return mask;
+}
+
+/* ioctl - I/O control */
+static long mpuirq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+        int retval = 0;
+        int data;
+
+        switch (cmd) {
+        case MPUIRQ_SET_TIMEOUT:
+                mpuirq_dev_data.timeout = arg;
+                break;
+
+        case MPUIRQ_GET_INTERRUPT_CNT:
+                data = mpuirq_data.interruptcount - 1;
+                if (mpuirq_data.interruptcount > 1)
+                        mpuirq_data.interruptcount = 1;
+
+                if (copy_to_user((int *)arg, &data, sizeof(int)))
+                        return -EFAULT;
+                break;
+        case MPUIRQ_GET_IRQ_TIME:
+                if (copy_to_user((int *)arg, &mpuirq_data.irqtime,
+                                 sizeof(mpuirq_data.irqtime)))
+                        return -EFAULT;
+                mpuirq_data.irqtime = 0;
+                break;
+        case MPUIRQ_SET_FREQUENCY_DIVIDER:
+                mpuirq_dev_data.accel_divider = arg;
+                break;
+        default:
+                retval = -EINVAL;
+        }
+        return retval;
+}
+
+static irqreturn_t mpuirq_handler(int irq, void *dev_id)
+{
+        static int mycount;
+        struct timeval irqtime;
+        mycount++;
+
+        mpuirq_data.interruptcount++;
+
+        /* wake up (unblock) for reading data from userspace */
+        /* and ignore first interrupt generated in module init */
+        mpuirq_dev_data.data_ready = 1;
+
+        do_gettimeofday(&irqtime);
+        mpuirq_data.irqtime = (((long long)irqtime.tv_sec) << 32);
+        mpuirq_data.irqtime += irqtime.tv_usec;
+        mpuirq_data.data_type = MPUIRQ_DATA_TYPE_MPU_IRQ;
+        mpuirq_data.data = 0;
+
+        wake_up_interruptible(&mpuirq_wait);
+
+        return IRQ_HANDLED;
+
+}
+
+/* define which file operations are supported */
+const struct file_operations mpuirq_fops = {
+        .owner = THIS_MODULE,
+        .read = mpuirq_read,
+        .poll = mpuirq_poll,
+
+        .unlocked_ioctl = mpuirq_ioctl,
+        .open = mpuirq_open,
+        .release = mpuirq_release,
+};
+
+static struct miscdevice mpuirq_device = {
+        .minor = MISC_DYNAMIC_MINOR,
+        .name = MPUIRQ_NAME,
+        .fops = &mpuirq_fops,
+};
+
+int mpuirq_init(struct i2c_client *mpu_client, struct mldl_cfg *mldl_cfg,
+                unsigned long irq_flags)
+{
+
+        int res;
+
+        mpuirq_dev_data.mpu_client = mpu_client;
+
+        dev_info(&mpu_client->adapter->dev,
+                 "Module Param interface = %s\n", interface);
+
+        mpuirq_dev_data.irq = mpu_client->irq;
+        mpuirq_dev_data.pid = 0;
+        mpuirq_dev_data.accel_divider = -1;
+        mpuirq_dev_data.data_ready = 0;
+        mpuirq_dev_data.timeout = 0;
+        mpuirq_dev_data.dev = &mpuirq_device;
+
+        if (mpuirq_dev_data.irq) {
+                irq_flags |= IRQF_SHARED;
+                res =
+                    request_irq(mpuirq_dev_data.irq, mpuirq_handler, irq_flags,
+                                interface, &mpuirq_dev_data.irq);
+                if (res) {
+                        dev_err(&mpu_client->adapter->dev,
+                                "myirqtest: cannot register IRQ %d\n",
+                                mpuirq_dev_data.irq);
+                } else {
+                        res = misc_register(&mpuirq_device);
+                        if (res < 0) {
+                                dev_err(&mpu_client->adapter->dev,
+                                        "misc_register returned %d\n", res);
+                                free_irq(mpuirq_dev_data.irq,
+                                         &mpuirq_dev_data.irq);
+                        }
+                }
+
+        } else {
+                res = 0;
+        }
+
+        return res;
+}
+EXPORT_SYMBOL(mpuirq_init);
+
+void mpuirq_exit(void)
+{
+        if (mpuirq_dev_data.irq > 0)
+                free_irq(mpuirq_dev_data.irq, &mpuirq_dev_data.irq);
+
+        dev_info(mpuirq_device.this_device, "Unregistering %s\n", MPUIRQ_NAME);
+        misc_deregister(&mpuirq_device);
+
+        return;
+}
+EXPORT_SYMBOL(mpuirq_exit);
+
+module_param(interface, charp, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(interface, "The Interface name");
diff --git a/drivers/misc/inv_mpu/mpuirq.h b/drivers/misc/inv_mpu/mpuirq.h
new file mode 100644
index 00000000000..073867d830c
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpuirq.h
@@ -0,0 +1,37 @@
+/*
+        $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/>.
+        $
+ */
+
+#ifndef __MPUIRQ__
+#define __MPUIRQ__
+
+#include <linux/i2c-dev.h>
+#include <linux/time.h>
+#include <linux/ioctl.h>
+#include "mldl_cfg.h"
+
+#define MPUIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x40, unsigned long)
+#define MPUIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x41, unsigned long)
+#define MPUIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x42, struct timeval)
+#define MPUIRQ_SET_FREQUENCY_DIVIDER _IOW(MPU_IOCTL, 0x43, unsigned long)
+
+void mpuirq_exit(void);
+int mpuirq_init(struct i2c_client *mpu_client, struct mldl_cfg *mldl_cfg,
+                unsigned long irq_flags);
+
+#endif
diff --git a/drivers/misc/inv_mpu/pressure/Kconfig b/drivers/misc/inv_mpu/pressure/Kconfig
new file mode 100644
index 00000000000..f1c021e8f12
--- /dev/null
+++ b/drivers/misc/inv_mpu/pressure/Kconfig
@@ -0,0 +1,20 @@
+menuconfig: INV_SENSORS_PRESSURE
+        bool "Pressure Sensor Slaves"
+        depends on INV_SENSORS
+        default y
+        help
+          Select y to see a list of supported pressure sensors that can be
+          integrated with the MPUxxxx set of motion processors.
+
+if INV_SENSORS_PRESSURE
+
+config MPU_SENSORS_BMA085
+        tristate "Bosch BMA085"
+        help
+          This enables support for the Bosch bma085 pressure sensor
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one accelerometer can be registered at a time.
+          Specifying more that one accelerometer in the board file will result
+          in runtime errors.
+
+endif
diff --git a/drivers/misc/inv_mpu/pressure/Makefile b/drivers/misc/inv_mpu/pressure/Makefile
new file mode 100644
index 00000000000..595923d809d
--- /dev/null
+++ b/drivers/misc/inv_mpu/pressure/Makefile
@@ -0,0 +1,8 @@
+#
+# Pressure Slaves to MPUxxxx
+#
+obj-$(CONFIG_MPU_SENSORS_BMA085) += inv_mpu_bma085.o
+inv_mpu_bma085-objs +=  bma085.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
diff --git a/drivers/misc/inv_mpu/pressure/bma085.c b/drivers/misc/inv_mpu/pressure/bma085.c
new file mode 100644
index 00000000000..696d2b6e183
--- /dev/null
+++ b/drivers/misc/inv_mpu/pressure/bma085.c
@@ -0,0 +1,367 @@
+/*
+        $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/>.
+        $
+ */
+
+/**
+ *  @defgroup   ACCELDL (Motion Library - Pressure Driver Layer)
+ *  @brief      Provides the interface to setup and handle a pressure
+ *              connected to the secondary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   bma085.c
+ *      @brief  Pressure setup and handling methods.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "log.h"
+
+/*
+ * this structure holds all device specific calibration parameters
+ */
+struct bmp085_calibration_param_t {
+        short ac1;
+        short ac2;
+        short ac3;
+        unsigned short ac4;
+        unsigned short ac5;
+        unsigned short ac6;
+        short b1;
+        short b2;
+        short mb;
+        short mc;
+        short md;
+        long param_b5;
+};
+
+struct bmp085_calibration_param_t cal_param;
+
+#define PRESSURE_BMA085_PARAM_MG      3038        /* calibration parameter */
+#define PRESSURE_BMA085_PARAM_MH     -7357        /* calibration parameter */
+#define PRESSURE_BMA085_PARAM_MI      3791        /* calibration parameter */
+
+/*********************************************
+ *    Pressure Initialization Functions
+ *********************************************/
+
+static int bma085_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+        return result;
+}
+
+#define PRESSURE_BMA085_PROM_START_ADDR  (0xAA)
+#define PRESSURE_BMA085_PROM_DATA_LEN    (22)
+#define PRESSURE_BMP085_CTRL_MEAS_REG    (0xF4)
+/* temperature measurent */
+#define PRESSURE_BMP085_T_MEAS           (0x2E)
+/* pressure measurement; oversampling_setting */
+#define PRESSURE_BMP085_P_MEAS_OSS_0     (0x34)
+#define PRESSURE_BMP085_P_MEAS_OSS_1     (0x74)
+#define PRESSURE_BMP085_P_MEAS_OSS_2     (0xB4)
+#define PRESSURE_BMP085_P_MEAS_OSS_3     (0xF4)
+#define PRESSURE_BMP085_ADC_OUT_MSB_REG  (0xF6)
+#define PRESSURE_BMP085_ADC_OUT_LSB_REG  (0xF7)
+
+static int bma085_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char data[PRESSURE_BMA085_PROM_DATA_LEN];
+
+        result =
+            inv_serial_read(mlsl_handle, pdata->address,
+                           PRESSURE_BMA085_PROM_START_ADDR,
+                           PRESSURE_BMA085_PROM_DATA_LEN, data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* parameters AC1-AC6 */
+        cal_param.ac1 = (data[0] << 8) | data[1];
+        cal_param.ac2 = (data[2] << 8) | data[3];
+        cal_param.ac3 = (data[4] << 8) | data[5];
+        cal_param.ac4 = (data[6] << 8) | data[7];
+        cal_param.ac5 = (data[8] << 8) | data[9];
+        cal_param.ac6 = (data[10] << 8) | data[11];
+
+        /* parameters B1,B2 */
+        cal_param.b1 = (data[12] << 8) | data[13];
+        cal_param.b2 = (data[14] << 8) | data[15];
+
+        /* parameters MB,MC,MD */
+        cal_param.mb = (data[16] << 8) | data[17];
+        cal_param.mc = (data[18] << 8) | data[19];
+        cal_param.md = (data[20] << 8) | data[21];
+
+        return result;
+}
+
+static int bma085_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        int result;
+        long pressure, x1, x2, x3, b3, b6;
+        unsigned long b4, b7;
+        unsigned long up;
+        unsigned short ut;
+        short oversampling_setting = 0;
+        short temperature;
+        long divisor;
+
+        /* get temprature */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       PRESSURE_BMP085_CTRL_MEAS_REG,
+                                       PRESSURE_BMP085_T_MEAS);
+        msleep(5);
+        result =
+            inv_serial_read(mlsl_handle, pdata->address,
+                           PRESSURE_BMP085_ADC_OUT_MSB_REG, 2,
+                           (unsigned char *)data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        ut = (data[0] << 8) | data[1];
+
+        x1 = (((long) ut - (long)cal_param.ac6) * (long)cal_param.ac5) >> 15;
+        divisor = x1 + cal_param.md;
+        if (!divisor)
+                return INV_ERROR_DIVIDE_BY_ZERO;
+
+        x2 = ((long)cal_param.mc << 11) / (x1 + cal_param.md);
+        cal_param.param_b5 = x1 + x2;
+        /* temperature in 0.1 degree C */
+        temperature = (short)((cal_param.param_b5 + 8) >> 4);
+
+        /* get pressure */
+        result = inv_serial_single_write(mlsl_handle, pdata->address,
+                                       PRESSURE_BMP085_CTRL_MEAS_REG,
+                                       PRESSURE_BMP085_P_MEAS_OSS_0);
+        msleep(5);
+        result =
+            inv_serial_read(mlsl_handle, pdata->address,
+                           PRESSURE_BMP085_ADC_OUT_MSB_REG, 2,
+                           (unsigned char *)data);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        up = (((unsigned long) data[0] << 8) | ((unsigned long) data[1]));
+
+        b6 = cal_param.param_b5 - 4000;
+        /* calculate B3 */
+        x1 = (b6*b6) >> 12;
+        x1 *= cal_param.b2;
+        x1 >>= 11;
+
+        x2 = (cal_param.ac2*b6);
+        x2 >>= 11;
+
+        x3 = x1 + x2;
+
+        b3 = (((((long)cal_param.ac1) * 4 + x3)
+            << oversampling_setting) + 2) >> 2;
+
+        /* calculate B4 */
+        x1 = (cal_param.ac3 * b6) >> 13;
+        x2 = (cal_param.b1 * ((b6*b6) >> 12)) >> 16;
+        x3 = ((x1 + x2) + 2) >> 2;
+        b4 = (cal_param.ac4 * (unsigned long) (x3 + 32768)) >> 15;
+        if (!b4)
+                return INV_ERROR;
+
+        b7 = ((unsigned long)(up - b3) * (50000>>oversampling_setting));
+        if (b7 < 0x80000000)
+                pressure = (b7 << 1) / b4;
+        else
+                pressure = (b7 / b4) << 1;
+
+        x1 = pressure >> 8;
+        x1 *= x1;
+        x1 = (x1 * PRESSURE_BMA085_PARAM_MG) >> 16;
+        x2 = (pressure * PRESSURE_BMA085_PARAM_MH) >> 16;
+        /* pressure in Pa */
+        pressure += (x1 + x2 + PRESSURE_BMA085_PARAM_MI) >> 4;
+
+        data[0] = (unsigned char)(pressure >> 16);
+        data[1] = (unsigned char)(pressure >> 8);
+        data[2] = (unsigned char)(pressure & 0xFF);
+
+        return result;
+}
+
+static struct ext_slave_descr bma085_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = bma085_suspend,
+        .resume           = bma085_resume,
+        .read             = bma085_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "bma085",
+        .type             = EXT_SLAVE_TYPE_PRESSURE,
+        .id               = PRESSURE_ID_BMA085,
+        .read_reg         = 0xF6,
+        .read_len         = 3,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {0, 0},
+};
+
+static
+struct ext_slave_descr *bma085_get_slave_descr(void)
+{
+        return &bma085_descr;
+}
+
+/* Platform data for the MPU */
+struct bma085_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int bma085_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct bma085_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        bma085_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int bma085_mod_remove(struct i2c_client *client)
+{
+        struct bma085_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                bma085_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id bma085_mod_id[] = {
+        { "bma085", PRESSURE_ID_BMA085 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, bma085_mod_id);
+
+static struct i2c_driver bma085_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = bma085_mod_probe,
+        .remove = bma085_mod_remove,
+        .id_table = bma085_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "bma085_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init bma085_mod_init(void)
+{
+        int res = i2c_add_driver(&bma085_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "bma085_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit bma085_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&bma085_mod_driver);
+}
+
+module_init(bma085_mod_init);
+module_exit(bma085_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate BMA085 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("bma085_mod");
+/**
+ *  @}
+**/
diff --git a/drivers/misc/inv_mpu/slaveirq.c b/drivers/misc/inv_mpu/slaveirq.c
new file mode 100644
index 00000000000..22cfa4e458e
--- /dev/null
+++ b/drivers/misc/inv_mpu/slaveirq.c
@@ -0,0 +1,268 @@
+/*
+        $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/>.
+        $
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/wait.h>
+#include <linux/slab.h>
+
+#include <linux/mpu.h>
+#include "slaveirq.h"
+#include "mldl_cfg.h"
+
+/* function which gets slave data and sends it to SLAVE */
+
+struct slaveirq_dev_data {
+        struct miscdevice dev;
+        struct i2c_client *slave_client;
+        struct mpuirq_data data;
+        wait_queue_head_t slaveirq_wait;
+        int irq;
+        int pid;
+        int data_ready;
+        int timeout;
+};
+
+/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28
+ * drivers: misc: pass miscdevice pointer via file private data
+ */
+static int slaveirq_open(struct inode *inode, struct file *file)
+{
+        /* Device node is availabe in the file->private_data, this is
+         * exactly what we want so we leave it there */
+        struct slaveirq_dev_data *data =
+            container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+        dev_dbg(data->dev.this_device,
+                "%s current->pid %d\n", __func__, current->pid);
+        data->pid = current->pid;
+        return 0;
+}
+
+static int slaveirq_release(struct inode *inode, struct file *file)
+{
+        struct slaveirq_dev_data *data =
+            container_of(file->private_data, struct slaveirq_dev_data, dev);
+        dev_dbg(data->dev.this_device, "slaveirq_release\n");
+        return 0;
+}
+
+/* read function called when from /dev/slaveirq is read */
+static ssize_t slaveirq_read(struct file *file,
+                             char *buf, size_t count, loff_t *ppos)
+{
+        int len, err;
+        struct slaveirq_dev_data *data =
+            container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+        if (!data->data_ready && data->timeout &&
+            !(file->f_flags & O_NONBLOCK)) {
+                wait_event_interruptible_timeout(data->slaveirq_wait,
+                                                 data->data_ready,
+                                                 data->timeout);
+        }
+
+        if (data->data_ready && NULL != buf && count >= sizeof(data->data)) {
+                err = copy_to_user(buf, &data->data, sizeof(data->data));
+                data->data.data_type = 0;
+        } else {
+                return 0;
+        }
+        if (err != 0) {
+                dev_err(data->dev.this_device,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        data->data_ready = 0;
+        len = sizeof(data->data);
+        return len;
+}
+
+static unsigned int slaveirq_poll(struct file *file,
+                                  struct poll_table_struct *poll)
+{
+        int mask = 0;
+        struct slaveirq_dev_data *data =
+            container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+        poll_wait(file, &data->slaveirq_wait, poll);
+        if (data->data_ready)
+                mask |= POLLIN | POLLRDNORM;
+        return mask;
+}
+
+/* ioctl - I/O control */
+static long slaveirq_ioctl(struct file *file,
+                           unsigned int cmd, unsigned long arg)
+{
+        int retval = 0;
+        int tmp;
+        struct slaveirq_dev_data *data =
+            container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+        switch (cmd) {
+        case SLAVEIRQ_SET_TIMEOUT:
+                data->timeout = arg;
+                break;
+
+        case SLAVEIRQ_GET_INTERRUPT_CNT:
+                tmp = data->data.interruptcount - 1;
+                if (data->data.interruptcount > 1)
+                        data->data.interruptcount = 1;
+
+                if (copy_to_user((int *)arg, &tmp, sizeof(int)))
+                        return -EFAULT;
+                break;
+        case SLAVEIRQ_GET_IRQ_TIME:
+                if (copy_to_user((int *)arg, &data->data.irqtime,
+                                 sizeof(data->data.irqtime)))
+                        return -EFAULT;
+                data->data.irqtime = 0;
+                break;
+        default:
+                retval = -EINVAL;
+        }
+        return retval;
+}
+
+static irqreturn_t slaveirq_handler(int irq, void *dev_id)
+{
+        struct slaveirq_dev_data *data = (struct slaveirq_dev_data *)dev_id;
+        static int mycount;
+        struct timeval irqtime;
+        mycount++;
+
+        data->data.interruptcount++;
+
+        /* wake up (unblock) for reading data from userspace */
+        data->data_ready = 1;
+
+        do_gettimeofday(&irqtime);
+        data->data.irqtime = (((long long)irqtime.tv_sec) << 32);
+        data->data.irqtime += irqtime.tv_usec;
+        data->data.data_type |= 1;
+
+        wake_up_interruptible(&data->slaveirq_wait);
+
+        return IRQ_HANDLED;
+
+}
+
+/* define which file operations are supported */
+static const struct file_operations slaveirq_fops = {
+        .owner = THIS_MODULE,
+        .read = slaveirq_read,
+        .poll = slaveirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+        .compat_ioctl = slaveirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+        .unlocked_ioctl = slaveirq_ioctl,
+#endif
+        .open = slaveirq_open,
+        .release = slaveirq_release,
+};
+
+int slaveirq_init(struct i2c_adapter *slave_adapter,
+                  struct ext_slave_platform_data *pdata, char *name)
+{
+
+        int res;
+        struct slaveirq_dev_data *data;
+
+        if (!pdata->irq)
+                return -EINVAL;
+
+        pdata->irq_data = kzalloc(sizeof(*data), GFP_KERNEL);
+        data = (struct slaveirq_dev_data *)pdata->irq_data;
+        if (!data)
+                return -ENOMEM;
+
+        data->dev.minor = MISC_DYNAMIC_MINOR;
+        data->dev.name = name;
+        data->dev.fops = &slaveirq_fops;
+        data->irq = pdata->irq;
+        data->pid = 0;
+        data->data_ready = 0;
+        data->timeout = 0;
+
+        init_waitqueue_head(&data->slaveirq_wait);
+
+        res = request_irq(data->irq, slaveirq_handler,
+                        IRQF_TRIGGER_RISING | IRQF_SHARED,
+                          data->dev.name, data);
+
+        if (res) {
+                dev_err(&slave_adapter->dev,
+                        "myirqtest: cannot register IRQ %d\n", data->irq);
+                goto out_request_irq;
+        }
+
+        res = misc_register(&data->dev);
+        if (res < 0) {
+                dev_err(&slave_adapter->dev,
+                        "misc_register returned %d\n", res);
+                goto out_misc_register;
+        }
+
+        return res;
+
+out_misc_register:
+        free_irq(data->irq, data);
+out_request_irq:
+        kfree(pdata->irq_data);
+        pdata->irq_data = NULL;
+
+        return res;
+}
+EXPORT_SYMBOL(slaveirq_init);
+
+void slaveirq_exit(struct ext_slave_platform_data *pdata)
+{
+        struct slaveirq_dev_data *data = pdata->irq_data;
+
+        if (!pdata->irq_data || data->irq <= 0)
+                return;
+
+        dev_info(data->dev.this_device, "Unregistering %s\n", data->dev.name);
+
+        free_irq(data->irq, data);
+        misc_deregister(&data->dev);
+        kfree(pdata->irq_data);
+        pdata->irq_data = NULL;
+}
+EXPORT_SYMBOL(slaveirq_exit);
diff --git a/drivers/misc/inv_mpu/slaveirq.h b/drivers/misc/inv_mpu/slaveirq.h
new file mode 100644
index 00000000000..6926634ff94
--- /dev/null
+++ b/drivers/misc/inv_mpu/slaveirq.h
@@ -0,0 +1,36 @@
+/*
+        $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/>.
+        $
+ */
+
+#ifndef __SLAVEIRQ__
+#define __SLAVEIRQ__
+
+#include <linux/i2c-dev.h>
+
+#include <linux/mpu.h>
+#include "mpuirq.h"
+
+#define SLAVEIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x50, unsigned long)
+#define SLAVEIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x51, unsigned long)
+#define SLAVEIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x52, unsigned long)
+
+void slaveirq_exit(struct ext_slave_platform_data *pdata);
+int slaveirq_init(struct i2c_adapter *slave_adapter,
+                  struct ext_slave_platform_data *pdata, char *name);
+
+#endif
diff --git a/drivers/misc/inv_mpu/timerirq.c b/drivers/misc/inv_mpu/timerirq.c
new file mode 100644
index 00000000000..601858f9c4d
--- /dev/null
+++ b/drivers/misc/inv_mpu/timerirq.c
@@ -0,0 +1,296 @@
+/*
+        $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/>.
+        $
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/timer.h>
+#include <linux/slab.h>
+
+#include <linux/mpu.h>
+#include "mltypes.h"
+#include "timerirq.h"
+
+/* function which gets timer data and sends it to TIMER */
+struct timerirq_data {
+        int pid;
+        int data_ready;
+        int run;
+        int timeout;
+        unsigned long period;
+        struct mpuirq_data data;
+        struct completion timer_done;
+        wait_queue_head_t timerirq_wait;
+        struct timer_list timer;
+        struct miscdevice *dev;
+};
+
+static struct miscdevice *timerirq_dev_data;
+
+static void timerirq_handler(unsigned long arg)
+{
+        struct timerirq_data *data = (struct timerirq_data *)arg;
+        struct timeval irqtime;
+
+        data->data.interruptcount++;
+
+        data->data_ready = 1;
+
+        do_gettimeofday(&irqtime);
+        data->data.irqtime = (((long long)irqtime.tv_sec) << 32);
+        data->data.irqtime += irqtime.tv_usec;
+        data->data.data_type |= 1;
+
+        dev_dbg(data->dev->this_device,
+                "%s, %lld, %ld\n", __func__, data->data.irqtime,
+                (unsigned long)data);
+
+        wake_up_interruptible(&data->timerirq_wait);
+
+        if (data->run)
+                mod_timer(&data->timer,
+                          jiffies + msecs_to_jiffies(data->period));
+        else
+                complete(&data->timer_done);
+}
+
+static int start_timerirq(struct timerirq_data *data)
+{
+        dev_dbg(data->dev->this_device,
+                "%s current->pid %d\n", __func__, current->pid);
+
+        /* Timer already running... success */
+        if (data->run)
+                return 0;
+
+        /* Don't allow a period of 0 since this would fire constantly */
+        if (!data->period)
+                return -EINVAL;
+
+        data->run = true;
+        data->data_ready = false;
+
+        init_completion(&data->timer_done);
+        setup_timer(&data->timer, timerirq_handler, (unsigned long)data);
+
+        return mod_timer(&data->timer,
+                         jiffies + msecs_to_jiffies(data->period));
+}
+
+static int stop_timerirq(struct timerirq_data *data)
+{
+        dev_dbg(data->dev->this_device,
+                "%s current->pid %lx\n", __func__, (unsigned long)data);
+
+        if (data->run) {
+                data->run = false;
+                mod_timer(&data->timer, jiffies + 1);
+                wait_for_completion(&data->timer_done);
+        }
+        return 0;
+}
+
+/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28
+ * drivers: misc: pass miscdevice pointer via file private data
+ */
+static int timerirq_open(struct inode *inode, struct file *file)
+{
+        /* Device node is availabe in the file->private_data, this is
+         * exactly what we want so we leave it there */
+        struct miscdevice *dev_data = file->private_data;
+        struct timerirq_data *data = kzalloc(sizeof(*data), GFP_KERNEL);
+        if (!data)
+                return -ENOMEM;
+
+        data->dev = dev_data;
+        file->private_data = data;
+        data->pid = current->pid;
+        init_waitqueue_head(&data->timerirq_wait);
+
+        dev_dbg(data->dev->this_device,
+                "%s current->pid %d\n", __func__, current->pid);
+        return 0;
+}
+
+static int timerirq_release(struct inode *inode, struct file *file)
+{
+        struct timerirq_data *data = file->private_data;
+        dev_dbg(data->dev->this_device, "timerirq_release\n");
+        if (data->run)
+                stop_timerirq(data);
+        kfree(data);
+        return 0;
+}
+
+/* read function called when from /dev/timerirq is read */
+static ssize_t timerirq_read(struct file *file,
+                             char *buf, size_t count, loff_t *ppos)
+{
+        int len, err;
+        struct timerirq_data *data = file->private_data;
+
+        if (!data->data_ready && data->timeout &&
+            !(file->f_flags & O_NONBLOCK)) {
+                wait_event_interruptible_timeout(data->timerirq_wait,
+                                                 data->data_ready,
+                                                 data->timeout);
+        }
+
+        if (data->data_ready && NULL != buf && count >= sizeof(data->data)) {
+                err = copy_to_user(buf, &data->data, sizeof(data->data));
+                data->data.data_type = 0;
+        } else {
+                return 0;
+        }
+        if (err != 0) {
+                dev_err(data->dev->this_device,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        data->data_ready = 0;
+        len = sizeof(data->data);
+        return len;
+}
+
+static unsigned int timerirq_poll(struct file *file,
+                                  struct poll_table_struct *poll)
+{
+        int mask = 0;
+        struct timerirq_data *data = file->private_data;
+
+        poll_wait(file, &data->timerirq_wait, poll);
+        if (data->data_ready)
+                mask |= POLLIN | POLLRDNORM;
+        return mask;
+}
+
+/* ioctl - I/O control */
+static long timerirq_ioctl(struct file *file,
+                           unsigned int cmd, unsigned long arg)
+{
+        int retval = 0;
+        int tmp;
+        struct timerirq_data *data = file->private_data;
+
+        dev_dbg(data->dev->this_device,
+                "%s current->pid %d, %d, %ld\n",
+                __func__, current->pid, cmd, arg);
+
+        if (!data)
+                return -EFAULT;
+
+        switch (cmd) {
+        case TIMERIRQ_SET_TIMEOUT:
+                data->timeout = arg;
+                break;
+        case TIMERIRQ_GET_INTERRUPT_CNT:
+                tmp = data->data.interruptcount - 1;
+                if (data->data.interruptcount > 1)
+                        data->data.interruptcount = 1;
+
+                if (copy_to_user((int *)arg, &tmp, sizeof(int)))
+                        return -EFAULT;
+                break;
+        case TIMERIRQ_START:
+                data->period = arg;
+                retval = start_timerirq(data);
+                break;
+        case TIMERIRQ_STOP:
+                retval = stop_timerirq(data);
+                break;
+        default:
+                retval = -EINVAL;
+        }
+        return retval;
+}
+
+/* define which file operations are supported */
+static const struct file_operations timerirq_fops = {
+        .owner = THIS_MODULE,
+        .read = timerirq_read,
+        .poll = timerirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+        .compat_ioctl = timerirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+        .unlocked_ioctl = timerirq_ioctl,
+#endif
+        .open = timerirq_open,
+        .release = timerirq_release,
+};
+
+static int __init timerirq_init(void)
+{
+
+        int res;
+        static struct miscdevice *data;
+
+        data = kzalloc(sizeof(*data), GFP_KERNEL);
+        if (!data)
+                return -ENOMEM;
+        timerirq_dev_data = data;
+        data->minor = MISC_DYNAMIC_MINOR;
+        data->name = "timerirq";
+        data->fops = &timerirq_fops;
+
+        res = misc_register(data);
+        if (res < 0) {
+                dev_err(data->this_device, "misc_register returned %d\n", res);
+                return res;
+        }
+
+        return res;
+}
+
+module_init(timerirq_init);
+
+static void __exit timerirq_exit(void)
+{
+        struct miscdevice *data = timerirq_dev_data;
+
+        dev_info(data->this_device, "Unregistering %s\n", data->name);
+
+        misc_deregister(data);
+        kfree(data);
+
+        timerirq_dev_data = NULL;
+}
+
+module_exit(timerirq_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Timer IRQ device driver.");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("timerirq");
diff --git a/drivers/misc/inv_mpu/timerirq.h b/drivers/misc/inv_mpu/timerirq.h
new file mode 100644
index 00000000000..f69f07a45a3
--- /dev/null
+++ b/drivers/misc/inv_mpu/timerirq.h
@@ -0,0 +1,30 @@
+/*
+        $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/>.
+        $
+ */
+
+#ifndef __TIMERIRQ__
+#define __TIMERIRQ__
+
+#include <linux/mpu.h>
+
+#define TIMERIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x60, unsigned long)
+#define TIMERIRQ_GET_INTERRUPT_CNT     _IOW(MPU_IOCTL, 0x61, unsigned long)
+#define TIMERIRQ_START                 _IOW(MPU_IOCTL, 0x62, unsigned long)
+#define TIMERIRQ_STOP                  _IO(MPU_IOCTL, 0x63)
+
+#endif
diff --git a/drivers/misc/iwmc3200top/Kconfig b/drivers/misc/iwmc3200top/Kconfig
new file mode 100644
index 00000000000..9e4b88fb57f
--- /dev/null
+++ b/drivers/misc/iwmc3200top/Kconfig
@@ -0,0 +1,20 @@
+config IWMC3200TOP
+        tristate "Intel Wireless MultiCom Top Driver"
+        depends on MMC && EXPERIMENTAL
+        select FW_LOADER
+        ---help---
+          Intel Wireless MultiCom 3200 Top driver is responsible for
+          for firmware load and enabled coms enumeration
+
+config IWMC3200TOP_DEBUG
+        bool "Enable full debug output of iwmc3200top Driver"
+        depends on IWMC3200TOP
+        ---help---
+          Enable full debug output of iwmc3200top Driver
+
+config IWMC3200TOP_DEBUGFS
+        bool "Enable Debugfs debugging interface for iwmc3200top"
+        depends on IWMC3200TOP
+        ---help---
+          Enable creation of debugfs files for iwmc3200top
+
diff --git a/drivers/misc/iwmc3200top/Makefile b/drivers/misc/iwmc3200top/Makefile
new file mode 100644
index 00000000000..fbf53fb4634
--- /dev/null
+++ b/drivers/misc/iwmc3200top/Makefile
@@ -0,0 +1,29 @@
+# iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+# drivers/misc/iwmc3200top/Makefile
+#
+# Copyright (C) 2009 Intel 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 version
+# 2 as published by the Free Software Foundation.
+#
+# 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, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+# 02110-1301, USA.
+#
+#
+# Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+#  -
+#
+#
+
+obj-$(CONFIG_IWMC3200TOP)       += iwmc3200top.o
+iwmc3200top-objs        := main.o fw-download.o
+iwmc3200top-$(CONFIG_IWMC3200TOP_DEBUG) += log.o
+iwmc3200top-$(CONFIG_IWMC3200TOP_DEBUGFS) += debugfs.o
diff --git a/drivers/misc/iwmc3200top/debugfs.c b/drivers/misc/iwmc3200top/debugfs.c
new file mode 100644
index 00000000000..62fbaec4820
--- /dev/null
+++ b/drivers/misc/iwmc3200top/debugfs.c
@@ -0,0 +1,137 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/debufs.c
+ *
+ * Copyright (C) 2009 Intel 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/ctype.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio.h>
+#include <linux/debugfs.h>
+
+#include "iwmc3200top.h"
+#include "fw-msg.h"
+#include "log.h"
+#include "debugfs.h"
+
+
+
+/*      Constants definition        */
+#define HEXADECIMAL_RADIX       16
+
+/*      Functions definition        */
+
+
+#define DEBUGFS_ADD(name, parent) do {                                  \
+        dbgfs->dbgfs_##parent##_files.file_##name =                     \
+        debugfs_create_file(#name, 0644, dbgfs->dir_##parent, priv,     \
+                                &iwmct_dbgfs_##name##_ops);             \
+} while (0)
+
+#define DEBUGFS_RM(name)  do {          \
+        debugfs_remove(name);           \
+        name = NULL;                    \
+} while (0)
+
+#define DEBUGFS_READ_FUNC(name)                                         \
+ssize_t iwmct_dbgfs_##name##_read(struct file *file,                    \
+                                  char __user *user_buf,                \
+                                  size_t count, loff_t *ppos);
+
+#define DEBUGFS_WRITE_FUNC(name)                                        \
+ssize_t iwmct_dbgfs_##name##_write(struct file *file,                   \
+                                   const char __user *user_buf,         \
+                                   size_t count, loff_t *ppos);
+
+#define DEBUGFS_READ_FILE_OPS(name)                                     \
+        DEBUGFS_READ_FUNC(name)                                         \
+        static const struct file_operations iwmct_dbgfs_##name##_ops = {  \
+                .read = iwmct_dbgfs_##name##_read,                      \
+                .open = iwmct_dbgfs_open_file_generic,                  \
+                .llseek = generic_file_llseek,                          \
+        };
+
+#define DEBUGFS_WRITE_FILE_OPS(name)                                    \
+        DEBUGFS_WRITE_FUNC(name)                                        \
+        static const struct file_operations iwmct_dbgfs_##name##_ops = {  \
+                .write = iwmct_dbgfs_##name##_write,                    \
+                .open = iwmct_dbgfs_open_file_generic,                  \
+                .llseek = generic_file_llseek,                          \
+        };
+
+#define DEBUGFS_READ_WRITE_FILE_OPS(name)                               \
+        DEBUGFS_READ_FUNC(name)                                         \
+        DEBUGFS_WRITE_FUNC(name)                                        \
+        static const struct file_operations iwmct_dbgfs_##name##_ops = {\
+                .write = iwmct_dbgfs_##name##_write,                    \
+                .read = iwmct_dbgfs_##name##_read,                      \
+                .open = iwmct_dbgfs_open_file_generic,                  \
+                .llseek = generic_file_llseek,                          \
+        };
+
+
+/*      Debugfs file ops definitions        */
+
+/*
+ * Create the debugfs files and directories
+ *
+ */
+void iwmct_dbgfs_register(struct iwmct_priv *priv, const char *name)
+{
+        struct iwmct_debugfs *dbgfs;
+
+        dbgfs = kzalloc(sizeof(struct iwmct_debugfs), GFP_KERNEL);
+        if (!dbgfs) {
+                LOG_ERROR(priv, DEBUGFS, "failed to allocate %zd bytes\n",
+                                        sizeof(struct iwmct_debugfs));
+                return;
+        }
+
+        priv->dbgfs = dbgfs;
+        dbgfs->name = name;
+        dbgfs->dir_drv = debugfs_create_dir(name, NULL);
+        if (!dbgfs->dir_drv) {
+                LOG_ERROR(priv, DEBUGFS, "failed to create debugfs dir\n");
+                return;
+        }
+
+        return;
+}
+
+/**
+ * Remove the debugfs files and directories
+ *
+ */
+void iwmct_dbgfs_unregister(struct iwmct_debugfs *dbgfs)
+{
+        if (!dbgfs)
+                return;
+
+        DEBUGFS_RM(dbgfs->dir_drv);
+        kfree(dbgfs);
+        dbgfs = NULL;
+}
+
diff --git a/drivers/misc/iwmc3200top/debugfs.h b/drivers/misc/iwmc3200top/debugfs.h
new file mode 100644
index 00000000000..71d45759b40
--- /dev/null
+++ b/drivers/misc/iwmc3200top/debugfs.h
@@ -0,0 +1,58 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/debufs.h
+ *
+ * Copyright (C) 2009 Intel 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#ifndef __DEBUGFS_H__
+#define __DEBUGFS_H__
+
+
+#ifdef CONFIG_IWMC3200TOP_DEBUGFS
+
+struct iwmct_debugfs {
+        const char *name;
+        struct dentry *dir_drv;
+        struct dir_drv_files {
+        } dbgfs_drv_files;
+};
+
+void iwmct_dbgfs_register(struct iwmct_priv *priv, const char *name);
+void iwmct_dbgfs_unregister(struct iwmct_debugfs *dbgfs);
+
+#else /* CONFIG_IWMC3200TOP_DEBUGFS */
+
+struct iwmct_debugfs;
+
+static inline void
+iwmct_dbgfs_register(struct iwmct_priv *priv, const char *name)
+{}
+
+static inline void
+iwmct_dbgfs_unregister(struct iwmct_debugfs *dbgfs)
+{}
+
+#endif /* CONFIG_IWMC3200TOP_DEBUGFS */
+
+#endif /* __DEBUGFS_H__ */
+
diff --git a/drivers/misc/iwmc3200top/fw-download.c b/drivers/misc/iwmc3200top/fw-download.c
new file mode 100644
index 00000000000..e27afde6e99
--- /dev/null
+++ b/drivers/misc/iwmc3200top/fw-download.c
@@ -0,0 +1,358 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/fw-download.c
+ *
+ * Copyright (C) 2009 Intel 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#include <linux/firmware.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/slab.h>
+#include <asm/unaligned.h>
+
+#include "iwmc3200top.h"
+#include "log.h"
+#include "fw-msg.h"
+
+#define CHECKSUM_BYTES_NUM sizeof(u32)
+
+/**
+  init parser struct with file
+ */
+static int iwmct_fw_parser_init(struct iwmct_priv *priv, const u8 *file,
+                              size_t file_size, size_t block_size)
+{
+        struct iwmct_parser *parser = &priv->parser;
+        struct iwmct_fw_hdr *fw_hdr = &parser->versions;
+
+        LOG_TRACE(priv, FW_DOWNLOAD, "-->\n");
+
+        LOG_INFO(priv, FW_DOWNLOAD, "file_size=%zd\n", file_size);
+
+        parser->file = file;
+        parser->file_size = file_size;
+        parser->cur_pos = 0;
+        parser->entry_point = 0;
+        parser->buf = kzalloc(block_size, GFP_KERNEL);
+        if (!parser->buf) {
+                LOG_ERROR(priv, FW_DOWNLOAD, "kzalloc error\n");
+                return -ENOMEM;
+        }
+        parser->buf_size = block_size;
+
+        /* extract fw versions */
+        memcpy(fw_hdr, parser->file, sizeof(struct iwmct_fw_hdr));
+        LOG_INFO(priv, FW_DOWNLOAD, "fw versions are:\n"
+                "top %u.%u.%u gps %u.%u.%u bt %u.%u.%u tic %s\n",
+                fw_hdr->top_major, fw_hdr->top_minor, fw_hdr->top_revision,
+                fw_hdr->gps_major, fw_hdr->gps_minor, fw_hdr->gps_revision,
+                fw_hdr->bt_major, fw_hdr->bt_minor, fw_hdr->bt_revision,
+                fw_hdr->tic_name);
+
+        parser->cur_pos += sizeof(struct iwmct_fw_hdr);
+
+        LOG_TRACE(priv, FW_DOWNLOAD, "<--\n");
+        return 0;
+}
+
+static bool iwmct_checksum(struct iwmct_priv *priv)
+{
+        struct iwmct_parser *parser = &priv->parser;
+        __le32 *file = (__le32 *)parser->file;
+        int i, pad, steps;
+        u32 accum = 0;
+        u32 checksum;
+        u32 mask = 0xffffffff;
+
+        pad = (parser->file_size - CHECKSUM_BYTES_NUM) % 4;
+        steps =  (parser->file_size - CHECKSUM_BYTES_NUM) / 4;
+
+        LOG_INFO(priv, FW_DOWNLOAD, "pad=%d steps=%d\n", pad, steps);
+
+        for (i = 0; i < steps; i++)
+                accum += le32_to_cpu(file[i]);
+
+        if (pad) {
+                mask <<= 8 * (4 - pad);
+                accum += le32_to_cpu(file[steps]) & mask;
+        }
+
+        checksum = get_unaligned_le32((__le32 *)(parser->file +
+                        parser->file_size - CHECKSUM_BYTES_NUM));
+
+        LOG_INFO(priv, FW_DOWNLOAD,
+                "compare checksum accum=0x%x to checksum=0x%x\n",
+                accum, checksum);
+
+        return checksum == accum;
+}
+
+static int iwmct_parse_next_section(struct iwmct_priv *priv, const u8 **p_sec,
+                                  size_t *sec_size, __le32 *sec_addr)
+{
+        struct iwmct_parser *parser = &priv->parser;
+        struct iwmct_dbg *dbg = &priv->dbg;
+        struct iwmct_fw_sec_hdr *sec_hdr;
+
+        LOG_TRACE(priv, FW_DOWNLOAD, "-->\n");
+
+        while (parser->cur_pos + sizeof(struct iwmct_fw_sec_hdr)
+                <= parser->file_size) {
+
+                sec_hdr = (struct iwmct_fw_sec_hdr *)
+                                (parser->file + parser->cur_pos);
+                parser->cur_pos += sizeof(struct iwmct_fw_sec_hdr);
+
+                LOG_INFO(priv, FW_DOWNLOAD,
+                        "sec hdr: type=%s addr=0x%x size=%d\n",
+                        sec_hdr->type, sec_hdr->target_addr,
+                        sec_hdr->data_size);
+
+                if (strcmp(sec_hdr->type, "ENT") == 0)
+                        parser->entry_point = le32_to_cpu(sec_hdr->target_addr);
+                else if (strcmp(sec_hdr->type, "LBL") == 0)
+                        strcpy(dbg->label_fw, parser->file + parser->cur_pos);
+                else if (((strcmp(sec_hdr->type, "TOP") == 0) &&
+                          (priv->barker & BARKER_DNLOAD_TOP_MSK)) ||
+                         ((strcmp(sec_hdr->type, "GPS") == 0) &&
+                          (priv->barker & BARKER_DNLOAD_GPS_MSK)) ||
+                         ((strcmp(sec_hdr->type, "BTH") == 0) &&
+                          (priv->barker & BARKER_DNLOAD_BT_MSK))) {
+                        *sec_addr = sec_hdr->target_addr;
+                        *sec_size = le32_to_cpu(sec_hdr->data_size);
+                        *p_sec = parser->file + parser->cur_pos;
+                        parser->cur_pos += le32_to_cpu(sec_hdr->data_size);
+                        return 1;
+                } else if (strcmp(sec_hdr->type, "LOG") != 0)
+                        LOG_WARNING(priv, FW_DOWNLOAD,
+                                    "skipping section type %s\n",
+                                    sec_hdr->type);
+
+                parser->cur_pos += le32_to_cpu(sec_hdr->data_size);
+                LOG_INFO(priv, FW_DOWNLOAD,
+                        "finished with section cur_pos=%zd\n", parser->cur_pos);
+        }
+
+        LOG_TRACE(priv, INIT, "<--\n");
+        return 0;
+}
+
+static int iwmct_download_section(struct iwmct_priv *priv, const u8 *p_sec,
+                                size_t sec_size, __le32 addr)
+{
+        struct iwmct_parser *parser = &priv->parser;
+        struct iwmct_fw_load_hdr *hdr = (struct iwmct_fw_load_hdr *)parser->buf;
+        const u8 *cur_block = p_sec;
+        size_t sent = 0;
+        int cnt = 0;
+        int ret = 0;
+        u32 cmd = 0;
+
+        LOG_TRACE(priv, FW_DOWNLOAD, "-->\n");
+        LOG_INFO(priv, FW_DOWNLOAD, "Download address 0x%x size 0x%zx\n",
+                                addr, sec_size);
+
+        while (sent < sec_size) {
+                int i;
+                u32 chksm = 0;
+                u32 reset = atomic_read(&priv->reset);
+                /* actual FW data */
+                u32 data_size = min(parser->buf_size - sizeof(*hdr),
+                                    sec_size - sent);
+                /* Pad to block size */
+                u32 trans_size = (data_size + sizeof(*hdr) +
+                                  IWMC_SDIO_BLK_SIZE - 1) &
+                                  ~(IWMC_SDIO_BLK_SIZE - 1);
+                ++cnt;
+
+                /* in case of reset, interrupt FW DOWNLAOD */
+                if (reset) {
+                        LOG_INFO(priv, FW_DOWNLOAD,
+                                 "Reset detected. Abort FW download!!!");
+                        ret = -ECANCELED;
+                        goto exit;
+                }
+
+                memset(parser->buf, 0, parser->buf_size);
+                cmd |= IWMC_OPCODE_WRITE << CMD_HDR_OPCODE_POS;
+                cmd |= IWMC_CMD_SIGNATURE << CMD_HDR_SIGNATURE_POS;
+                cmd |= (priv->dbg.direct ? 1 : 0) << CMD_HDR_DIRECT_ACCESS_POS;
+                cmd |= (priv->dbg.checksum ? 1 : 0) << CMD_HDR_USE_CHECKSUM_POS;
+                hdr->data_size = cpu_to_le32(data_size);
+                hdr->target_addr = addr;
+
+                /* checksum is allowed for sizes divisible by 4 */
+                if (data_size & 0x3)
+                        cmd &= ~CMD_HDR_USE_CHECKSUM_MSK;
+
+                memcpy(hdr->data, cur_block, data_size);
+
+
+                if (cmd & CMD_HDR_USE_CHECKSUM_MSK) {
+
+                        chksm = data_size + le32_to_cpu(addr) + cmd;
+                        for (i = 0; i < data_size >> 2; i++)
+                                chksm += ((u32 *)cur_block)[i];
+
+                        hdr->block_chksm = cpu_to_le32(chksm);
+                        LOG_INFO(priv, FW_DOWNLOAD, "Checksum = 0x%X\n",
+                                 hdr->block_chksm);
+                }
+
+                LOG_INFO(priv, FW_DOWNLOAD, "trans#%d, len=%d, sent=%zd, "
+                                "sec_size=%zd, startAddress 0x%X\n",
+                                cnt, trans_size, sent, sec_size, addr);
+
+                if (priv->dbg.dump)
+                        LOG_HEXDUMP(FW_DOWNLOAD, parser->buf, trans_size);
+
+
+                hdr->cmd = cpu_to_le32(cmd);
+                /* send it down */
+                /* TODO: add more proper sending and error checking */
+                ret = iwmct_tx(priv, parser->buf, trans_size);
+                if (ret != 0) {
+                        LOG_INFO(priv, FW_DOWNLOAD,
+                                "iwmct_tx returned %d\n", ret);
+                        goto exit;
+                }
+
+                addr = cpu_to_le32(le32_to_cpu(addr) + data_size);
+                sent += data_size;
+                cur_block = p_sec + sent;
+
+                if (priv->dbg.blocks && (cnt + 1) >= priv->dbg.blocks) {
+                        LOG_INFO(priv, FW_DOWNLOAD,
+                                "Block number limit is reached [%d]\n",
+                                priv->dbg.blocks);
+                        break;
+                }
+        }
+
+        if (sent < sec_size)
+                ret = -EINVAL;
+exit:
+        LOG_TRACE(priv, FW_DOWNLOAD, "<--\n");
+        return ret;
+}
+
+static int iwmct_kick_fw(struct iwmct_priv *priv, bool jump)
+{
+        struct iwmct_parser *parser = &priv->parser;
+        struct iwmct_fw_load_hdr *hdr = (struct iwmct_fw_load_hdr *)parser->buf;
+        int ret;
+        u32 cmd;
+
+        LOG_TRACE(priv, FW_DOWNLOAD, "-->\n");
+
+        memset(parser->buf, 0, parser->buf_size);
+        cmd = IWMC_CMD_SIGNATURE << CMD_HDR_SIGNATURE_POS;
+        if (jump) {
+                cmd |= IWMC_OPCODE_JUMP << CMD_HDR_OPCODE_POS;
+                hdr->target_addr = cpu_to_le32(parser->entry_point);
+                LOG_INFO(priv, FW_DOWNLOAD, "jump address 0x%x\n",
+                                parser->entry_point);
+        } else {
+                cmd |= IWMC_OPCODE_LAST_COMMAND << CMD_HDR_OPCODE_POS;
+                LOG_INFO(priv, FW_DOWNLOAD, "last command\n");
+        }
+
+        hdr->cmd = cpu_to_le32(cmd);
+
+        LOG_HEXDUMP(FW_DOWNLOAD, parser->buf, sizeof(*hdr));
+        /* send it down */
+        /* TODO: add more proper sending and error checking */
+        ret = iwmct_tx(priv, parser->buf, IWMC_SDIO_BLK_SIZE);
+        if (ret)
+                LOG_INFO(priv, FW_DOWNLOAD, "iwmct_tx returned %d", ret);
+
+        LOG_TRACE(priv, FW_DOWNLOAD, "<--\n");
+        return 0;
+}
+
+int iwmct_fw_load(struct iwmct_priv *priv)
+{
+        const u8 *fw_name = FW_NAME(FW_API_VER);
+        const struct firmware *raw;
+        const u8 *pdata;
+        size_t len;
+        __le32 addr;
+        int ret;
+
+
+        LOG_INFO(priv, FW_DOWNLOAD, "barker download request 0x%x is:\n",
+                        priv->barker);
+        LOG_INFO(priv, FW_DOWNLOAD, "*******  Top FW %s requested ********\n",
+                        (priv->barker & BARKER_DNLOAD_TOP_MSK) ? "was" : "not");
+        LOG_INFO(priv, FW_DOWNLOAD, "*******  GPS FW %s requested ********\n",
+                        (priv->barker & BARKER_DNLOAD_GPS_MSK) ? "was" : "not");
+        LOG_INFO(priv, FW_DOWNLOAD, "*******  BT FW %s requested ********\n",
+                        (priv->barker & BARKER_DNLOAD_BT_MSK) ? "was" : "not");
+
+
+        /* get the firmware */
+        ret = request_firmware(&raw, fw_name, &priv->func->dev);
+        if (ret < 0) {
+                LOG_ERROR(priv, FW_DOWNLOAD, "%s request_firmware failed %d\n",
+                          fw_name, ret);
+                goto exit;
+        }
+
+        if (raw->size < sizeof(struct iwmct_fw_sec_hdr)) {
+                LOG_ERROR(priv, FW_DOWNLOAD, "%s smaller then (%zd) (%zd)\n",
+                          fw_name, sizeof(struct iwmct_fw_sec_hdr), raw->size);
+                goto exit;
+        }
+
+        LOG_INFO(priv, FW_DOWNLOAD, "Read firmware '%s'\n", fw_name);
+
+        /* clear parser struct */
+        ret = iwmct_fw_parser_init(priv, raw->data, raw->size, priv->trans_len);
+        if (ret < 0) {
+                LOG_ERROR(priv, FW_DOWNLOAD,
+                          "iwmct_parser_init failed: Reason %d\n", ret);
+                goto exit;
+        }
+
+        if (!iwmct_checksum(priv)) {
+                LOG_ERROR(priv, FW_DOWNLOAD, "checksum error\n");
+                ret = -EINVAL;
+                goto exit;
+        }
+
+        /* download firmware to device */
+        while (iwmct_parse_next_section(priv, &pdata, &len, &addr)) {
+                ret = iwmct_download_section(priv, pdata, len, addr);
+                if (ret) {
+                        LOG_ERROR(priv, FW_DOWNLOAD,
+                                  "%s download section failed\n", fw_name);
+                        goto exit;
+                }
+        }
+
+        ret = iwmct_kick_fw(priv, !!(priv->barker & BARKER_DNLOAD_JUMP_MSK));
+
+exit:
+        kfree(priv->parser.buf);
+        release_firmware(raw);
+        return ret;
+}
diff --git a/drivers/misc/iwmc3200top/fw-msg.h b/drivers/misc/iwmc3200top/fw-msg.h
new file mode 100644
index 00000000000..9e26b75bd48
--- /dev/null
+++ b/drivers/misc/iwmc3200top/fw-msg.h
@@ -0,0 +1,113 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/fw-msg.h
+ *
+ * Copyright (C) 2009 Intel 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#ifndef __FWMSG_H__
+#define __FWMSG_H__
+
+#define COMM_TYPE_D2H                   0xFF
+#define COMM_TYPE_H2D                   0xEE
+
+#define COMM_CATEGORY_OPERATIONAL       0x00
+#define COMM_CATEGORY_DEBUG             0x01
+#define COMM_CATEGORY_TESTABILITY       0x02
+#define COMM_CATEGORY_DIAGNOSTICS       0x03
+
+#define OP_DBG_ZSTR_MSG                 cpu_to_le16(0x1A)
+
+#define FW_LOG_SRC_MAX                  32
+#define FW_LOG_SRC_ALL                  255
+
+#define FW_STRING_TABLE_ADDR            cpu_to_le32(0x0C000000)
+
+#define CMD_DBG_LOG_LEVEL               cpu_to_le16(0x0001)
+#define CMD_TST_DEV_RESET               cpu_to_le16(0x0060)
+#define CMD_TST_FUNC_RESET              cpu_to_le16(0x0062)
+#define CMD_TST_IFACE_RESET             cpu_to_le16(0x0064)
+#define CMD_TST_CPU_UTILIZATION         cpu_to_le16(0x0065)
+#define CMD_TST_TOP_DEEP_SLEEP          cpu_to_le16(0x0080)
+#define CMD_TST_WAKEUP                  cpu_to_le16(0x0081)
+#define CMD_TST_FUNC_WAKEUP             cpu_to_le16(0x0082)
+#define CMD_TST_FUNC_DEEP_SLEEP_REQUEST cpu_to_le16(0x0083)
+#define CMD_TST_GET_MEM_DUMP            cpu_to_le16(0x0096)
+
+#define OP_OPR_ALIVE                    cpu_to_le16(0x0010)
+#define OP_OPR_CMD_ACK                  cpu_to_le16(0x001F)
+#define OP_OPR_CMD_NACK                 cpu_to_le16(0x0020)
+#define OP_TST_MEM_DUMP                 cpu_to_le16(0x0043)
+
+#define CMD_FLAG_PADDING_256            0x80
+
+#define FW_HCMD_BLOCK_SIZE              256
+
+struct msg_hdr {
+        u8 type;
+        u8 category;
+        __le16 opcode;
+        u8 seqnum;
+        u8 flags;
+        __le16 length;
+} __attribute__((__packed__));
+
+struct log_hdr {
+        __le32 timestamp;
+        u8 severity;
+        u8 logsource;
+        __le16 reserved;
+} __attribute__((__packed__));
+
+struct mdump_hdr {
+        u8 dmpid;
+        u8 frag;
+        __le16 size;
+        __le32 addr;
+} __attribute__((__packed__));
+
+struct top_msg {
+        struct msg_hdr hdr;
+        union {
+                /* D2H messages */
+                struct {
+                        struct log_hdr log_hdr;
+                        u8 data[1];
+                } __attribute__((__packed__)) log;
+
+                struct {
+                        struct log_hdr log_hdr;
+                        struct mdump_hdr md_hdr;
+                        u8 data[1];
+                } __attribute__((__packed__)) mdump;
+
+                /* H2D messages */
+                struct {
+                        u8 logsource;
+                        u8 sevmask;
+                } __attribute__((__packed__)) logdefs[FW_LOG_SRC_MAX];
+                struct mdump_hdr mdump_req;
+        } u;
+} __attribute__((__packed__));
+
+
+#endif /* __FWMSG_H__ */
diff --git a/drivers/misc/iwmc3200top/iwmc3200top.h b/drivers/misc/iwmc3200top/iwmc3200top.h
new file mode 100644
index 00000000000..620973ed8bf
--- /dev/null
+++ b/drivers/misc/iwmc3200top/iwmc3200top.h
@@ -0,0 +1,205 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/iwmc3200top.h
+ *
+ * Copyright (C) 2009 Intel 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#ifndef __IWMC3200TOP_H__
+#define __IWMC3200TOP_H__
+
+#include <linux/workqueue.h>
+
+#define DRV_NAME "iwmc3200top"
+#define FW_API_VER 1
+#define _FW_NAME(api) DRV_NAME "." #api ".fw"
+#define FW_NAME(api) _FW_NAME(api)
+
+#define IWMC_SDIO_BLK_SIZE                      256
+#define IWMC_DEFAULT_TR_BLK                     64
+#define IWMC_SDIO_DATA_ADDR                     0x0
+#define IWMC_SDIO_INTR_ENABLE_ADDR              0x14
+#define IWMC_SDIO_INTR_STATUS_ADDR              0x13
+#define IWMC_SDIO_INTR_CLEAR_ADDR               0x13
+#define IWMC_SDIO_INTR_GET_SIZE_ADDR            0x2C
+
+#define COMM_HUB_HEADER_LENGTH 16
+#define LOGGER_HEADER_LENGTH   10
+
+
+#define BARKER_DNLOAD_BT_POS            0
+#define BARKER_DNLOAD_BT_MSK            BIT(BARKER_DNLOAD_BT_POS)
+#define BARKER_DNLOAD_GPS_POS           1
+#define BARKER_DNLOAD_GPS_MSK           BIT(BARKER_DNLOAD_GPS_POS)
+#define BARKER_DNLOAD_TOP_POS           2
+#define BARKER_DNLOAD_TOP_MSK           BIT(BARKER_DNLOAD_TOP_POS)
+#define BARKER_DNLOAD_RESERVED1_POS     3
+#define BARKER_DNLOAD_RESERVED1_MSK     BIT(BARKER_DNLOAD_RESERVED1_POS)
+#define BARKER_DNLOAD_JUMP_POS          4
+#define BARKER_DNLOAD_JUMP_MSK          BIT(BARKER_DNLOAD_JUMP_POS)
+#define BARKER_DNLOAD_SYNC_POS          5
+#define BARKER_DNLOAD_SYNC_MSK          BIT(BARKER_DNLOAD_SYNC_POS)
+#define BARKER_DNLOAD_RESERVED2_POS     6
+#define BARKER_DNLOAD_RESERVED2_MSK     (0x3 << BARKER_DNLOAD_RESERVED2_POS)
+#define BARKER_DNLOAD_BARKER_POS        8
+#define BARKER_DNLOAD_BARKER_MSK        (0xffffff << BARKER_DNLOAD_BARKER_POS)
+
+#define IWMC_BARKER_REBOOT      (0xdeadbe << BARKER_DNLOAD_BARKER_POS)
+/* whole field barker */
+#define IWMC_BARKER_ACK         0xfeedbabe
+
+#define IWMC_CMD_SIGNATURE      0xcbbc
+
+#define CMD_HDR_OPCODE_POS              0
+#define CMD_HDR_OPCODE_MSK_MSK          (0xf << CMD_HDR_OPCODE_MSK_POS)
+#define CMD_HDR_RESPONSE_CODE_POS       4
+#define CMD_HDR_RESPONSE_CODE_MSK       (0xf << CMD_HDR_RESPONSE_CODE_POS)
+#define CMD_HDR_USE_CHECKSUM_POS        8
+#define CMD_HDR_USE_CHECKSUM_MSK        BIT(CMD_HDR_USE_CHECKSUM_POS)
+#define CMD_HDR_RESPONSE_REQUIRED_POS   9
+#define CMD_HDR_RESPONSE_REQUIRED_MSK   BIT(CMD_HDR_RESPONSE_REQUIRED_POS)
+#define CMD_HDR_DIRECT_ACCESS_POS       10
+#define CMD_HDR_DIRECT_ACCESS_MSK       BIT(CMD_HDR_DIRECT_ACCESS_POS)
+#define CMD_HDR_RESERVED_POS            11
+#define CMD_HDR_RESERVED_MSK            BIT(0x1f << CMD_HDR_RESERVED_POS)
+#define CMD_HDR_SIGNATURE_POS           16
+#define CMD_HDR_SIGNATURE_MSK           BIT(0xffff << CMD_HDR_SIGNATURE_POS)
+
+enum {
+        IWMC_OPCODE_PING = 0,
+        IWMC_OPCODE_READ = 1,
+        IWMC_OPCODE_WRITE = 2,
+        IWMC_OPCODE_JUMP = 3,
+        IWMC_OPCODE_REBOOT = 4,
+        IWMC_OPCODE_PERSISTENT_WRITE = 5,
+        IWMC_OPCODE_PERSISTENT_READ = 6,
+        IWMC_OPCODE_READ_MODIFY_WRITE = 7,
+        IWMC_OPCODE_LAST_COMMAND = 15
+};
+
+struct iwmct_fw_load_hdr {
+        __le32 cmd;
+        __le32 target_addr;
+        __le32 data_size;
+        __le32 block_chksm;
+        u8 data[0];
+};
+
+/**
+ * struct iwmct_fw_hdr
+ * holds all sw components versions
+ */
+struct iwmct_fw_hdr {
+        u8 top_major;
+        u8 top_minor;
+        u8 top_revision;
+        u8 gps_major;
+        u8 gps_minor;
+        u8 gps_revision;
+        u8 bt_major;
+        u8 bt_minor;
+        u8 bt_revision;
+        u8 tic_name[31];
+};
+
+/**
+ * struct iwmct_fw_sec_hdr
+ * @type: function type
+ * @data_size: section's data size
+ * @target_addr: download address
+ */
+struct iwmct_fw_sec_hdr {
+        u8 type[4];
+        __le32 data_size;
+        __le32 target_addr;
+};
+
+/**
+ * struct iwmct_parser
+ * @file: fw image
+ * @file_size: fw size
+ * @cur_pos: position in file
+ * @buf: temp buf for download
+ * @buf_size: size of buf
+ * @entry_point: address to jump in fw kick-off
+ */
+struct iwmct_parser {
+        const u8 *file;
+        size_t file_size;
+        size_t cur_pos;
+        u8 *buf;
+        size_t buf_size;
+        u32 entry_point;
+        struct iwmct_fw_hdr versions;
+};
+
+
+struct iwmct_work_struct {
+        struct list_head list;
+        ssize_t iosize;
+};
+
+struct iwmct_dbg {
+        int blocks;
+        bool dump;
+        bool jump;
+        bool direct;
+        bool checksum;
+        bool fw_download;
+        int block_size;
+        int download_trans_blks;
+
+        char label_fw[256];
+};
+
+struct iwmct_debugfs;
+
+struct iwmct_priv {
+        struct sdio_func *func;
+        struct iwmct_debugfs *dbgfs;
+        struct iwmct_parser parser;
+        atomic_t reset;
+        atomic_t dev_sync;
+        u32 trans_len;
+        u32 barker;
+        struct iwmct_dbg dbg;
+
+        /* drivers work items */
+        struct work_struct bus_rescan_worker;
+        struct work_struct isr_worker;
+
+        /* drivers wait queue */
+        wait_queue_head_t wait_q;
+
+        /* rx request list */
+        struct list_head read_req_list;
+};
+
+extern int iwmct_tx(struct iwmct_priv *priv, void *src, int count);
+extern int iwmct_fw_load(struct iwmct_priv *priv);
+
+extern void iwmct_dbg_init_params(struct iwmct_priv *drv);
+extern void iwmct_dbg_init_drv_attrs(struct device_driver *drv);
+extern void iwmct_dbg_remove_drv_attrs(struct device_driver *drv);
+extern int iwmct_send_hcmd(struct iwmct_priv *priv, u8 *cmd, u16 len);
+
+#endif  /*  __IWMC3200TOP_H__  */
diff --git a/drivers/misc/iwmc3200top/log.c b/drivers/misc/iwmc3200top/log.c
new file mode 100644
index 00000000000..a36a55a49ca
--- /dev/null
+++ b/drivers/misc/iwmc3200top/log.c
@@ -0,0 +1,348 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/log.c
+ *
+ * Copyright (C) 2009 Intel 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include "fw-msg.h"
+#include "iwmc3200top.h"
+#include "log.h"
+
+/* Maximal hexadecimal string size of the FW memdump message */
+#define LOG_MSG_SIZE_MAX                12400
+
+/* iwmct_logdefs is a global used by log macros */
+u8 iwmct_logdefs[LOG_SRC_MAX];
+static u8 iwmct_fw_logdefs[FW_LOG_SRC_MAX];
+
+
+static int _log_set_log_filter(u8 *logdefs, int size, u8 src, u8 logmask)
+{
+        int i;
+
+        if (src < size)
+                logdefs[src] = logmask;
+        else if (src == LOG_SRC_ALL)
+                for (i = 0; i < size; i++)
+                        logdefs[i] = logmask;
+        else
+                return -1;
+
+        return 0;
+}
+
+
+int iwmct_log_set_filter(u8 src, u8 logmask)
+{
+        return _log_set_log_filter(iwmct_logdefs, LOG_SRC_MAX, src, logmask);
+}
+
+
+int iwmct_log_set_fw_filter(u8 src, u8 logmask)
+{
+        return _log_set_log_filter(iwmct_fw_logdefs,
+                                   FW_LOG_SRC_MAX, src, logmask);
+}
+
+
+static int log_msg_format_hex(char *str, int slen, u8 *ibuf,
+                              int ilen, char *pref)
+{
+        int pos = 0;
+        int i;
+        int len;
+
+        for (pos = 0, i = 0; pos < slen - 2 && pref[i] != '\0'; i++, pos++)
+                str[pos] = pref[i];
+
+        for (i = 0; pos < slen - 2 && i < ilen; pos += len, i++)
+                len = snprintf(&str[pos], slen - pos - 1, " %2.2X", ibuf[i]);
+
+        if (i < ilen)
+                return -1;
+
+        return 0;
+}
+
+/*      NOTE: This function is not thread safe.
+        Currently it's called only from sdio rx worker - no race there
+*/
+void iwmct_log_top_message(struct iwmct_priv *priv, u8 *buf, int len)
+{
+        struct top_msg *msg;
+        static char logbuf[LOG_MSG_SIZE_MAX];
+
+        msg = (struct top_msg *)buf;
+
+        if (len < sizeof(msg->hdr) + sizeof(msg->u.log.log_hdr)) {
+                LOG_ERROR(priv, FW_MSG, "Log message from TOP "
+                          "is too short %d (expected %zd)\n",
+                          len, sizeof(msg->hdr) + sizeof(msg->u.log.log_hdr));
+                return;
+        }
+
+        if (!(iwmct_fw_logdefs[msg->u.log.log_hdr.logsource] &
+                BIT(msg->u.log.log_hdr.severity)) ||
+            !(iwmct_logdefs[LOG_SRC_FW_MSG] & BIT(msg->u.log.log_hdr.severity)))
+                return;
+
+        switch (msg->hdr.category) {
+        case COMM_CATEGORY_TESTABILITY:
+                if (!(iwmct_logdefs[LOG_SRC_TST] &
+                      BIT(msg->u.log.log_hdr.severity)))
+                        return;
+                if (log_msg_format_hex(logbuf, LOG_MSG_SIZE_MAX, buf,
+                                       le16_to_cpu(msg->hdr.length) +
+                                       sizeof(msg->hdr), "<TST>"))
+                        LOG_WARNING(priv, TST,
+                                  "TOP TST message is too long, truncating...");
+                LOG_WARNING(priv, TST, "%s\n", logbuf);
+                break;
+        case COMM_CATEGORY_DEBUG:
+                if (msg->hdr.opcode == OP_DBG_ZSTR_MSG)
+                        LOG_INFO(priv, FW_MSG, "%s %s", "<DBG>",
+                                       ((u8 *)msg) + sizeof(msg->hdr)
+                                        + sizeof(msg->u.log.log_hdr));
+                else {
+                        if (log_msg_format_hex(logbuf, LOG_MSG_SIZE_MAX, buf,
+                                        le16_to_cpu(msg->hdr.length)
+                                                + sizeof(msg->hdr),
+                                        "<DBG>"))
+                                LOG_WARNING(priv, FW_MSG,
+                                        "TOP DBG message is too long,"
+                                        "truncating...");
+                        LOG_WARNING(priv, FW_MSG, "%s\n", logbuf);
+                }
+                break;
+        default:
+                break;
+        }
+}
+
+static int _log_get_filter_str(u8 *logdefs, int logdefsz, char *buf, int size)
+{
+        int i, pos, len;
+        for (i = 0, pos = 0; (pos < size-1) && (i < logdefsz); i++) {
+                len = snprintf(&buf[pos], size - pos - 1, "0x%02X%02X,",
+                                i, logdefs[i]);
+                pos += len;
+        }
+        buf[pos-1] = '\n';
+        buf[pos] = '\0';
+
+        if (i < logdefsz)
+                return -1;
+        return 0;
+}
+
+int log_get_filter_str(char *buf, int size)
+{
+        return _log_get_filter_str(iwmct_logdefs, LOG_SRC_MAX, buf, size);
+}
+
+int log_get_fw_filter_str(char *buf, int size)
+{
+        return _log_get_filter_str(iwmct_fw_logdefs, FW_LOG_SRC_MAX, buf, size);
+}
+
+#define HEXADECIMAL_RADIX       16
+#define LOG_SRC_FORMAT          7 /* log level is in format of "0xXXXX," */
+
+ssize_t show_iwmct_log_level(struct device *d,
+                                struct device_attribute *attr, char *buf)
+{
+        struct iwmct_priv *priv = dev_get_drvdata(d);
+        char *str_buf;
+        int buf_size;
+        ssize_t ret;
+
+        buf_size = (LOG_SRC_FORMAT * LOG_SRC_MAX) + 1;
+        str_buf = kzalloc(buf_size, GFP_KERNEL);
+        if (!str_buf) {
+                LOG_ERROR(priv, DEBUGFS,
+                        "failed to allocate %d bytes\n", buf_size);
+                ret = -ENOMEM;
+                goto exit;
+        }
+
+        if (log_get_filter_str(str_buf, buf_size) < 0) {
+                ret = -EINVAL;
+                goto exit;
+        }
+
+        ret = sprintf(buf, "%s", str_buf);
+
+exit:
+        kfree(str_buf);
+        return ret;
+}
+
+ssize_t store_iwmct_log_level(struct device *d,
+                        struct device_attribute *attr,
+                        const char *buf, size_t count)
+{
+        struct iwmct_priv *priv = dev_get_drvdata(d);
+        char *token, *str_buf = NULL;
+        long val;
+        ssize_t ret = count;
+        u8 src, mask;
+
+        if (!count)
+                goto exit;
+
+        str_buf = kzalloc(count, GFP_KERNEL);
+        if (!str_buf) {
+                LOG_ERROR(priv, DEBUGFS,
+                        "failed to allocate %zd bytes\n", count);
+                ret = -ENOMEM;
+                goto exit;
+        }
+
+        memcpy(str_buf, buf, count);
+
+        while ((token = strsep(&str_buf, ",")) != NULL) {
+                while (isspace(*token))
+                        ++token;
+                if (strict_strtol(token, HEXADECIMAL_RADIX, &val)) {
+                        LOG_ERROR(priv, DEBUGFS,
+                                  "failed to convert string to long %s\n",
+                                  token);
+                        ret = -EINVAL;
+                        goto exit;
+                }
+
+                mask  = val & 0xFF;
+                src = (val & 0XFF00) >> 8;
+                iwmct_log_set_filter(src, mask);
+        }
+
+exit:
+        kfree(str_buf);
+        return ret;
+}
+
+ssize_t show_iwmct_log_level_fw(struct device *d,
+                        struct device_attribute *attr, char *buf)
+{
+        struct iwmct_priv *priv = dev_get_drvdata(d);
+        char *str_buf;
+        int buf_size;
+        ssize_t ret;
+
+        buf_size = (LOG_SRC_FORMAT * FW_LOG_SRC_MAX) + 2;
+
+        str_buf = kzalloc(buf_size, GFP_KERNEL);
+        if (!str_buf) {
+                LOG_ERROR(priv, DEBUGFS,
+                        "failed to allocate %d bytes\n", buf_size);
+                ret = -ENOMEM;
+                goto exit;
+        }
+
+        if (log_get_fw_filter_str(str_buf, buf_size) < 0) {
+                ret = -EINVAL;
+                goto exit;
+        }
+
+        ret = sprintf(buf, "%s", str_buf);
+
+exit:
+        kfree(str_buf);
+        return ret;
+}
+
+ssize_t store_iwmct_log_level_fw(struct device *d,
+                        struct device_attribute *attr,
+                        const char *buf, size_t count)
+{
+        struct iwmct_priv *priv = dev_get_drvdata(d);
+        struct top_msg cmd;
+        char *token, *str_buf = NULL;
+        ssize_t ret = count;
+        u16 cmdlen = 0;
+        int i;
+        long val;
+        u8 src, mask;
+
+        if (!count)
+                goto exit;
+
+        str_buf = kzalloc(count, GFP_KERNEL);
+        if (!str_buf) {
+                LOG_ERROR(priv, DEBUGFS,
+                        "failed to allocate %zd bytes\n", count);
+                ret = -ENOMEM;
+                goto exit;
+        }
+
+        memcpy(str_buf, buf, count);
+
+        cmd.hdr.type = COMM_TYPE_H2D;
+        cmd.hdr.category = COMM_CATEGORY_DEBUG;
+        cmd.hdr.opcode = CMD_DBG_LOG_LEVEL;
+
+        for (i = 0; ((token = strsep(&str_buf, ",")) != NULL) &&
+                     (i < FW_LOG_SRC_MAX); i++) {
+
+                while (isspace(*token))
+                        ++token;
+
+                if (strict_strtol(token, HEXADECIMAL_RADIX, &val)) {
+                        LOG_ERROR(priv, DEBUGFS,
+                                  "failed to convert string to long %s\n",
+                                  token);
+                        ret = -EINVAL;
+                        goto exit;
+                }
+
+                mask  = val & 0xFF; /* LSB */
+                src = (val & 0XFF00) >> 8; /* 2nd least significant byte. */
+                iwmct_log_set_fw_filter(src, mask);
+
+                cmd.u.logdefs[i].logsource = src;
+                cmd.u.logdefs[i].sevmask = mask;
+        }
+
+        cmd.hdr.length = cpu_to_le16(i * sizeof(cmd.u.logdefs[0]));
+        cmdlen = (i * sizeof(cmd.u.logdefs[0]) + sizeof(cmd.hdr));
+
+        ret = iwmct_send_hcmd(priv, (u8 *)&cmd, cmdlen);
+        if (ret) {
+                LOG_ERROR(priv, DEBUGFS,
+                          "Failed to send %d bytes of fwcmd, ret=%zd\n",
+                          cmdlen, ret);
+                goto exit;
+        } else
+                LOG_INFO(priv, DEBUGFS, "fwcmd sent (%d bytes)\n", cmdlen);
+
+        ret = count;
+
+exit:
+        kfree(str_buf);
+        return ret;
+}
+
diff --git a/drivers/misc/iwmc3200top/log.h b/drivers/misc/iwmc3200top/log.h
new file mode 100644
index 00000000000..4434bb16cea
--- /dev/null
+++ b/drivers/misc/iwmc3200top/log.h
@@ -0,0 +1,171 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/log.h
+ *
+ * Copyright (C) 2009 Intel 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#ifndef __LOG_H__
+#define __LOG_H__
+
+
+/* log severity:
+ * The log levels here match FW log levels
+ * so values need to stay as is */
+#define LOG_SEV_CRITICAL                0
+#define LOG_SEV_ERROR                   1
+#define LOG_SEV_WARNING                 2
+#define LOG_SEV_INFO                    3
+#define LOG_SEV_INFOEX                  4
+
+/* Log levels not defined for FW */
+#define LOG_SEV_TRACE                   5
+#define LOG_SEV_DUMP                    6
+
+#define LOG_SEV_FW_FILTER_ALL           \
+        (BIT(LOG_SEV_CRITICAL)  |       \
+         BIT(LOG_SEV_ERROR)     |       \
+         BIT(LOG_SEV_WARNING)   |       \
+         BIT(LOG_SEV_INFO)      |       \
+         BIT(LOG_SEV_INFOEX))
+
+#define LOG_SEV_FILTER_ALL              \
+        (BIT(LOG_SEV_CRITICAL)  |       \
+         BIT(LOG_SEV_ERROR)     |       \
+         BIT(LOG_SEV_WARNING)   |       \
+         BIT(LOG_SEV_INFO)      |       \
+         BIT(LOG_SEV_INFOEX)    |       \
+         BIT(LOG_SEV_TRACE)     |       \
+         BIT(LOG_SEV_DUMP))
+
+/* log source */
+#define LOG_SRC_INIT                    0
+#define LOG_SRC_DEBUGFS                 1
+#define LOG_SRC_FW_DOWNLOAD             2
+#define LOG_SRC_FW_MSG                  3
+#define LOG_SRC_TST                     4
+#define LOG_SRC_IRQ                     5
+
+#define LOG_SRC_MAX                     6
+#define LOG_SRC_ALL                     0xFF
+
+/**
+ * Default intitialization runtime log level
+ */
+#ifndef LOG_SEV_FILTER_RUNTIME
+#define LOG_SEV_FILTER_RUNTIME                  \
+        (BIT(LOG_SEV_CRITICAL)  |               \
+         BIT(LOG_SEV_ERROR)     |               \
+         BIT(LOG_SEV_WARNING))
+#endif
+
+#ifndef FW_LOG_SEV_FILTER_RUNTIME
+#define FW_LOG_SEV_FILTER_RUNTIME       LOG_SEV_FILTER_ALL
+#endif
+
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+/**
+ * Log macros
+ */
+
+#define priv2dev(priv) (&(priv->func)->dev)
+
+#define LOG_CRITICAL(priv, src, fmt, args...)                           \
+do {                                                                    \
+        if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_CRITICAL))     \
+                dev_crit(priv2dev(priv), "%s %d: " fmt,                 \
+                        __func__, __LINE__, ##args);                    \
+} while (0)
+
+#define LOG_ERROR(priv, src, fmt, args...)                              \
+do {                                                                    \
+        if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_ERROR))        \
+                dev_err(priv2dev(priv), "%s %d: " fmt,                  \
+                        __func__, __LINE__, ##args);                    \
+} while (0)
+
+#define LOG_WARNING(priv, src, fmt, args...)                            \
+do {                                                                    \
+        if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_WARNING))      \
+                dev_warn(priv2dev(priv), "%s %d: " fmt,                 \
+                         __func__, __LINE__, ##args);                   \
+} while (0)
+
+#define LOG_INFO(priv, src, fmt, args...)                               \
+do {                                                                    \
+        if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_INFO))         \
+                dev_info(priv2dev(priv), "%s %d: " fmt,                 \
+                         __func__, __LINE__, ##args);                   \
+} while (0)
+
+#define LOG_TRACE(priv, src, fmt, args...)                              \
+do {                                                                    \
+        if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_TRACE))        \
+                dev_dbg(priv2dev(priv), "%s %d: " fmt,                  \
+                         __func__, __LINE__, ##args);                   \
+} while (0)
+
+#define LOG_HEXDUMP(src, ptr, len)                                      \
+do {                                                                    \
+        if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_DUMP)) \
+                print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_NONE,        \
+                                16, 1, ptr, len, false);                \
+} while (0)
+
+void iwmct_log_top_message(struct iwmct_priv *priv, u8 *buf, int len);
+
+extern u8 iwmct_logdefs[];
+
+int iwmct_log_set_filter(u8 src, u8 logmask);
+int iwmct_log_set_fw_filter(u8 src, u8 logmask);
+
+ssize_t show_iwmct_log_level(struct device *d,
+                        struct device_attribute *attr, char *buf);
+ssize_t store_iwmct_log_level(struct device *d,
+                        struct device_attribute *attr,
+                        const char *buf, size_t count);
+ssize_t show_iwmct_log_level_fw(struct device *d,
+                        struct device_attribute *attr, char *buf);
+ssize_t store_iwmct_log_level_fw(struct device *d,
+                        struct device_attribute *attr,
+                        const char *buf, size_t count);
+
+#else
+
+#define LOG_CRITICAL(priv, src, fmt, args...)
+#define LOG_ERROR(priv, src, fmt, args...)
+#define LOG_WARNING(priv, src, fmt, args...)
+#define LOG_INFO(priv, src, fmt, args...)
+#define LOG_TRACE(priv, src, fmt, args...)
+#define LOG_HEXDUMP(src, ptr, len)
+
+static inline void iwmct_log_top_message(struct iwmct_priv *priv,
+                                         u8 *buf, int len) {}
+static inline int iwmct_log_set_filter(u8 src, u8 logmask) { return 0; }
+static inline int iwmct_log_set_fw_filter(u8 src, u8 logmask) { return 0; }
+
+#endif /* CONFIG_IWMC3200TOP_DEBUG */
+
+int log_get_filter_str(char *buf, int size);
+int log_get_fw_filter_str(char *buf, int size);
+
+#endif /* __LOG_H__ */
diff --git a/drivers/misc/iwmc3200top/main.c b/drivers/misc/iwmc3200top/main.c
new file mode 100644
index 00000000000..b1f4563be9a
--- /dev/null
+++ b/drivers/misc/iwmc3200top/main.c
@@ -0,0 +1,662 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/main.c
+ *
+ * Copyright (C) 2009 Intel 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/debugfs.h>
+#include <linux/mmc/sdio_ids.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio.h>
+
+#include "iwmc3200top.h"
+#include "log.h"
+#include "fw-msg.h"
+#include "debugfs.h"
+
+
+#define DRIVER_DESCRIPTION "Intel(R) IWMC 3200 Top Driver"
+#define DRIVER_COPYRIGHT "Copyright (c) 2008 Intel Corporation."
+
+#define DRIVER_VERSION  "0.1.62"
+
+MODULE_DESCRIPTION(DRIVER_DESCRIPTION);
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR(DRIVER_COPYRIGHT);
+MODULE_FIRMWARE(FW_NAME(FW_API_VER));
+
+
+static inline int __iwmct_tx(struct iwmct_priv *priv, void *src, int count)
+{
+        return sdio_memcpy_toio(priv->func, IWMC_SDIO_DATA_ADDR, src, count);
+
+}
+int iwmct_tx(struct iwmct_priv *priv, void *src, int count)
+{
+        int ret;
+        sdio_claim_host(priv->func);
+        ret =  __iwmct_tx(priv, src, count);
+        sdio_release_host(priv->func);
+        return ret;
+}
+/*
+ * This workers main task is to wait for OP_OPR_ALIVE
+ * from TOP FW until ALIVE_MSG_TIMOUT timeout is elapsed.
+ * When OP_OPR_ALIVE received it will issue
+ * a call to "bus_rescan_devices".
+ */
+static void iwmct_rescan_worker(struct work_struct *ws)
+{
+        struct iwmct_priv *priv;
+        int ret;
+
+        priv = container_of(ws, struct iwmct_priv, bus_rescan_worker);
+
+        LOG_INFO(priv, FW_MSG, "Calling bus_rescan\n");
+
+        ret = bus_rescan_devices(priv->func->dev.bus);
+        if (ret < 0)
+                LOG_INFO(priv, INIT, "bus_rescan_devices FAILED!!!\n");
+}
+
+static void op_top_message(struct iwmct_priv *priv, struct top_msg *msg)
+{
+        switch (msg->hdr.opcode) {
+        case OP_OPR_ALIVE:
+                LOG_INFO(priv, FW_MSG, "Got ALIVE from device, wake rescan\n");
+                schedule_work(&priv->bus_rescan_worker);
+                break;
+        default:
+                LOG_INFO(priv, FW_MSG, "Received msg opcode 0x%X\n",
+                        msg->hdr.opcode);
+                break;
+        }
+}
+
+
+static void handle_top_message(struct iwmct_priv *priv, u8 *buf, int len)
+{
+        struct top_msg *msg;
+
+        msg = (struct top_msg *)buf;
+
+        if (msg->hdr.type != COMM_TYPE_D2H) {
+                LOG_ERROR(priv, FW_MSG,
+                        "Message from TOP with invalid message type 0x%X\n",
+                        msg->hdr.type);
+                return;
+        }
+
+        if (len < sizeof(msg->hdr)) {
+                LOG_ERROR(priv, FW_MSG,
+                        "Message from TOP is too short for message header "
+                        "received %d bytes, expected at least %zd bytes\n",
+                        len, sizeof(msg->hdr));
+                return;
+        }
+
+        if (len < le16_to_cpu(msg->hdr.length) + sizeof(msg->hdr)) {
+                LOG_ERROR(priv, FW_MSG,
+                        "Message length (%d bytes) is shorter than "
+                        "in header (%d bytes)\n",
+                        len, le16_to_cpu(msg->hdr.length));
+                return;
+        }
+
+        switch (msg->hdr.category) {
+        case COMM_CATEGORY_OPERATIONAL:
+                op_top_message(priv, (struct top_msg *)buf);
+                break;
+
+        case COMM_CATEGORY_DEBUG:
+        case COMM_CATEGORY_TESTABILITY:
+        case COMM_CATEGORY_DIAGNOSTICS:
+                iwmct_log_top_message(priv, buf, len);
+                break;
+
+        default:
+                LOG_ERROR(priv, FW_MSG,
+                        "Message from TOP with unknown category 0x%X\n",
+                        msg->hdr.category);
+                break;
+        }
+}
+
+int iwmct_send_hcmd(struct iwmct_priv *priv, u8 *cmd, u16 len)
+{
+        int ret;
+        u8 *buf;
+
+        LOG_TRACE(priv, FW_MSG, "Sending hcmd:\n");
+
+        /* add padding to 256 for IWMC */
+        ((struct top_msg *)cmd)->hdr.flags |= CMD_FLAG_PADDING_256;
+
+        LOG_HEXDUMP(FW_MSG, cmd, len);
+
+        if (len > FW_HCMD_BLOCK_SIZE) {
+                LOG_ERROR(priv, FW_MSG, "size %d exceeded hcmd max size %d\n",
+                          len, FW_HCMD_BLOCK_SIZE);
+                return -1;
+        }
+
+        buf = kzalloc(FW_HCMD_BLOCK_SIZE, GFP_KERNEL);
+        if (!buf) {
+                LOG_ERROR(priv, FW_MSG, "kzalloc error, buf size %d\n",
+                          FW_HCMD_BLOCK_SIZE);
+                return -1;
+        }
+
+        memcpy(buf, cmd, len);
+        ret = iwmct_tx(priv, buf, FW_HCMD_BLOCK_SIZE);
+
+        kfree(buf);
+        return ret;
+}
+
+
+static void iwmct_irq_read_worker(struct work_struct *ws)
+{
+        struct iwmct_priv *priv;
+        struct iwmct_work_struct *read_req;
+        __le32 *buf = NULL;
+        int ret;
+        int iosize;
+        u32 barker;
+        bool is_barker;
+
+        priv = container_of(ws, struct iwmct_priv, isr_worker);
+
+        LOG_TRACE(priv, IRQ, "enter iwmct_irq_read_worker %p\n", ws);
+
+        /* --------------------- Handshake with device -------------------- */
+        sdio_claim_host(priv->func);
+
+        /* all list manipulations have to be protected by
+         * sdio_claim_host/sdio_release_host */
+        if (list_empty(&priv->read_req_list)) {
+                LOG_ERROR(priv, IRQ, "read_req_list empty in read worker\n");
+                goto exit_release;
+        }
+
+        read_req = list_entry(priv->read_req_list.next,
+                              struct iwmct_work_struct, list);
+
+        list_del(&read_req->list);
+        iosize = read_req->iosize;
+        kfree(read_req);
+
+        buf = kzalloc(iosize, GFP_KERNEL);
+        if (!buf) {
+                LOG_ERROR(priv, IRQ, "kzalloc error, buf size %d\n", iosize);
+                goto exit_release;
+        }
+
+        LOG_INFO(priv, IRQ, "iosize=%d, buf=%p, func=%d\n",
+                                iosize, buf, priv->func->num);
+
+        /* read from device */
+        ret = sdio_memcpy_fromio(priv->func, buf, IWMC_SDIO_DATA_ADDR, iosize);
+        if (ret) {
+                LOG_ERROR(priv, IRQ, "error %d reading buffer\n", ret);
+                goto exit_release;
+        }
+
+        LOG_HEXDUMP(IRQ, (u8 *)buf, iosize);
+
+        barker = le32_to_cpu(buf[0]);
+
+        /* Verify whether it's a barker and if not - treat as regular Rx */
+        if (barker == IWMC_BARKER_ACK ||
+            (barker & BARKER_DNLOAD_BARKER_MSK) == IWMC_BARKER_REBOOT) {
+
+                /* Valid Barker is equal on first 4 dwords */
+                is_barker = (buf[1] == buf[0]) &&
+                            (buf[2] == buf[0]) &&
+                            (buf[3] == buf[0]);
+
+                if (!is_barker) {
+                        LOG_WARNING(priv, IRQ,
+                                "Potentially inconsistent barker "
+                                "%08X_%08X_%08X_%08X\n",
+                                le32_to_cpu(buf[0]), le32_to_cpu(buf[1]),
+                                le32_to_cpu(buf[2]), le32_to_cpu(buf[3]));
+                }
+        } else {
+                is_barker = false;
+        }
+
+        /* Handle Top CommHub message */
+        if (!is_barker) {
+                sdio_release_host(priv->func);
+                handle_top_message(priv, (u8 *)buf, iosize);
+                goto exit;
+        } else if (barker == IWMC_BARKER_ACK) { /* Handle barkers */
+                if (atomic_read(&priv->dev_sync) == 0) {
+                        LOG_ERROR(priv, IRQ,
+                                  "ACK barker arrived out-of-sync\n");
+                        goto exit_release;
+                }
+
+                /* Continuing to FW download (after Sync is completed)*/
+                atomic_set(&priv->dev_sync, 0);
+                LOG_INFO(priv, IRQ, "ACK barker arrived "
+                                "- starting FW download\n");
+        } else { /* REBOOT barker */
+                LOG_INFO(priv, IRQ, "Received reboot barker: %x\n", barker);
+                priv->barker = barker;
+
+                if (barker & BARKER_DNLOAD_SYNC_MSK) {
+                        /* Send the same barker back */
+                        ret = __iwmct_tx(priv, buf, iosize);
+                        if (ret) {
+                                LOG_ERROR(priv, IRQ,
+                                         "error %d echoing barker\n", ret);
+                                goto exit_release;
+                        }
+                        LOG_INFO(priv, IRQ, "Echoing barker to device\n");
+                        atomic_set(&priv->dev_sync, 1);
+                        goto exit_release;
+                }
+
+                /* Continuing to FW download (without Sync) */
+                LOG_INFO(priv, IRQ, "No sync requested "
+                                    "- starting FW download\n");
+        }
+
+        sdio_release_host(priv->func);
+
+        if (priv->dbg.fw_download)
+                iwmct_fw_load(priv);
+        else
+                LOG_ERROR(priv, IRQ, "FW download not allowed\n");
+
+        goto exit;
+
+exit_release:
+        sdio_release_host(priv->func);
+exit:
+        kfree(buf);
+        LOG_TRACE(priv, IRQ, "exit iwmct_irq_read_worker\n");
+}
+
+static void iwmct_irq(struct sdio_func *func)
+{
+        struct iwmct_priv *priv;
+        int val, ret;
+        int iosize;
+        int addr = IWMC_SDIO_INTR_GET_SIZE_ADDR;
+        struct iwmct_work_struct *read_req;
+
+        priv = sdio_get_drvdata(func);
+
+        LOG_TRACE(priv, IRQ, "enter iwmct_irq\n");
+
+        /* read the function's status register */
+        val = sdio_readb(func, IWMC_SDIO_INTR_STATUS_ADDR, &ret);
+
+        LOG_TRACE(priv, IRQ, "iir value = %d, ret=%d\n", val, ret);
+
+        if (!val) {
+                LOG_ERROR(priv, IRQ, "iir = 0, exiting ISR\n");
+                goto exit_clear_intr;
+        }
+
+
+        /*
+         * read 2 bytes of the transaction size
+         * IMPORTANT: sdio transaction size has to be read before clearing
+         * sdio interrupt!!!
+         */
+        val = sdio_readb(priv->func, addr++, &ret);
+        iosize = val;
+        val = sdio_readb(priv->func, addr++, &ret);
+        iosize += val << 8;
+
+        LOG_INFO(priv, IRQ, "READ size %d\n", iosize);
+
+        if (iosize == 0) {
+                LOG_ERROR(priv, IRQ, "READ size %d, exiting ISR\n", iosize);
+                goto exit_clear_intr;
+        }
+
+        /* allocate a work structure to pass iosize to the worker */
+        read_req = kzalloc(sizeof(struct iwmct_work_struct), GFP_KERNEL);
+        if (!read_req) {
+                LOG_ERROR(priv, IRQ, "failed to allocate read_req, exit ISR\n");
+                goto exit_clear_intr;
+        }
+
+        INIT_LIST_HEAD(&read_req->list);
+        read_req->iosize = iosize;
+
+        list_add_tail(&priv->read_req_list, &read_req->list);
+
+        /* clear the function's interrupt request bit (write 1 to clear) */
+        sdio_writeb(func, 1, IWMC_SDIO_INTR_CLEAR_ADDR, &ret);
+
+        schedule_work(&priv->isr_worker);
+
+        LOG_TRACE(priv, IRQ, "exit iwmct_irq\n");
+
+        return;
+
+exit_clear_intr:
+        /* clear the function's interrupt request bit (write 1 to clear) */
+        sdio_writeb(func, 1, IWMC_SDIO_INTR_CLEAR_ADDR, &ret);
+}
+
+
+static int blocks;
+module_param(blocks, int, 0604);
+MODULE_PARM_DESC(blocks, "max_blocks_to_send");
+
+static int dump;
+module_param(dump, bool, 0604);
+MODULE_PARM_DESC(dump, "dump_hex_content");
+
+static int jump = 1;
+module_param(jump, bool, 0604);
+
+static int direct = 1;
+module_param(direct, bool, 0604);
+
+static int checksum = 1;
+module_param(checksum, bool, 0604);
+
+static int fw_download = 1;
+module_param(fw_download, bool, 0604);
+
+static int block_size = IWMC_SDIO_BLK_SIZE;
+module_param(block_size, int, 0404);
+
+static int download_trans_blks = IWMC_DEFAULT_TR_BLK;
+module_param(download_trans_blks, int, 0604);
+
+static int rubbish_barker;
+module_param(rubbish_barker, bool, 0604);
+
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+static int log_level[LOG_SRC_MAX];
+static unsigned int log_level_argc;
+module_param_array(log_level, int, &log_level_argc, 0604);
+MODULE_PARM_DESC(log_level, "log_level");
+
+static int log_level_fw[FW_LOG_SRC_MAX];
+static unsigned int log_level_fw_argc;
+module_param_array(log_level_fw, int, &log_level_fw_argc, 0604);
+MODULE_PARM_DESC(log_level_fw, "log_level_fw");
+#endif
+
+void iwmct_dbg_init_params(struct iwmct_priv *priv)
+{
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+        int i;
+
+        for (i = 0; i < log_level_argc; i++) {
+                dev_notice(&priv->func->dev, "log_level[%d]=0x%X\n",
+                                                i, log_level[i]);
+                iwmct_log_set_filter((log_level[i] >> 8) & 0xFF,
+                               log_level[i] & 0xFF);
+        }
+        for (i = 0; i < log_level_fw_argc; i++) {
+                dev_notice(&priv->func->dev, "log_level_fw[%d]=0x%X\n",
+                                                i, log_level_fw[i]);
+                iwmct_log_set_fw_filter((log_level_fw[i] >> 8) & 0xFF,
+                                  log_level_fw[i] & 0xFF);
+        }
+#endif
+
+        priv->dbg.blocks = blocks;
+        LOG_INFO(priv, INIT, "blocks=%d\n", blocks);
+        priv->dbg.dump = (bool)dump;
+        LOG_INFO(priv, INIT, "dump=%d\n", dump);
+        priv->dbg.jump = (bool)jump;
+        LOG_INFO(priv, INIT, "jump=%d\n", jump);
+        priv->dbg.direct = (bool)direct;
+        LOG_INFO(priv, INIT, "direct=%d\n", direct);
+        priv->dbg.checksum = (bool)checksum;
+        LOG_INFO(priv, INIT, "checksum=%d\n", checksum);
+        priv->dbg.fw_download = (bool)fw_download;
+        LOG_INFO(priv, INIT, "fw_download=%d\n", fw_download);
+        priv->dbg.block_size = block_size;
+        LOG_INFO(priv, INIT, "block_size=%d\n", block_size);
+        priv->dbg.download_trans_blks = download_trans_blks;
+        LOG_INFO(priv, INIT, "download_trans_blks=%d\n", download_trans_blks);
+}
+
+/*****************************************************************************
+ *
+ * sysfs attributes
+ *
+ *****************************************************************************/
+static ssize_t show_iwmct_fw_version(struct device *d,
+                                  struct device_attribute *attr, char *buf)
+{
+        struct iwmct_priv *priv = dev_get_drvdata(d);
+        return sprintf(buf, "%s\n", priv->dbg.label_fw);
+}
+static DEVICE_ATTR(cc_label_fw, S_IRUGO, show_iwmct_fw_version, NULL);
+
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+static DEVICE_ATTR(log_level, S_IWUSR | S_IRUGO,
+                   show_iwmct_log_level, store_iwmct_log_level);
+static DEVICE_ATTR(log_level_fw, S_IWUSR | S_IRUGO,
+                   show_iwmct_log_level_fw, store_iwmct_log_level_fw);
+#endif
+
+static struct attribute *iwmct_sysfs_entries[] = {
+        &dev_attr_cc_label_fw.attr,
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+        &dev_attr_log_level.attr,
+        &dev_attr_log_level_fw.attr,
+#endif
+        NULL
+};
+
+static struct attribute_group iwmct_attribute_group = {
+        .name = NULL,           /* put in device directory */
+        .attrs = iwmct_sysfs_entries,
+};
+
+
+static int iwmct_probe(struct sdio_func *func,
+                           const struct sdio_device_id *id)
+{
+        struct iwmct_priv *priv;
+        int ret;
+        int val = 1;
+        int addr = IWMC_SDIO_INTR_ENABLE_ADDR;
+
+        dev_dbg(&func->dev, "enter iwmct_probe\n");
+
+        dev_dbg(&func->dev, "IRQ polling period id %u msecs, HZ is %d\n",
+                jiffies_to_msecs(2147483647), HZ);
+
+        priv = kzalloc(sizeof(struct iwmct_priv), GFP_KERNEL);
+        if (!priv) {
+                dev_err(&func->dev, "kzalloc error\n");
+                return -ENOMEM;
+        }
+        priv->func = func;
+        sdio_set_drvdata(func, priv);
+
+        INIT_WORK(&priv->bus_rescan_worker, iwmct_rescan_worker);
+        INIT_WORK(&priv->isr_worker, iwmct_irq_read_worker);
+
+        init_waitqueue_head(&priv->wait_q);
+
+        sdio_claim_host(func);
+        /* FIXME: Remove after it is fixed in the Boot ROM upgrade */
+        func->enable_timeout = 10;
+
+        /* In our HW, setting the block size also wakes up the boot rom. */
+        ret = sdio_set_block_size(func, priv->dbg.block_size);
+        if (ret) {
+                LOG_ERROR(priv, INIT,
+                        "sdio_set_block_size() failure: %d\n", ret);
+                goto error_sdio_enable;
+        }
+
+        ret = sdio_enable_func(func);
+        if (ret) {
+                LOG_ERROR(priv, INIT, "sdio_enable_func() failure: %d\n", ret);
+                goto error_sdio_enable;
+        }
+
+        /* init reset and dev_sync states */
+        atomic_set(&priv->reset, 0);
+        atomic_set(&priv->dev_sync, 0);
+
+        /* init read req queue */
+        INIT_LIST_HEAD(&priv->read_req_list);
+
+        /* process configurable parameters */
+        iwmct_dbg_init_params(priv);
+        ret = sysfs_create_group(&func->dev.kobj, &iwmct_attribute_group);
+        if (ret) {
+                LOG_ERROR(priv, INIT, "Failed to register attributes and "
+                         "initialize module_params\n");
+                goto error_dev_attrs;
+        }
+
+        iwmct_dbgfs_register(priv, DRV_NAME);
+
+        if (!priv->dbg.direct && priv->dbg.download_trans_blks > 8) {
+                LOG_INFO(priv, INIT,
+                         "Reducing transaction to 8 blocks = 2K (from %d)\n",
+                         priv->dbg.download_trans_blks);
+                priv->dbg.download_trans_blks = 8;
+        }
+        priv->trans_len = priv->dbg.download_trans_blks * priv->dbg.block_size;
+        LOG_INFO(priv, INIT, "Transaction length = %d\n", priv->trans_len);
+
+        ret = sdio_claim_irq(func, iwmct_irq);
+        if (ret) {
+                LOG_ERROR(priv, INIT, "sdio_claim_irq() failure: %d\n", ret);
+                goto error_claim_irq;
+        }
+
+
+        /* Enable function's interrupt */
+        sdio_writeb(priv->func, val, addr, &ret);
+        if (ret) {
+                LOG_ERROR(priv, INIT, "Failure writing to "
+                          "Interrupt Enable Register (%d): %d\n", addr, ret);
+                goto error_enable_int;
+        }
+
+        sdio_release_host(func);
+
+        LOG_INFO(priv, INIT, "exit iwmct_probe\n");
+
+        return ret;
+
+error_enable_int:
+        sdio_release_irq(func);
+error_claim_irq:
+        sdio_disable_func(func);
+error_dev_attrs:
+        iwmct_dbgfs_unregister(priv->dbgfs);
+        sysfs_remove_group(&func->dev.kobj, &iwmct_attribute_group);
+error_sdio_enable:
+        sdio_release_host(func);
+        return ret;
+}
+
+static void iwmct_remove(struct sdio_func *func)
+{
+        struct iwmct_work_struct *read_req;
+        struct iwmct_priv *priv = sdio_get_drvdata(func);
+
+        LOG_INFO(priv, INIT, "enter\n");
+
+        sdio_claim_host(func);
+        sdio_release_irq(func);
+        sdio_release_host(func);
+
+        /* Make sure works are finished */
+        flush_work_sync(&priv->bus_rescan_worker);
+        flush_work_sync(&priv->isr_worker);
+
+        sdio_claim_host(func);
+        sdio_disable_func(func);
+        sysfs_remove_group(&func->dev.kobj, &iwmct_attribute_group);
+        iwmct_dbgfs_unregister(priv->dbgfs);
+        sdio_release_host(func);
+
+        /* free read requests */
+        while (!list_empty(&priv->read_req_list)) {
+                read_req = list_entry(priv->read_req_list.next,
+                        struct iwmct_work_struct, list);
+
+                list_del(&read_req->list);
+                kfree(read_req);
+        }
+
+        kfree(priv);
+}
+
+
+static const struct sdio_device_id iwmct_ids[] = {
+        /* Intel Wireless MultiCom 3200 Top Driver */
+        { SDIO_DEVICE(SDIO_VENDOR_ID_INTEL, 0x1404)},
+        { },    /* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(sdio, iwmct_ids);
+
+static struct sdio_driver iwmct_driver = {
+        .probe          = iwmct_probe,
+        .remove         = iwmct_remove,
+        .name           = DRV_NAME,
+        .id_table       = iwmct_ids,
+};
+
+static int __init iwmct_init(void)
+{
+        int rc;
+
+        /* Default log filter settings */
+        iwmct_log_set_filter(LOG_SRC_ALL, LOG_SEV_FILTER_RUNTIME);
+        iwmct_log_set_filter(LOG_SRC_FW_MSG, LOG_SEV_FW_FILTER_ALL);
+        iwmct_log_set_fw_filter(LOG_SRC_ALL, FW_LOG_SEV_FILTER_RUNTIME);
+
+        rc = sdio_register_driver(&iwmct_driver);
+
+        return rc;
+}
+
+static void __exit iwmct_exit(void)
+{
+        sdio_unregister_driver(&iwmct_driver);
+}
+
+module_init(iwmct_init);
+module_exit(iwmct_exit);
+
diff --git a/drivers/misc/max1749.c b/drivers/misc/max1749.c
new file mode 100644
index 00000000000..e9896428900
--- /dev/null
+++ b/drivers/misc/max1749.c
@@ -0,0 +1,118 @@
+/*
+ * drivers/misc/max1749.c
+ *
+ * Driver for MAX1749, vibrator motor driver.
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/regulator/consumer.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+
+#include "../staging/android/timed_output.h"
+
+static struct regulator *regulator;
+static int timeout;
+
+static void vibrator_start(void)
+{
+        regulator_enable(regulator);
+}
+
+static void vibrator_stop(void)
+{
+        int ret;
+
+        ret = regulator_is_enabled(regulator);
+        if (ret > 0)
+                regulator_disable(regulator);
+}
+
+/*
+ * Timeout value can be changed from sysfs entry
+ * created by timed_output_dev.
+ * echo 100 > /sys/class/timed_output/vibrator/enable
+ */
+static void vibrator_enable(struct timed_output_dev *dev, int value)
+{
+        timeout = value;
+        if (!regulator)
+                return;
+
+        if (value) {
+                vibrator_start();
+                msleep(value);
+                vibrator_stop();
+        } else {
+                vibrator_stop();
+        }
+}
+
+/*
+ * Timeout value can be read from sysfs entry
+ * created by timed_output_dev.
+ * cat /sys/class/timed_output/vibrator/enable
+ */
+static int vibrator_get_time(struct timed_output_dev *dev)
+{
+        return timeout;
+}
+
+static struct timed_output_dev vibrator_dev = {
+        .name           = "vibrator",
+        .get_time       = vibrator_get_time,
+        .enable         = vibrator_enable,
+};
+
+static int __init vibrator_init(void)
+{
+        int status;
+
+        regulator = regulator_get(NULL, "vdd_vbrtr");
+        if (IS_ERR_OR_NULL(regulator)) {
+                pr_err("vibrator_init:Couldn't get regulator vdd_vbrtr\n");
+                regulator = NULL;
+                return PTR_ERR(regulator);
+        }
+
+        status = timed_output_dev_register(&vibrator_dev);
+
+        if (status) {
+                regulator_put(regulator);
+                regulator = NULL;
+        }
+        return status;
+}
+
+static void __exit vibrator_exit(void)
+{
+        if (regulator) {
+                timed_output_dev_unregister(&vibrator_dev);
+                regulator_put(regulator);
+                regulator = NULL;
+        }
+}
+
+MODULE_DESCRIPTION("timed output vibrator device");
+MODULE_AUTHOR("GPL");
+
+module_init(vibrator_init);
+module_exit(vibrator_exit);
diff --git a/drivers/misc/mpu3050/Kconfig b/drivers/misc/mpu3050/Kconfig
new file mode 100644
index 00000000000..de240fa0ad8
--- /dev/null
+++ b/drivers/misc/mpu3050/Kconfig
@@ -0,0 +1,65 @@
+
+menu "Motion Sensors Support"
+
+choice
+    tristate "Motion Processing Unit"
+    depends on I2C
+    optional
+
+config MPU_SENSORS_MPU3050
+    tristate "MPU3050"
+    help
+      If you say yes here you get support for the MPU3050 Gyroscope driver
+      This driver can also be built as a module.  If so, the module
+      will be called mpu3050.
+
+config MPU_SENSORS_MPU6000
+    tristate "MPU6000"
+    help
+      If you say yes here you get support for the MPU6000 Gyroscope driver
+      This driver can also be built as a module.  If so, the module
+      will be called mpu6000.
+
+endchoice
+
+choice
+    prompt "Accelerometer Type"
+    depends on MPU_SENSORS_MPU3050
+    optional
+
+config MPU_SENSORS_KXTF9
+    bool "Kionix KXTF9"
+    help
+      This enables support for the Kionix KXFT9 accelerometer
+
+endchoice
+
+choice
+    prompt "Compass Type"
+    depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050
+    optional
+
+config MPU_SENSORS_AK8975
+    bool "AKM ak8975"
+    help
+      This enables support for the AKM ak8975 compass
+
+endchoice
+
+config MPU_SENSORS_TIMERIRQ
+    tristate "Timer IRQ"
+    help
+    If you say yes here you get access to the timerirq device handle which
+    can be used to select on. This can be used instead of IRQ's, sleeping,
+    or timer threads. Reading from this device returns the same type of
+    information as reading from the MPU and slave IRQ's.
+
+config MPU_SENSORS_DEBUG
+    bool "MPU debug"
+    depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6000 || MPU_SENSORS_TIMERIRQ
+    help
+      If you say yes here you get extra debug messages from the MPU3050
+      and other slave sensors.
+
+endmenu
+
diff --git a/drivers/misc/mpu3050/Makefile b/drivers/misc/mpu3050/Makefile
new file mode 100644
index 00000000000..89ac46fdac5
--- /dev/null
+++ b/drivers/misc/mpu3050/Makefile
@@ -0,0 +1,132 @@
+
+# Kernel makefile for motions sensors
+#
+#
+
+# MPU
+obj-$(CONFIG_MPU_SENSORS_MPU3050)       += mpu3050.o
+mpu3050-objs += mpuirq.o \
+        slaveirq.o \
+        mpu-dev.o \
+        mpu-i2c.o \
+        mlsl-kernel.o \
+        mlos-kernel.o \
+        $(MLLITE_DIR)mldl_cfg.o
+
+#
+# Accel options
+#
+ifdef CONFIG_MPU_SENSORS_ADXL346
+mpu3050-objs += $(MLLITE_DIR)accel/adxl346.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_BMA150
+mpu3050-objs += $(MLLITE_DIR)accel/bma150.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_BMA222
+mpu3050-objs += $(MLLITE_DIR)accel/bma222.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_KXSD9
+mpu3050-objs += $(MLLITE_DIR)accel/kxsd9.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_KXTF9
+mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LIS331DLH
+mpu3050-objs += $(MLLITE_DIR)accel/lis331.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LIS3DH
+mpu3050-objs += $(MLLITE_DIR)accel/lis3dh.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LSM303DLHA
+mpu3050-objs += $(MLLITE_DIR)accel/lsm303a.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMA8450
+mpu3050-objs += $(MLLITE_DIR)accel/mma8450.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMA845X
+mpu3050-objs += $(MLLITE_DIR)accel/mma845x.o
+endif
+
+#
+# Compass options
+#
+ifdef CONFIG_MPU_SENSORS_AK8975
+mpu3050-objs += $(MLLITE_DIR)compass/ak8975.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_AMI30X
+mpu3050-objs += $(MLLITE_DIR)compass/ami30x.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_AMI306
+mpu3050-objs += $(MLLITE_DIR)compass/ami306.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_HMC5883
+mpu3050-objs += $(MLLITE_DIR)compass/hmc5883.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LSM303DLHM
+mpu3050-objs += $(MLLITE_DIR)compass/lsm303m.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMC314X
+mpu3050-objs += $(MLLITE_DIR)compass/mmc314x.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_YAS529
+mpu3050-objs += $(MLLITE_DIR)compass/yas529-kernel.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_YAS530
+mpu3050-objs += $(MLLITE_DIR)compass/yas530.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_HSCDTD002B
+mpu3050-objs += $(MLLITE_DIR)compass/hscdtd002b.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_HSCDTD004A
+mpu3050-objs += $(MLLITE_DIR)compass/hscdtd004a.o
+endif
+#
+# Pressure options
+#
+ifdef CONFIG_MPU_SENSORS_BMA085
+mpu3050-objs += $(MLLITE_DIR)pressure/bma085.o
+endif
+
+EXTRA_CFLAGS += -I$(M)/$(MLLITE_DIR) \
+                -I$(M)/../../include \
+                -Idrivers/misc/mpu3050 \
+                -Iinclude/linux
+
+obj-$(CONFIG_MPU_SENSORS_MPU6000)+= mpu6000.o
+mpu6000-objs += mpuirq.o \
+        slaveirq.o \
+        mpu-dev.o \
+        mpu-i2c.o \
+        mlsl-kernel.o \
+        mlos-kernel.o \
+        $(MLLITE_DIR)mldl_cfg.o \
+        $(MLLITE_DIR)accel/mantis.o
+
+ifdef CONFIG_MPU_SENSORS_AK8975
+mpu6000-objs += $(MLLITE_DIR)compass/ak8975.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MPU6000
+EXTRA_CFLAGS += -DM_HW
+endif
+
+obj-$(CONFIG_MPU_SENSORS_TIMERIRQ)+= timerirq.o
+
diff --git a/drivers/misc/mpu3050/accel/kxtf9.c b/drivers/misc/mpu3050/accel/kxtf9.c
new file mode 100644
index 00000000000..938cd572a8f
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/kxtf9.c
@@ -0,0 +1,669 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+/**
+ *  @defgroup   ACCELDL (Motion Library - Accelerometer Driver Layer)
+ *  @brief      Provides the interface to setup and handle an accelerometers
+ *              connected to the secondary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   kxtf9.c
+ *      @brief  Accelerometer setup and handling methods.
+*/
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+#ifdef __KERNEL__
+#include <linux/module.h>
+#endif
+
+#include "mpu.h"
+#include "mlsl.h"
+#include "mlos.h"
+
+#include <log.h>
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+#define KXTF9_XOUT_HPF_L                (0x00) /* 0000 0000 */
+#define KXTF9_XOUT_HPF_H                (0x01) /* 0000 0001 */
+#define KXTF9_YOUT_HPF_L                (0x02) /* 0000 0010 */
+#define KXTF9_YOUT_HPF_H                (0x03) /* 0000 0011 */
+#define KXTF9_ZOUT_HPF_L                (0x04) /* 0001 0100 */
+#define KXTF9_ZOUT_HPF_H                (0x05) /* 0001 0101 */
+#define KXTF9_XOUT_L                    (0x06) /* 0000 0110 */
+#define KXTF9_XOUT_H                    (0x07) /* 0000 0111 */
+#define KXTF9_YOUT_L                    (0x08) /* 0000 1000 */
+#define KXTF9_YOUT_H                    (0x09) /* 0000 1001 */
+#define KXTF9_ZOUT_L                    (0x0A) /* 0001 1010 */
+#define KXTF9_ZOUT_H                    (0x0B) /* 0001 1011 */
+#define KXTF9_ST_RESP                   (0x0C) /* 0000 1100 */
+#define KXTF9_WHO_AM_I                  (0x0F) /* 0000 1111 */
+#define KXTF9_TILT_POS_CUR              (0x10) /* 0001 0000 */
+#define KXTF9_TILT_POS_PRE              (0x11) /* 0001 0001 */
+#define KXTF9_INT_SRC_REG1              (0x15) /* 0001 0101 */
+#define KXTF9_INT_SRC_REG2              (0x16) /* 0001 0110 */
+#define KXTF9_STATUS_REG                (0x18) /* 0001 1000 */
+#define KXTF9_INT_REL                   (0x1A) /* 0001 1010 */
+#define KXTF9_CTRL_REG1                 (0x1B) /* 0001 1011 */
+#define KXTF9_CTRL_REG2                 (0x1C) /* 0001 1100 */
+#define KXTF9_CTRL_REG3                 (0x1D) /* 0001 1101 */
+#define KXTF9_INT_CTRL_REG1             (0x1E) /* 0001 1110 */
+#define KXTF9_INT_CTRL_REG2             (0x1F) /* 0001 1111 */
+#define KXTF9_INT_CTRL_REG3             (0x20) /* 0010 0000 */
+#define KXTF9_DATA_CTRL_REG             (0x21) /* 0010 0001 */
+#define KXTF9_TILT_TIMER                (0x28) /* 0010 1000 */
+#define KXTF9_WUF_TIMER                 (0x29) /* 0010 1001 */
+#define KXTF9_TDT_TIMER                 (0x2B) /* 0010 1011 */
+#define KXTF9_TDT_H_THRESH              (0x2C) /* 0010 1100 */
+#define KXTF9_TDT_L_THRESH              (0x2D) /* 0010 1101 */
+#define KXTF9_TDT_TAP_TIMER             (0x2E) /* 0010 1110 */
+#define KXTF9_TDT_TOTAL_TIMER           (0x2F) /* 0010 1111 */
+#define KXTF9_TDT_LATENCY_TIMER         (0x30) /* 0011 0000 */
+#define KXTF9_TDT_WINDOW_TIMER          (0x31) /* 0011 0001 */
+#define KXTF9_WUF_THRESH                (0x5A) /* 0101 1010 */
+#define KXTF9_TILT_ANGLE                (0x5C) /* 0101 1100 */
+#define KXTF9_HYST_SET                  (0x5F) /* 0101 1111 */
+
+#define KXTF9_MAX_DUR (0xFF)
+#define KXTF9_MAX_THS (0xFF)
+#define KXTF9_THS_COUNTS_P_G (32)
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+struct kxtf9_config {
+        unsigned int odr; /* Output data rate mHz */
+        unsigned int fsr; /* full scale range mg */
+        unsigned int ths; /* Motion no-motion thseshold mg */
+        unsigned int dur; /* Motion no-motion duration ms */
+        unsigned int irq_type;
+        unsigned char reg_ths;
+        unsigned char reg_dur;
+        unsigned char reg_odr;
+        unsigned char reg_int_cfg1;
+        unsigned char reg_int_cfg2;
+        unsigned char ctrl_reg1;
+};
+
+struct kxtf9_private_data {
+        struct kxtf9_config suspend;
+        struct kxtf9_config resume;
+};
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+static int kxtf9_set_ths(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct kxtf9_config *config,
+                        int apply,
+                        long ths)
+{
+        int result = ML_SUCCESS;
+        if ((ths * KXTF9_THS_COUNTS_P_G / 1000) > KXTF9_MAX_THS)
+                ths = (KXTF9_MAX_THS * 1000) / KXTF9_THS_COUNTS_P_G;
+
+        if (ths < 0)
+                ths = 0;
+
+        config->ths = ths;
+        config->reg_ths = (unsigned char)
+                ((long)(ths * KXTF9_THS_COUNTS_P_G) / 1000);
+        MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+        if (apply)
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_WUF_THRESH,
+                                        config->reg_ths);
+        return result;
+}
+
+static int kxtf9_set_dur(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct kxtf9_config *config,
+                        int apply,
+                        long dur)
+{
+        int result = ML_SUCCESS;
+        long reg_dur = (dur * config->odr) / 1000000;
+        config->dur = dur;
+
+        if (reg_dur > KXTF9_MAX_DUR)
+                reg_dur = KXTF9_MAX_DUR;
+
+        config->reg_dur = (unsigned char) reg_dur;
+        MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+        if (apply)
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_WUF_TIMER,
+                                        (unsigned char)reg_dur);
+        return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int kxtf9_set_irq(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct kxtf9_config *config,
+                        int apply,
+                        long irq_type)
+{
+        int result = ML_SUCCESS;
+        struct kxtf9_private_data *private_data = pdata->private_data;
+
+        config->irq_type = (unsigned char)irq_type;
+        config->ctrl_reg1 &= ~0x22;
+        if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+                config->ctrl_reg1 |= 0x20;
+                config->reg_int_cfg1 = 0x38;
+                config->reg_int_cfg2 = 0x00;
+        } else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+                config->ctrl_reg1 |= 0x02;
+                if ((unsigned long) config ==
+                        (unsigned long) &private_data->suspend)
+                        config->reg_int_cfg1 = 0x34;
+                else
+                        config->reg_int_cfg1 = 0x24;
+                config->reg_int_cfg2 = 0xE0;
+        } else {
+                config->reg_int_cfg1 = 0x00;
+                config->reg_int_cfg2 = 0x00;
+        }
+
+        if (apply) {
+                /* Must clear bit 7 before writing new configuration */
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_CTRL_REG1, 0x40);
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_INT_CTRL_REG1,
+                                        config->reg_int_cfg1);
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_INT_CTRL_REG2,
+                                        config->reg_int_cfg2);
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_CTRL_REG1,
+                                        config->ctrl_reg1);
+        }
+        MPL_LOGV("CTRL_REG1: %lx, INT_CFG1: %lx, INT_CFG2: %lx\n",
+                (unsigned long)config->ctrl_reg1,
+                (unsigned long)config->reg_int_cfg1,
+                (unsigned long)config->reg_int_cfg2);
+
+        return result;
+}
+
+/**
+ * Set the Output data rate for the particular configuration
+ *
+ * @param config Config to modify with new ODR
+ * @param odr Output data rate in units of 1/1000Hz
+ */
+static int kxtf9_set_odr(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct kxtf9_config *config,
+                        int apply,
+                        long odr)
+{
+        unsigned char bits;
+        int result = ML_SUCCESS;
+
+        /* Data sheet says there is 12.5 hz, but that seems to produce a single
+         * correct data value, thus we remove it from the table */
+        if (odr > 400000) {
+                config->odr = 800000;
+                bits = 0x06;
+        } else if (odr > 200000) {
+                config->odr = 400000;
+                bits = 0x05;
+        } else if (odr > 100000) {
+                config->odr = 200000;
+                bits = 0x04;
+        } else if (odr > 50000) {
+                config->odr = 100000;
+                bits = 0x03;
+        } else if (odr > 25000) {
+                config->odr = 50000;
+                bits = 0x02;
+        } else if (odr != 0) {
+                config->odr = 25000;
+                bits = 0x01;
+        } else {
+                config->odr = 0;
+                bits = 0;
+        }
+
+        config->reg_odr = bits;
+        kxtf9_set_dur(mlsl_handle, pdata,
+                config, apply, config->dur);
+        MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+        if (apply) {
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_DATA_CTRL_REG,
+                                        config->reg_odr);
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_CTRL_REG1,
+                                        0x40);
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_CTRL_REG1,
+                                        config->ctrl_reg1);
+        }
+        return result;
+}
+
+/**
+ * Set the full scale range of the accels
+ *
+ * @param config pointer to configuration
+ * @param fsr requested full scale range
+ */
+static int kxtf9_set_fsr(void *mlsl_handle,
+                        struct ext_slave_platform_data *pdata,
+                        struct kxtf9_config *config,
+                        int apply,
+                        long fsr)
+{
+        int result = ML_SUCCESS;
+
+        config->ctrl_reg1 = (config->ctrl_reg1 & 0xE7);
+        if (fsr <= 2000) {
+                config->fsr = 2000;
+                config->ctrl_reg1 |= 0x00;
+        } else if (fsr <= 4000) {
+                config->fsr = 4000;
+                config->ctrl_reg1 |= 0x08;
+        } else {
+                config->fsr = 8000;
+                config->ctrl_reg1 |= 0x10;
+        }
+
+        MPL_LOGV("FSR: %d\n", config->fsr);
+        if (apply) {
+                /* Must clear bit 7 before writing new configuration */
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_CTRL_REG1, 0x40);
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                        KXTF9_CTRL_REG1, config->ctrl_reg1);
+        }
+        return result;
+}
+
+static int kxtf9_suspend(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result;
+        unsigned char data;
+        struct kxtf9_private_data *private_data = pdata->private_data;
+
+        /* Wake up */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_CTRL_REG1, 0x40);
+        ERROR_CHECK(result);
+        /* INT_CTRL_REG1: */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_INT_CTRL_REG1,
+                                private_data->suspend.reg_int_cfg1);
+        ERROR_CHECK(result);
+        /* WUF_THRESH: */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_WUF_THRESH,
+                                private_data->suspend.reg_ths);
+        ERROR_CHECK(result);
+        /* DATA_CTRL_REG */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_DATA_CTRL_REG,
+                                private_data->suspend.reg_odr);
+        ERROR_CHECK(result);
+        /* WUF_TIMER */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_WUF_TIMER, private_data->suspend.reg_dur);
+        ERROR_CHECK(result);
+
+        /* Normal operation  */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_CTRL_REG1,
+                                private_data->suspend.ctrl_reg1);
+        ERROR_CHECK(result);
+        result = MLSLSerialRead(mlsl_handle, pdata->address,
+                                KXTF9_INT_REL, 1, &data);
+        ERROR_CHECK(result);
+
+        return result;
+}
+
+/* full scale setting - register and mask */
+#define ACCEL_KIONIX_CTRL_REG      (0x1b)
+#define ACCEL_KIONIX_CTRL_MASK     (0x18)
+
+static int kxtf9_resume(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata)
+{
+        int result = ML_SUCCESS;
+        unsigned char data;
+        struct kxtf9_private_data *private_data = pdata->private_data;
+
+        /* Wake up */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_CTRL_REG1, 0x40);
+        ERROR_CHECK(result);
+        /* INT_CTRL_REG1: */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_INT_CTRL_REG1,
+                                private_data->resume.reg_int_cfg1);
+        ERROR_CHECK(result);
+        /* WUF_THRESH: */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_WUF_THRESH, private_data->resume.reg_ths);
+        ERROR_CHECK(result);
+        /* DATA_CTRL_REG */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_DATA_CTRL_REG,
+                                private_data->resume.reg_odr);
+        ERROR_CHECK(result);
+        /* WUF_TIMER */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_WUF_TIMER, private_data->resume.reg_dur);
+        ERROR_CHECK(result);
+
+        /* Normal operation  */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_CTRL_REG1,
+                                private_data->resume.ctrl_reg1);
+        ERROR_CHECK(result);
+        result = MLSLSerialRead(mlsl_handle, pdata->address,
+                                KXTF9_INT_REL, 1, &data);
+        ERROR_CHECK(result);
+
+        return ML_SUCCESS;
+}
+
+static int kxtf9_init(void *mlsl_handle,
+                struct ext_slave_descr *slave,
+                struct ext_slave_platform_data *pdata)
+{
+
+        struct kxtf9_private_data *private_data;
+        int result = ML_SUCCESS;
+
+        private_data = (struct kxtf9_private_data *)
+                MLOSMalloc(sizeof(struct kxtf9_private_data));
+
+        if (!private_data)
+                return ML_ERROR_MEMORY_EXAUSTED;
+
+        /* RAM reset */
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_CTRL_REG1,
+                                0x40); /* Fastest Reset */
+        ERROR_CHECK(result);
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_DATA_CTRL_REG,
+                                0x36); /* Fastest Reset */
+        ERROR_CHECK(result);
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                KXTF9_CTRL_REG3, 0xcd); /* Reset */
+        ERROR_CHECK(result);
+        MLOSSleep(2);
+
+        pdata->private_data = private_data;
+
+        private_data->resume.ctrl_reg1 = 0xC0;
+        private_data->suspend.ctrl_reg1 = 0x40;
+
+        result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->suspend,
+                        FALSE, 1000);
+        ERROR_CHECK(result);
+        result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->resume,
+                        FALSE,  2540);
+        ERROR_CHECK(result);
+
+        result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->suspend,
+                        FALSE, 50000);
+        ERROR_CHECK(result);
+        result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->resume,
+                        FALSE, 200000);
+
+        result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+                        FALSE, 2000);
+        ERROR_CHECK(result);
+        result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->resume,
+                        FALSE, 2000);
+        ERROR_CHECK(result);
+
+        result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->suspend,
+                        FALSE, 80);
+        ERROR_CHECK(result);
+        result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->resume,
+                        FALSE, 40);
+        ERROR_CHECK(result);
+
+        result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->suspend,
+                        FALSE,
+                        MPU_SLAVE_IRQ_TYPE_NONE);
+        ERROR_CHECK(result);
+        result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->resume,
+                        FALSE,
+                        MPU_SLAVE_IRQ_TYPE_NONE);
+        ERROR_CHECK(result);
+        return result;
+}
+
+static int kxtf9_exit(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        if (pdata->private_data)
+                return MLOSFree(pdata->private_data);
+        else
+                return ML_SUCCESS;
+}
+
+static int kxtf9_config(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_config *data)
+{
+        int retval;
+        long odr;
+        struct kxtf9_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return ML_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                return kxtf9_set_odr(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                odr = *((long *)data->data);
+                if (odr != 0)
+                        private_data->resume.ctrl_reg1 |= 0x80;
+
+                retval = kxtf9_set_odr(mlsl_handle, pdata,
+                                &private_data->resume,
+                                data->apply,
+                                odr);
+                return retval;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                return kxtf9_set_fsr(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                return kxtf9_set_fsr(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                return kxtf9_set_ths(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                return kxtf9_set_ths(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                return kxtf9_set_dur(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                return kxtf9_set_dur(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                return kxtf9_set_irq(mlsl_handle, pdata,
+                                        &private_data->suspend,
+                                        data->apply,
+                                        *((long *)data->data));
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                return kxtf9_set_irq(mlsl_handle, pdata,
+                                        &private_data->resume,
+                                        data->apply,
+                                        *((long *)data->data));
+        default:
+                return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return ML_SUCCESS;
+}
+
+static int kxtf9_get_config(void *mlsl_handle,
+                                struct ext_slave_descr *slave,
+                                struct ext_slave_platform_data *pdata,
+                                struct ext_slave_config *data)
+{
+        struct kxtf9_private_data *private_data = pdata->private_data;
+        if (!data->data)
+                return ML_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.odr;
+                break;
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.odr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.fsr;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_THS:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.ths;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.ths;
+                break;
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.dur;
+                break;
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.dur;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->suspend.irq_type;
+                break;
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+                (*(unsigned long *)data->data) =
+                        (unsigned long) private_data->resume.irq_type;
+                break;
+        default:
+                return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return ML_SUCCESS;
+}
+
+static int kxtf9_read(void *mlsl_handle,
+                      struct ext_slave_descr *slave,
+                      struct ext_slave_platform_data *pdata,
+                      unsigned char *data)
+{
+        int result;
+        unsigned char reg;
+        result = MLSLSerialRead(mlsl_handle, pdata->address,
+                                KXTF9_INT_SRC_REG2, 1, &reg);
+        ERROR_CHECK(result);
+
+        if (!(reg & 0x10))
+                return ML_ERROR_ACCEL_DATA_NOT_READY;
+
+        result = MLSLSerialRead(mlsl_handle, pdata->address,
+                                slave->reg, slave->len, data);
+        ERROR_CHECK(result);
+        return result;
+}
+
+static struct ext_slave_descr kxtf9_descr = {
+        /*.init             = */ kxtf9_init,
+        /*.exit             = */ kxtf9_exit,
+        /*.suspend          = */ kxtf9_suspend,
+        /*.resume           = */ kxtf9_resume,
+        /*.read             = */ kxtf9_read,
+        /*.config           = */ kxtf9_config,
+        /*.get_config       = */ kxtf9_get_config,
+        /*.name             = */ "kxtf9",
+        /*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+        /*.id               = */ ACCEL_ID_KXTF9,
+        /*.reg              = */ 0x06,
+        /*.len              = */ 6,
+        /*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+        /*.range            = */ {2, 0},
+};
+
+struct ext_slave_descr *kxtf9_get_slave_descr(void)
+{
+        return &kxtf9_descr;
+}
+EXPORT_SYMBOL(kxtf9_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/ak8975.c b/drivers/misc/mpu3050/compass/ak8975.c
new file mode 100644
index 00000000000..b8aed30ba39
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/ak8975.c
@@ -0,0 +1,258 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+/**
+ *  @defgroup   COMPASSDL (Motion Library - Accelerometer Driver Layer)
+ *  @brief      Provides the interface to setup and handle an accelerometers
+ *              connected to the secondary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   AK8975.c
+ *      @brief  Magnetometer setup and handling methods for AKM 8975 compass.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <string.h>
+
+#ifdef __KERNEL__
+#include <linux/module.h>
+#endif
+
+#include "mpu.h"
+#include "mlsl.h"
+#include "mlos.h"
+
+#include <log.h>
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+
+#define AK8975_REG_ST1  (0x02)
+#define AK8975_REG_HXL  (0x03)
+#define AK8975_REG_ST2  (0x09)
+
+#define AK8975_REG_CNTL (0x0A)
+
+#define AK8975_CNTL_MODE_POWER_DOWN         (0x00)
+#define AK8975_CNTL_MODE_SINGLE_MEASUREMENT (0x01)
+
+int ak8975_suspend(void *mlsl_handle,
+                   struct ext_slave_descr *slave,
+                   struct ext_slave_platform_data *pdata)
+{
+        int result = ML_SUCCESS;
+        result =
+            MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                  AK8975_REG_CNTL,
+                                  AK8975_CNTL_MODE_POWER_DOWN);
+        MLOSSleep(1);           /* wait at least 100us */
+        ERROR_CHECK(result);
+        return result;
+}
+
+int ak8975_resume(void *mlsl_handle,
+                  struct ext_slave_descr *slave,
+                  struct ext_slave_platform_data *pdata)
+{
+        int result = ML_SUCCESS;
+        result =
+            MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                  AK8975_REG_CNTL,
+                                  AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
+        ERROR_CHECK(result);
+        return result;
+}
+
+int ak8975_read(void *mlsl_handle,
+                struct ext_slave_descr *slave,
+                struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+        unsigned char regs[8];
+        unsigned char *stat = &regs[0];
+        unsigned char *stat2 = &regs[7];
+        int result = ML_SUCCESS;
+        int status = ML_SUCCESS;
+
+        result =
+            MLSLSerialRead(mlsl_handle, pdata->address, AK8975_REG_ST1,
+                           8, regs);
+        ERROR_CHECK(result);
+
+        /*
+         * ST : data ready -
+         * Measurement has been completed and data is ready to be read.
+         */
+        if (*stat & 0x01) {
+                memcpy(data, &regs[1], 6);
+                status = ML_SUCCESS;
+        }
+
+        /*
+         * ST2 : data error -
+         * occurs when data read is started outside of a readable period;
+         * data read would not be correct.
+         * Valid in continuous measurement mode only.
+         * In single measurement mode this error should not occour but we
+         * stil account for it and return an error, since the data would be
+         * corrupted.
+         * DERR bit is self-clearing when ST2 register is read.
+         */
+        if (*stat2 & 0x04)
+                status = ML_ERROR_COMPASS_DATA_ERROR;
+        /*
+         * ST2 : overflow -
+         * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT.
+         * This is likely to happen in presence of an external magnetic
+         * disturbance; it indicates, the sensor data is incorrect and should
+         * be ignored.
+         * An error is returned.
+         * HOFL bit clears when a new measurement starts.
+         */
+        if (*stat2 & 0x08)
+                status = ML_ERROR_COMPASS_DATA_OVERFLOW;
+        /*
+         * ST : overrun -
+         * the previous sample was not fetched and lost.
+         * Valid in continuous measurement mode only.
+         * In single measurement mode this error should not occour and we
+         * don't consider this condition an error.
+         * DOR bit is self-clearing when ST2 or any meas. data register is
+         * read.
+         */
+        if (*stat & 0x02) {
+                /* status = ML_ERROR_COMPASS_DATA_UNDERFLOW; */
+                status = ML_SUCCESS;
+        }
+
+        /*
+         * trigger next measurement if:
+         *    - stat is non zero;
+         *    - if stat is zero and stat2 is non zero.
+         * Won't trigger if data is not ready and there was no error.
+         */
+        if (*stat != 0x00 || *stat2 != 0x00) {
+                result =
+                    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+                                          AK8975_REG_CNTL,
+                                          AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
+                ERROR_CHECK(result);
+        }
+
+        return status;
+}
+
+static int ak8975_config(void *mlsl_handle,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_config *data)
+{
+        int result;
+        if (!data->data)
+                return ML_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_WRITE_REGISTERS:
+                result = MLSLSerialWrite(mlsl_handle, pdata->address,
+                                        data->len,
+                                        (unsigned char *)data->data);
+                ERROR_CHECK(result);
+                break;
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return ML_SUCCESS;
+}
+
+static int ak8975_get_config(void *mlsl_handle,
+                                struct ext_slave_descr *slave,
+                                struct ext_slave_platform_data *pdata,
+                                struct ext_slave_config *data)
+{
+        int result;
+        if (!data->data)
+                return ML_ERROR_INVALID_PARAMETER;
+
+        switch (data->key) {
+        case MPU_SLAVE_READ_REGISTERS:
+        {
+                unsigned char *serial_data = (unsigned char *)data->data;
+                result = MLSLSerialRead(mlsl_handle, pdata->address,
+                                        serial_data[0],
+                                        data->len - 1,
+                                        &serial_data[1]);
+                ERROR_CHECK(result);
+                break;
+        }
+        case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+        case MPU_SLAVE_CONFIG_ODR_RESUME:
+        case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+        case MPU_SLAVE_CONFIG_FSR_RESUME:
+        case MPU_SLAVE_CONFIG_MOT_THS:
+        case MPU_SLAVE_CONFIG_NMOT_THS:
+        case MPU_SLAVE_CONFIG_MOT_DUR:
+        case MPU_SLAVE_CONFIG_NMOT_DUR:
+        case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+        case MPU_SLAVE_CONFIG_IRQ_RESUME:
+        default:
+                return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+        };
+
+        return ML_SUCCESS;
+}
+
+struct ext_slave_descr ak8975_descr = {
+        /*.init             = */ NULL,
+        /*.exit             = */ NULL,
+        /*.suspend          = */ ak8975_suspend,
+        /*.resume           = */ ak8975_resume,
+        /*.read             = */ ak8975_read,
+        /*.config           = */ ak8975_config,
+        /*.get_config       = */ ak8975_get_config,
+        /*.name             = */ "ak8975",
+        /*.type             = */ EXT_SLAVE_TYPE_COMPASS,
+        /*.id               = */ COMPASS_ID_AKM,
+        /*.reg              = */ 0x01,
+        /*.len              = */ 9,
+        /*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+        /*.range            = */ {9830, 4000}
+};
+
+struct ext_slave_descr *ak8975_get_slave_descr(void)
+{
+        return &ak8975_descr;
+}
+EXPORT_SYMBOL(ak8975_get_slave_descr);
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/log.h b/drivers/misc/mpu3050/log.h
new file mode 100644
index 00000000000..f2f9ea7ece8
--- /dev/null
+++ b/drivers/misc/mpu3050/log.h
@@ -0,0 +1,306 @@
+/*
+ * Copyright (C) 2010 InvenSense Inc
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * C/C++ logging functions.  See the logging documentation for API details.
+ *
+ * We'd like these to be available from C code (in case we import some from
+ * somewhere), so this has a C interface.
+ *
+ * The output will be correct when the log file is shared between multiple
+ * threads and/or multiple processes so long as the operating system
+ * supports O_APPEND.  These calls have mutex-protected data structures
+ * and so are NOT reentrant.  Do not use MPL_LOG in a signal handler.
+ */
+#ifndef _LIBS_CUTILS_MPL_LOG_H
+#define _LIBS_CUTILS_MPL_LOG_H
+
+#include <stdarg.h>
+
+#ifdef ANDROID
+#include <utils/Log.h>          /* For the LOG macro */
+#endif
+
+#ifdef __KERNEL__
+#include <linux/kernel.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Normally we strip MPL_LOGV (VERBOSE messages) from release builds.
+ * You can modify this (for example with "#define MPL_LOG_NDEBUG 0"
+ * at the top of your source file) to change that behavior.
+ */
+#ifndef MPL_LOG_NDEBUG
+#ifdef NDEBUG
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+#endif
+
+#ifdef __KERNEL__
+#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE
+#define MPL_LOG_DEFAULT KERN_DEFAULT
+#define MPL_LOG_VERBOSE KERN_CONT
+#define MPL_LOG_DEBUG   KERN_NOTICE
+#define MPL_LOG_INFO    KERN_INFO
+#define MPL_LOG_WARN    KERN_WARNING
+#define MPL_LOG_ERROR   KERN_ERR
+#define MPL_LOG_SILENT  MPL_LOG_VERBOSE
+
+#else
+        /* Based off the log priorities in android
+           /system/core/include/android/log.h */
+#define MPL_LOG_UNKNOWN         (0)
+#define MPL_LOG_DEFAULT         (1)
+#define MPL_LOG_VERBOSE         (2)
+#define MPL_LOG_DEBUG           (3)
+#define MPL_LOG_INFO            (4)
+#define MPL_LOG_WARN            (5)
+#define MPL_LOG_ERROR           (6)
+#define MPL_LOG_SILENT          (8)
+#endif
+
+
+/*
+ * This is the local tag used for the following simplified
+ * logging macros.  You can change this preprocessor definition
+ * before using the other macros to change the tag.
+ */
+#ifndef MPL_LOG_TAG
+#ifdef __KERNEL__
+#define MPL_LOG_TAG
+#else
+#define MPL_LOG_TAG NULL
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Simplified macro to send a verbose log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGV
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV(fmt, ...)                                              \
+        do {                                                            \
+                if (0)                                                  \
+                        MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+        } while (0)
+#else
+#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef CONDITION
+#define CONDITION(cond)     ((cond) != 0)
+#endif
+
+#ifndef MPL_LOGV_IF
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV_IF(cond, fmt, ...)  \
+        do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0)
+#else
+#define MPL_LOGV_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                              \
+                ? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
+                : (void)0)
+#endif
+#endif
+
+/*
+ * Simplified macro to send a debug log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGD
+#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGD_IF
+#define MPL_LOGD_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                             \
+                ? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+                : (void)0)
+#endif
+
+/*
+ * Simplified macro to send an info log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGI
+#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGI_IF
+#define MPL_LOGI_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                              \
+                ? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+                : (void)0)
+#endif
+
+/*
+ * Simplified macro to send a warning log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGW
+#ifdef __KERNEL__
+#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGW_IF
+#define MPL_LOGW_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                             \
+                ? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+                : (void)0)
+#endif
+
+/*
+ * Simplified macro to send an error log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGE
+#ifdef __KERNEL__
+#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGE_IF
+#define MPL_LOGE_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                             \
+                ? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+                : (void)0)
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Log a fatal error.  If the given condition fails, this stops program
+ * execution like a normal assertion, but also generating the given message.
+ * It is NOT stripped from release builds.  Note that the condition test
+ * is -inverted- from the normal assert() semantics.
+ */
+#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \
+        ((CONDITION(cond))                                         \
+                ? ((void)android_printAssert(#cond, MPL_LOG_TAG,   \
+                                                fmt, ##__VA_ARGS__))    \
+                : (void)0)
+
+#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \
+        (((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__)))
+
+/*
+ * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that
+ * are stripped out of release builds.
+ */
+#if MPL_LOG_NDEBUG
+#define MPL_LOG_FATAL_IF(cond, fmt, ...)                                \
+        do {                                                            \
+                if (0)                                                  \
+                        MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \
+        } while (0)
+#define MPL_LOG_FATAL(fmt, ...)                                         \
+        do {                                                            \
+                if (0)                                                  \
+                        MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)        \
+        } while (0)
+#else
+#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
+        MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__)
+#define MPL_LOG_FATAL(fmt, ...) \
+        MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Assertion that generates a log message when the assertion fails.
+ * Stripped out of release builds.  Uses the current MPL_LOG_TAG.
+ */
+#define MPL_LOG_ASSERT(cond, fmt, ...)                  \
+        MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__)
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Basic log message macro.
+ *
+ * Example:
+ *  MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno);
+ *
+ * The second argument may be NULL or "" to indicate the "global" tag.
+ */
+#ifndef MPL_LOG
+#define MPL_LOG(priority, tag, fmt, ...)                \
+        MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Log macro that allows you to specify a number for the priority.
+ */
+#ifndef MPL_LOG_PRI
+#ifdef ANDROID
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+        LOG(priority, tag, fmt, ##__VA_ARGS__)
+#elif defined __KERNEL__
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+        pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+        _MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+/*
+ * Log macro that allows you to pass in a varargs ("args" is a va_list).
+ */
+#ifndef MPL_LOG_PRI_VA
+#ifdef ANDROID
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+    android_vprintLog(priority, NULL, tag, fmt, args)
+#elif defined __KERNEL__
+/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
+#else
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+    _MLPrintVaLog(priority, NULL, tag, fmt, args)
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * ===========================================================================
+ *
+ * The stuff in the rest of this file should not be used directly.
+ */
+
+#ifndef ANDROID
+        int _MLPrintLog(int priority, const char *tag, const char *fmt,
+                        ...);
+        int _MLPrintVaLog(int priority, const char *tag, const char *fmt,
+                          va_list args);
+/* Final implementation of actual writing to a character device */
+        int _MLWriteLog(const char *buf, int buflen);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif                          /* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/drivers/misc/mpu3050/mldl_cfg.c b/drivers/misc/mpu3050/mldl_cfg.c
new file mode 100644
index 00000000000..9cc4cf69038
--- /dev/null
+++ b/drivers/misc/mpu3050/mldl_cfg.c
@@ -0,0 +1,1739 @@
+/*
+ $License:
+    Copyright (C) 2010 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 Files. - */
+/* ------------------ */
+
+#include <stddef.h>
+
+#include "mldl_cfg.h"
+#include "mpu.h"
+
+#include "mlsl.h"
+#include "mlos.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_cfg:"
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+#ifdef M_HW
+#define SLEEP   0
+#define WAKE_UP 7
+#define RESET   1
+#define STANDBY 1
+#else
+/* licteral significance of all parameters used in MLDLPowerMgmtMPU */
+#define SLEEP   1
+#define WAKE_UP 0
+#define RESET   1
+#define STANDBY 1
+#endif
+
+/*---------------------*/
+/*-    Prototypes.    -*/
+/*---------------------*/
+
+/*----------------------*/
+/*-  Static Functions. -*/
+/*----------------------*/
+
+static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+        unsigned char userCtrlReg;
+        int result;
+
+        if (!mldl_cfg->dmp_is_running)
+                return ML_SUCCESS;
+
+        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;
+
+        result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+                                       MPUREG_USER_CTRL, userCtrlReg);
+        ERROR_CHECK(result);
+        mldl_cfg->dmp_is_running = 0;
+
+        return result;
+
+}
+/**
+ * @brief Starts the DMP running
+ *
+ * @return ML_SUCCESS or non-zero error code
+ */
+static int dmp_start(struct mldl_cfg *pdata, void *mlsl_handle)
+{
+        unsigned char userCtrlReg;
+        int result;
+
+        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);
+
+        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;
+
+        userCtrlReg |= BIT_DMP_RST;
+
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+                                       MPUREG_USER_CTRL, userCtrlReg);
+        ERROR_CHECK(result);
+        pdata->dmp_is_running = pdata->dmp_enable;
+
+        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 ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
+                            void *mlsl_handle,
+                            unsigned char enable)
+{
+        unsigned char b;
+        int result;
+
+        if ((mldl_cfg->gyro_is_bypassed && enable) ||
+            (!mldl_cfg->gyro_is_bypassed && !enable))
+                return ML_SUCCESS;
+
+        /*---- get current 'USER_CTRL' into b ----*/
+        result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
+                                MPUREG_USER_CTRL, 1, &b);
+        ERROR_CHECK(result);
+
+        b &= ~BIT_AUX_IF_EN;
+
+        if (!enable) {
+                result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+                                               MPUREG_USER_CTRL,
+                                               (b | BIT_AUX_IF_EN));
+                ERROR_CHECK(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 = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+                                               MPUREG_AUX_SLV_ADDR, 0x7F);
+                ERROR_CHECK(result);
+                /*
+                 * 2) wait enough time for a nack to occur, then go into
+                 *    bypass mode:
+                 */
+                MLOSSleep(2);
+                result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+                                               MPUREG_USER_CTRL, (b));
+                ERROR_CHECK(result);
+                /*
+                 * 3) wait for up to one MPU cycle then restore the slave
+                 *    address
+                 */
+                MLOSSleep(SAMPLING_PERIOD_US(mldl_cfg) / 1000);
+                result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+                                               MPUREG_AUX_SLV_ADDR,
+                                               mldl_cfg->pdata->
+                                               accel.address);
+                ERROR_CHECK(result);
+
+                /*
+                 * 4) reset the ime interface
+                 */
+#ifdef M_HW
+                result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+                                               MPUREG_USER_CTRL,
+                                               (b | BIT_I2C_MST_RST));
+
+#else
+                result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+                                               MPUREG_USER_CTRL,
+                                               (b | BIT_AUX_IF_RST));
+#endif
+                ERROR_CHECK(result);
+                MLOSSleep(2);
+        }
+        mldl_cfg->gyro_is_bypassed = enable;
+
+        return result;
+}
+
+struct tsProdRevMap {
+        unsigned char siliconRev;
+        unsigned short sensTrim;
+};
+
+#define NUM_OF_PROD_REVS (DIM(prodRevsMap))
+
+/* NOTE : 'npp' is a non production part */
+#ifdef M_HW
+#define OLDEST_PROD_REV_SUPPORTED 1
+static struct tsProdRevMap prodRevsMap[] = {
+        {0, 0},
+        {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 */
+#define OLDEST_PROD_REV_SUPPORTED 11
+
+static struct tsProdRevMap prodRevsMap[] = {
+        {0, 0},
+        {MPU_SILICON_REV_A4, 131},      /* 1 A? OBSOLETED */
+        {MPU_SILICON_REV_A4, 131},      /* 2 | */
+        {MPU_SILICON_REV_A4, 131},      /* 3 V */
+        {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 */
+        {MPU_SILICON_REV_B1, 131},      /* 11 B1 */
+        {MPU_SILICON_REV_B1, 131},      /* 12 | */
+        {MPU_SILICON_REV_B1, 131},      /* 13 V */
+        {MPU_SILICON_REV_B1, 131},      /* 14 B4 */
+        {MPU_SILICON_REV_B4, 131},      /* 15 | */
+        {MPU_SILICON_REV_B4, 131},      /* 16 V */
+        {MPU_SILICON_REV_B4, 131},      /* 17 */
+        {MPU_SILICON_REV_B4, 131},      /* 18 */
+        {MPU_SILICON_REV_B4, 115},      /* 19  */
+        {MPU_SILICON_REV_B4, 115},      /* 20 */
+        {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 (npp)    */
+        {MPU_SILICON_REV_B6, 0},        /* 24 |  (npp)    */
+        {MPU_SILICON_REV_B6, 0},        /* 25 V  (npp)    */
+        {MPU_SILICON_REV_B6, 131},      /* 26    (B6/A11) */
+};
+#endif                          /* !M_HW */
+
+/**
+ *  @internal
+ *  @brief  Get the silicon revision ID from OTP.
+ *          The silicon revision number is in read from OTP bank 0,
+ *          ADDR6[7:2].  The corresponding ID is retrieved by lookup
+ *          in a map.
+ *  @return The silicon revision ID (0 on error).
+ */
+static int MLDLGetSiliconRev(struct mldl_cfg *pdata,
+                             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 memAddr = ((bank << 8) | 0x06);
+
+        result = MLSLSerialReadMem(mlsl_handle, pdata->addr,
+                                   memAddr, 1, &index);
+        ERROR_CHECK(result);
+        if (result)
+                return result;
+        index >>= 2;
+
+        /* clean the prefetch and cfg user bank bits */
+        result =
+            MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+                                  MPUREG_BANK_SEL, 0);
+        ERROR_CHECK(result);
+        if (result)
+                return result;
+
+        if (index < OLDEST_PROD_REV_SUPPORTED || NUM_OF_PROD_REVS <= index) {
+                pdata->silicon_revision = 0;
+                pdata->trim = 0;
+                MPL_LOGE("Unsupported Product Revision Detected : %d\n", index);
+                return ML_ERROR_INVALID_MODULE;
+        }
+
+        pdata->silicon_revision = prodRevsMap[index].siliconRev;
+        pdata->trim = prodRevsMap[index].sensTrim;
+
+        if (pdata->trim == 0) {
+                MPL_LOGE("sensitivity trim is 0"
+                         " - unsupported non production part.\n");
+                return ML_ERROR_INVALID_MODULE;
+        }
+
+        return result;
+}
+
+/**
+ *  @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 MLSerialOpen().
+ *  @note   Typically called before MLDmpOpen().
+ *
+ *  @param[in]  enable:
+ *                  0 to run at VDDIO (default),
+ *                  1 to run at VDD.
+ *
+ *  @return ML_SUCCESS if successfull, a non-zero error code otherwise.
+ */
+static int MLDLSetLevelShifterBit(struct mldl_cfg *pdata,
+                                  void *mlsl_handle,
+                                  unsigned char enable)
+{
+#ifndef M_HW
+        int result;
+        unsigned char reg;
+        unsigned char mask;
+        unsigned char regval;
+
+        if (0 == pdata->silicon_revision)
+                return ML_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 ((pdata->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 = MLSLSerialRead(mlsl_handle, pdata->addr, reg, 1, &regval);
+        if (result)
+                return result;
+
+        if (enable)
+                regval |= mask;
+        else
+                regval &= ~mask;
+
+        result =
+            MLSLSerialWriteSingle(mlsl_handle, pdata->addr, reg, regval);
+
+        return result;
+#else
+        return ML_SUCCESS;
+#endif
+}
+
+
+#ifdef M_HW
+/**
+ *  @internal
+ * @param reset 1 to reset hardware
+ */
+static tMLError mpu60xx_pwr_mgmt(struct mldl_cfg *pdata,
+                                 void *mlsl_handle,
+                                 unsigned char reset,
+                                 unsigned char powerselection)
+{
+        unsigned char b;
+        tMLError result;
+
+        if (powerselection < 0 || powerselection > 7)
+                return ML_ERROR_INVALID_PARAMETER;
+
+        result =
+            MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_1, 1,
+                           &b);
+        ERROR_CHECK(result);
+
+        b &= ~(BITS_PWRSEL);
+
+        if (reset) {
+                /* 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 | BIT_H_RESET);
+#define M_HW_RESET_ERRATTA
+#ifndef M_HW_RESET_ERRATTA
+                ERROR_CHECK(result);
+#else
+                MLOSSleep(50);
+#endif
+        }
+
+        b |= (powerselection << 4);
+
+        if (b & BITS_PWRSEL)
+                pdata->gyro_is_suspended = FALSE;
+        else
+                pdata->gyro_is_suspended = TRUE;
+
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+                                       MPUREG_PWR_MGM, b);
+        ERROR_CHECK(result);
+
+        return ML_SUCCESS;
+}
+
+/**
+ *  @internal
+ */
+static tMLError MLDLStandByGyros(struct mldl_cfg *pdata,
+                                 void *mlsl_handle,
+                                 unsigned char disable_gx,
+                                 unsigned char disable_gy,
+                                 unsigned char disable_gz)
+{
+        unsigned char b;
+        tMLError result;
+
+        result =
+            MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_2, 1,
+                           &b);
+        ERROR_CHECK(result);
+
+        b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
+        b |= (disable_gx << 2 | disable_gy << 1 | disable_gz);
+
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+                                       MPUREG_PWR_MGMT_2, b);
+        ERROR_CHECK(result);
+
+        return ML_SUCCESS;
+}
+
+/**
+ *  @internal
+ */
+static tMLError MLDLStandByAccels(struct mldl_cfg *pdata,
+                                  void *mlsl_handle,
+                                  unsigned char disable_ax,
+                                  unsigned char disable_ay,
+                                  unsigned char disable_az)
+{
+        unsigned char b;
+        tMLError result;
+
+        result =
+            MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_2, 1,
+                           &b);
+        ERROR_CHECK(result);
+
+        b &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
+        b |= (disable_ax << 2 | disable_ay << 1 | disable_az);
+
+        result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+                                       MPUREG_PWR_MGMT_2, b);
+        ERROR_CHECK(result);
+
+        return ML_SUCCESS;
+}
+
+#else                           /* ! M_HW */
+
+/**
+ * @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  ML_SUCCESS if successfull; a non-zero error code otherwise.
+ */
+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)
+{
+        unsigned char b;
+        int result;
+
+        result =
+            MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGM, 1,
+                           &b);
+        ERROR_CHECK(result);
+
+        /* If we are awake, we need to put it in bypass before resetting */
+        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) {
+                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);
+        }
+
+        /* Update the suspended state just in case we return early */
+        if (b & BIT_SLEEP)
+                pdata->gyro_is_suspended = TRUE;
+        else
+                pdata->gyro_is_suspended = FALSE;
+
+        /* 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 ML_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 = MLDLPowerMgmtMPU(pdata, 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 = MLDLSetI2CBypass(pdata, mlsl_handle, 1);
+                        ERROR_CHECK(result);
+                        b |= BIT_SLEEP;
+                        result =
+                            MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+                                                  MPUREG_PWR_MGM, b);
+                        ERROR_CHECK(result);
+                        pdata->gyro_is_suspended = TRUE;
+                } else {
+                        b &= ~BIT_SLEEP;
+                        result =
+                            MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+                                                  MPUREG_PWR_MGM, b);
+                        ERROR_CHECK(result);
+                        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
+          ---*/
+        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);
+                ERROR_CHECK(result);
+        }
+        if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) {
+                b ^= BIT_STBY_YG;
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+                                               MPUREG_PWR_MGM, b);
+                ERROR_CHECK(result);
+        }
+        if ((b & BIT_STBY_ZG) != ((disable_gz != 0) * BIT_STBY_ZG)) {
+                b ^= BIT_STBY_ZG;
+                result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+                                               MPUREG_PWR_MGM, b);
+                ERROR_CHECK(result);
+        }
+
+        return ML_SUCCESS;
+}
+#endif                          /* M_HW */
+
+
+void mpu_print_cfg(struct mldl_cfg *mldl_cfg)
+{
+        struct mpu3050_platform_data *pdata = mldl_cfg->pdata;
+        struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel;
+        struct ext_slave_platform_data *compass =
+            &mldl_cfg->pdata->compass;
+        struct ext_slave_platform_data *pressure =
+            &mldl_cfg->pdata->pressure;
+
+        MPL_LOGD("mldl_cfg.addr             = %02x\n", mldl_cfg->addr);
+        MPL_LOGD("mldl_cfg.int_config       = %02x\n",
+                 mldl_cfg->int_config);
+        MPL_LOGD("mldl_cfg.ext_sync         = %02x\n", mldl_cfg->ext_sync);
+        MPL_LOGD("mldl_cfg.full_scale       = %02x\n",
+                 mldl_cfg->full_scale);
+        MPL_LOGD("mldl_cfg.lpf              = %02x\n", mldl_cfg->lpf);
+        MPL_LOGD("mldl_cfg.clk_src          = %02x\n", mldl_cfg->clk_src);
+        MPL_LOGD("mldl_cfg.divider          = %02x\n", mldl_cfg->divider);
+        MPL_LOGD("mldl_cfg.dmp_enable       = %02x\n",
+                 mldl_cfg->dmp_enable);
+        MPL_LOGD("mldl_cfg.fifo_enable      = %02x\n",
+                 mldl_cfg->fifo_enable);
+        MPL_LOGD("mldl_cfg.dmp_cfg1         = %02x\n", mldl_cfg->dmp_cfg1);
+        MPL_LOGD("mldl_cfg.dmp_cfg2         = %02x\n", mldl_cfg->dmp_cfg2);
+        MPL_LOGD("mldl_cfg.offset_tc[0]     = %02x\n",
+                 mldl_cfg->offset_tc[0]);
+        MPL_LOGD("mldl_cfg.offset_tc[1]     = %02x\n",
+                 mldl_cfg->offset_tc[1]);
+        MPL_LOGD("mldl_cfg.offset_tc[2]     = %02x\n",
+                 mldl_cfg->offset_tc[2]);
+        MPL_LOGD("mldl_cfg.silicon_revision = %02x\n",
+                 mldl_cfg->silicon_revision);
+        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",
+                         (int) mldl_cfg->accel->suspend);
+                MPL_LOGD("slave_accel->resume       = %02x\n",
+                         (int) mldl_cfg->accel->resume);
+                MPL_LOGD("slave_accel->read         = %02x\n",
+                         (int) mldl_cfg->accel->read);
+                MPL_LOGD("slave_accel->type         = %02x\n",
+                         mldl_cfg->accel->type);
+                MPL_LOGD("slave_accel->reg          = %02x\n",
+                         mldl_cfg->accel->reg);
+                MPL_LOGD("slave_accel->len          = %02x\n",
+                         mldl_cfg->accel->len);
+                MPL_LOGD("slave_accel->endian       = %02x\n",
+                         mldl_cfg->accel->endian);
+                MPL_LOGD("slave_accel->range.mantissa= %02lx\n",
+                         mldl_cfg->accel->range.mantissa);
+                MPL_LOGD("slave_accel->range.fraction= %02lx\n",
+                         mldl_cfg->accel->range.fraction);
+        } else {
+                MPL_LOGD("slave_accel               = NULL\n");
+        }
+
+        if (mldl_cfg->compass) {
+                MPL_LOGD("slave_compass->suspend    = %02x\n",
+                         (int) mldl_cfg->compass->suspend);
+                MPL_LOGD("slave_compass->resume     = %02x\n",
+                         (int) mldl_cfg->compass->resume);
+                MPL_LOGD("slave_compass->read       = %02x\n",
+                         (int) mldl_cfg->compass->read);
+                MPL_LOGD("slave_compass->type       = %02x\n",
+                         mldl_cfg->compass->type);
+                MPL_LOGD("slave_compass->reg        = %02x\n",
+                         mldl_cfg->compass->reg);
+                MPL_LOGD("slave_compass->len        = %02x\n",
+                         mldl_cfg->compass->len);
+                MPL_LOGD("slave_compass->endian     = %02x\n",
+                         mldl_cfg->compass->endian);
+                MPL_LOGD("slave_compass->range.mantissa= %02lx\n",
+                         mldl_cfg->compass->range.mantissa);
+                MPL_LOGD("slave_compass->range.fraction= %02lx\n",
+                         mldl_cfg->compass->range.fraction);
+
+        } else {
+                MPL_LOGD("slave_compass             = NULL\n");
+        }
+
+        if (mldl_cfg->pressure) {
+                MPL_LOGD("slave_pressure->suspend    = %02x\n",
+                         (int) mldl_cfg->pressure->suspend);
+                MPL_LOGD("slave_pressure->resume     = %02x\n",
+                         (int) mldl_cfg->pressure->resume);
+                MPL_LOGD("slave_pressure->read       = %02x\n",
+                         (int) mldl_cfg->pressure->read);
+                MPL_LOGD("slave_pressure->type       = %02x\n",
+                         mldl_cfg->pressure->type);
+                MPL_LOGD("slave_pressure->reg        = %02x\n",
+                         mldl_cfg->pressure->reg);
+                MPL_LOGD("slave_pressure->len        = %02x\n",
+                         mldl_cfg->pressure->len);
+                MPL_LOGD("slave_pressure->endian     = %02x\n",
+                         mldl_cfg->pressure->endian);
+                MPL_LOGD("slave_pressure->range.mantissa= %02lx\n",
+                         mldl_cfg->pressure->range.mantissa);
+                MPL_LOGD("slave_pressure->range.fraction= %02lx\n",
+                         mldl_cfg->pressure->range.fraction);
+
+        } else {
+                MPL_LOGD("slave_pressure             = NULL\n");
+        }
+        MPL_LOGD("accel->get_slave_descr    = %x\n",
+                 (unsigned int) accel->get_slave_descr);
+        MPL_LOGD("accel->irq                = %02x\n", accel->irq);
+        MPL_LOGD("accel->adapt_num          = %02x\n", accel->adapt_num);
+        MPL_LOGD("accel->bus                = %02x\n", accel->bus);
+        MPL_LOGD("accel->address            = %02x\n", accel->address);
+        MPL_LOGD("accel->orientation        =\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n",
+                 accel->orientation[0], accel->orientation[1],
+                 accel->orientation[2], accel->orientation[3],
+                 accel->orientation[4], accel->orientation[5],
+                 accel->orientation[6], accel->orientation[7],
+                 accel->orientation[8]);
+        MPL_LOGD("compass->get_slave_descr  = %x\n",
+                 (unsigned int) compass->get_slave_descr);
+        MPL_LOGD("compass->irq              = %02x\n", compass->irq);
+        MPL_LOGD("compass->adapt_num        = %02x\n", compass->adapt_num);
+        MPL_LOGD("compass->bus              = %02x\n", compass->bus);
+        MPL_LOGD("compass->address          = %02x\n", compass->address);
+        MPL_LOGD("compass->orientation      =\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n",
+                 compass->orientation[0], compass->orientation[1],
+                 compass->orientation[2], compass->orientation[3],
+                 compass->orientation[4], compass->orientation[5],
+                 compass->orientation[6], compass->orientation[7],
+                 compass->orientation[8]);
+        MPL_LOGD("pressure->get_slave_descr  = %x\n",
+                 (unsigned int) pressure->get_slave_descr);
+        MPL_LOGD("pressure->irq             = %02x\n", pressure->irq);
+        MPL_LOGD("pressure->adapt_num       = %02x\n", pressure->adapt_num);
+        MPL_LOGD("pressure->bus             = %02x\n", pressure->bus);
+        MPL_LOGD("pressure->address         = %02x\n", pressure->address);
+        MPL_LOGD("pressure->orientation     =\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n",
+                 pressure->orientation[0], pressure->orientation[1],
+                 pressure->orientation[2], pressure->orientation[3],
+                 pressure->orientation[4], pressure->orientation[5],
+                 pressure->orientation[6], pressure->orientation[7],
+                 pressure->orientation[8]);
+
+        MPL_LOGD("pdata->int_config         = %02x\n", pdata->int_config);
+        MPL_LOGD("pdata->level_shifter      = %02x\n",
+                 pdata->level_shifter);
+        MPL_LOGD("pdata->orientation        =\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n"
+                 "                            %2d %2d %2d\n",
+                 pdata->orientation[0], pdata->orientation[1],
+                 pdata->orientation[2], pdata->orientation[3],
+                 pdata->orientation[4], pdata->orientation[5],
+                 pdata->orientation[6], pdata->orientation[7],
+                 pdata->orientation[8]);
+
+        MPL_LOGD("Struct sizes: mldl_cfg: %d, "
+                 "ext_slave_descr:%d, "
+                 "mpu3050_platform_data:%d: RamOffset: %d\n",
+                 sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr),
+                 sizeof(struct mpu3050_platform_data),
+                 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
+ *******************************************************************************
+ ******************************************************************************/
+
+/**
+ * Initializes the pdata structure to defaults.
+ *
+ * Opens the device to read silicon revision, product id and whoami.
+ *
+ * @param mldl_cfg
+ *          The internal device configuration data structure.
+ * @param mlsl_handle
+ *          The serial communication handle.
+ *
+ * @return ML_SUCCESS if silicon revision, product id and woami are supported
+ *         by this software.
+ */
+int mpu3050_open(struct mldl_cfg *mldl_cfg,
+                 void *mlsl_handle,
+                 void *accel_handle,
+                 void *compass_handle,
+                 void *pressure_handle)
+{
+        int result;
+        /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
+        mldl_cfg->ignore_system_suspend = FALSE;
+        mldl_cfg->int_config = BIT_INT_ANYRD_2CLEAR | BIT_DMP_INT_EN;
+        mldl_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ;
+        mldl_cfg->lpf = MPU_FILTER_42HZ;
+        mldl_cfg->full_scale = MPU_FS_2000DPS;
+        mldl_cfg->divider = 4;
+        mldl_cfg->dmp_enable = 1;
+        mldl_cfg->fifo_enable = 1;
+        mldl_cfg->ext_sync = 0;
+        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;
+#else
+                mldl_cfg->addr = 0x68;
+#endif
+        }
+
+        /*
+         * Reset,
+         * Take the DMP out of sleep, and
+         * read the product_id, sillicon rev and whoami
+         */
+#ifdef M_HW
+        result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
+                                  RESET, WAKE_UP);
+#else
+        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,
+                                MPUREG_XG_OFFS_TC, 1,
+                                &mldl_cfg->offset_tc[0]);
+        ERROR_CHECK(result);
+        result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
+                                MPUREG_YG_OFFS_TC, 1,
+                                &mldl_cfg->offset_tc[1]);
+        ERROR_CHECK(result);
+        result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
+                                MPUREG_ZG_OFFS_TC, 1,
+                                &mldl_cfg->offset_tc[2]);
+        ERROR_CHECK(result);
+
+        /* Configure the MPU */
+#ifdef M_HW
+        result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
+                                  FALSE, SLEEP);
+#else
+        result =
+            MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, 0, SLEEP, 0, 0, 0);
+#endif
+        ERROR_CHECK(result);
+
+        if (mldl_cfg->accel && mldl_cfg->accel->init) {
+                result = mldl_cfg->accel->init(accel_handle,
+                                               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);
+                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);
+                if (ML_SUCCESS != result) {
+                        MPL_LOGE("mldl_cfg->pressure->init returned %d\n",
+                                result);
+                        goto out_compass;
+                }
+        }
+
+        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:
+        if (mldl_cfg->compass->init)
+                mldl_cfg->compass->exit(compass_handle,
+                                mldl_cfg->compass,
+                                &mldl_cfg->pdata->compass);
+out_accel:
+        if (mldl_cfg->accel->init)
+                mldl_cfg->accel->exit(accel_handle,
+                                mldl_cfg->accel,
+                                &mldl_cfg->pdata->accel);
+        return result;
+
+}
+
+/**
+ * Close the mpu3050 interface
+ *
+ * @param mldl_cfg pointer to the configuration structure
+ * @param mlsl_handle pointer to the serial layer handle
+ *
+ * @return ML_SUCCESS or non-zero error code
+ */
+int mpu3050_close(struct mldl_cfg *mldl_cfg,
+                  void *mlsl_handle,
+                  void *accel_handle,
+                  void *compass_handle,
+                  void *pressure_handle)
+{
+        int result = ML_SUCCESS;
+        int ret_result = ML_SUCCESS;
+
+        if (mldl_cfg->accel && mldl_cfg->accel->exit) {
+                result = mldl_cfg->accel->exit(accel_handle,
+                                        mldl_cfg->accel,
+                                        &mldl_cfg->pdata->accel);
+                if (ML_SUCCESS != result)
+                        MPL_LOGE("Accel exit failed %d\n", result);
+                ret_result = result;
+        }
+        if (ML_SUCCESS == ret_result)
+                ret_result = result;
+
+        if (mldl_cfg->compass && mldl_cfg->compass->exit) {
+                result = mldl_cfg->compass->exit(compass_handle,
+                                                mldl_cfg->compass,
+                                                &mldl_cfg->pdata->compass);
+                if (ML_SUCCESS != result)
+                        MPL_LOGE("Compass exit failed %d\n", result);
+        }
+        if (ML_SUCCESS == ret_result)
+                ret_result = result;
+
+        if (mldl_cfg->pressure && mldl_cfg->pressure->exit) {
+                result = mldl_cfg->pressure->exit(pressure_handle,
+                                                mldl_cfg->pressure,
+                                                &mldl_cfg->pdata->pressure);
+                if (ML_SUCCESS != result)
+                        MPL_LOGE("Pressure exit failed %d\n", result);
+        }
+        if (ML_SUCCESS == ret_result)
+                ret_result = result;
+
+        return ret_result;
+}
+
+/**
+ *  @brief  resume the MPU3050 device and all the other sensor
+ *          devices from their low power state.
+ *
+ *  @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
+ *              onto a separate bus. Can match mlsl_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
+ *              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
+ *              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
+ *              mode of some sort).
+ *  @param  resume_compass
+ *              whether resuming the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @param  resume_pressure
+ *              whether resuming the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  ML_SUCCESS or a non-zero error code.
+ */
+int mpu3050_resume(struct mldl_cfg *mldl_cfg,
+                   void *gyro_handle,
+                   void *accel_handle,
+                   void *compass_handle,
+                   void *pressure_handle,
+                   bool resume_gyro,
+                   bool resume_accel,
+                   bool resume_compass,
+                   bool resume_pressure)
+{
+        int result = ML_SUCCESS;
+
+#ifdef CONFIG_MPU_SENSORS_DEBUG
+        mpu_print_cfg(mldl_cfg);
+#endif
+
+        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;
+
+        if (resume_gyro && mldl_cfg->gyro_is_suspended) {
+                result = gyro_resume(mldl_cfg, gyro_handle);
+                ERROR_CHECK(result);
+        }
+
+        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);
+                mldl_cfg->accel_is_suspended = FALSE;
+        }
+
+        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);
+                mldl_cfg->compass_is_suspended = FALSE;
+        }
+
+        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);
+        }
+
+        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 (!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 */
+        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  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 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 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 gyro_handle if
+ *              the pressure sensor device operates on the same
+ *              primary bus of MPU.
+ *  @param  accel
+ *              whether suspending the accelerometer device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @param  compass
+ *              whether suspending the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @param  pressure
+ *              whether suspending the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  ML_SUCCESS or a non-zero error code.
+ */
+int mpu3050_suspend(struct mldl_cfg *mldl_cfg,
+                    void *gyro_handle,
+                    void *accel_handle,
+                    void *compass_handle,
+                    void *pressure_handle,
+                    bool suspend_gyro,
+                    bool suspend_accel,
+                    bool suspend_compass,
+                    bool suspend_pressure)
+{
+        int result = ML_SUCCESS;
+
+        if (suspend_gyro && !mldl_cfg->gyro_is_suspended) {
+#ifdef M_HW
+                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, gyro_handle,
+                                        0, SLEEP, 0, 0, 0);
+#endif
+                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 (!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 (!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;
+}
+
+
+/**
+ *  @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 *accel_handle, unsigned char *data)
+{
+        if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->read)
+                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;
+}
+
+/**
+ *  @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 *compass_handle, unsigned char *data)
+{
+        if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->read)
+                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;
+}
+
+/**
+ *  @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 *pressure_handle, unsigned char *data)
+{
+        if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->read)
+                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;
+}
+
+int mpu3050_config_accel(struct mldl_cfg *mldl_cfg,
+                        void *accel_handle,
+                        struct ext_slave_config *data)
+{
+        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);
+        else
+                return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+}
+
+int mpu3050_config_compass(struct mldl_cfg *mldl_cfg,
+                        void *compass_handle,
+                        struct ext_slave_config *data)
+{
+        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);
+        else
+                return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+}
+
+int mpu3050_config_pressure(struct mldl_cfg *mldl_cfg,
+                        void *pressure_handle,
+                        struct ext_slave_config *data)
+{
+        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);
+        else
+                return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+
+/**
+ *@}
+ */
diff --git a/drivers/misc/mpu3050/mldl_cfg.h b/drivers/misc/mpu3050/mldl_cfg.h
new file mode 100644
index 00000000000..ad6a211c5d8
--- /dev/null
+++ b/drivers/misc/mpu3050/mldl_cfg.h
@@ -0,0 +1,199 @@
+/*
+ $License:
+    Copyright (C) 2010 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.h
+ *      @brief  The Motion Library Driver Layer Configuration header file.
+ */
+
+#ifndef __MLDL_CFG_H__
+#define __MLDL_CFG_H__
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include "mlsl.h"
+#include "mpu.h"
+
+/* --------------------- */
+/* -    Defines.       - */
+/* --------------------- */
+
+    /*************************************************************************/
+    /*  Sensors                                                              */
+    /*************************************************************************/
+
+#define ML_X_GYRO                       (0x0001)
+#define ML_Y_GYRO                       (0x0002)
+#define ML_Z_GYRO                       (0x0004)
+#define ML_DMP_PROCESSOR                (0x0008)
+
+#define ML_X_ACCEL                      (0x0010)
+#define ML_Y_ACCEL                      (0x0020)
+#define ML_Z_ACCEL                      (0x0040)
+
+#define ML_X_COMPASS                    (0x0080)
+#define ML_Y_COMPASS                    (0x0100)
+#define ML_Z_COMPASS                    (0x0200)
+
+#define ML_X_PRESSURE                   (0x0300)
+#define ML_Y_PRESSURE                   (0x0800)
+#define ML_Z_PRESSURE                   (0x1000)
+
+#define ML_TEMPERATURE                  (0x2000)
+#define ML_TIME                         (0x4000)
+
+#define ML_THREE_AXIS_GYRO              (0x000F)
+#define ML_THREE_AXIS_ACCEL             (0x0070)
+#define ML_THREE_AXIS_COMPASS           (0x0380)
+#define ML_THREE_AXIS_PRESSURE          (0x1C00)
+
+#define ML_FIVE_AXIS                    (0x007B)
+#define ML_SIX_AXIS_GYRO_ACCEL          (0x007F)
+#define ML_SIX_AXIS_ACCEL_COMPASS       (0x03F0)
+#define ML_NINE_AXIS                    (0x03FF)
+#define ML_ALL_SENSORS                  (0x7FFF)
+
+#define SAMPLING_RATE_HZ(mldl_cfg)                                      \
+        ((((((mldl_cfg)->lpf) == 0) || (((mldl_cfg)->lpf) == 7))        \
+                ? (8000)                                                \
+                : (1000))                                               \
+                / ((mldl_cfg)->divider + 1))
+
+#define SAMPLING_PERIOD_US(mldl_cfg)                                    \
+        ((1000000L * ((mldl_cfg)->divider + 1)) /                       \
+        (((((mldl_cfg)->lpf) == 0) || (((mldl_cfg)->lpf) == 7))         \
+                ? (8000)                                                \
+                : (1000)))
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/* Platform data for the MPU */
+struct mldl_cfg {
+        /* MPU related configuration */
+        unsigned long requested_sensors;
+        unsigned char ignore_system_suspend;
+        unsigned char addr;
+        unsigned char int_config;
+        unsigned char ext_sync;
+        unsigned char full_scale;
+        unsigned char lpf;
+        unsigned char clk_src;
+        unsigned char divider;
+        unsigned char dmp_enable;
+        unsigned char fifo_enable;
+        unsigned char dmp_cfg1;
+        unsigned char dmp_cfg2;
+        unsigned char gyro_power;
+        unsigned char offset_tc[MPU_NUM_AXES];
+        unsigned short offset[MPU_NUM_AXES];
+        unsigned char ram[MPU_MEM_NUM_RAM_BANKS][MPU_MEM_BANK_SIZE];
+
+        /* MPU Related stored status and info */
+        unsigned char silicon_revision;
+        unsigned char product_id;
+        unsigned short trim;
+
+        /* Driver/Kernel related state information */
+        int gyro_is_bypassed;
+        int dmp_is_running;
+        int gyro_is_suspended;
+        int accel_is_suspended;
+        int compass_is_suspended;
+        int pressure_is_suspended;
+        int gyro_needs_reset;
+
+        /* Slave related information */
+        struct ext_slave_descr *accel;
+        struct ext_slave_descr *compass;
+        struct ext_slave_descr *pressure;
+
+        /* Platform Data */
+        struct mpu3050_platform_data *pdata;
+};
+
+
+int mpu3050_open(struct mldl_cfg *mldl_cfg,
+                 void *mlsl_handle,
+                 void *accel_handle,
+                 void *compass_handle,
+                 void *pressure_handle);
+int mpu3050_close(struct mldl_cfg *mldl_cfg,
+                  void *mlsl_handle,
+                  void *accel_handle,
+                  void *compass_handle,
+                  void *pressure_handle);
+int mpu3050_resume(struct mldl_cfg *mldl_cfg,
+                   void *gyro_handle,
+                   void *accel_handle,
+                   void *compass_handle,
+                   void *pressure_handle,
+                   bool resume_gyro,
+                   bool resume_accel,
+                   bool resume_compass,
+                   bool resume_pressure);
+int mpu3050_suspend(struct mldl_cfg *mldl_cfg,
+                    void *gyro_handle,
+                    void *accel_handle,
+                    void *compass_handle,
+                    void *pressure_handle,
+                    bool suspend_gyro,
+                    bool suspend_accel,
+                    bool suspend_compass,
+                    bool suspend_pressure);
+int mpu3050_read_accel(struct mldl_cfg *mldl_cfg,
+                       void *accel_handle,
+                       unsigned char *data);
+int mpu3050_read_compass(struct mldl_cfg *mldl_cfg,
+                         void *compass_handle,
+                         unsigned char *data);
+int mpu3050_read_pressure(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+                          unsigned char *data);
+
+int mpu3050_config_accel(struct mldl_cfg *mldl_cfg,
+                         void *accel_handle,
+                         struct ext_slave_config *data);
+int mpu3050_config_compass(struct mldl_cfg *mldl_cfg,
+                           void *compass_handle,
+                           struct ext_slave_config *data);
+int mpu3050_config_pressure(struct mldl_cfg *mldl_cfg,
+                            void *pressure_handle,
+                            struct ext_slave_config *data);
+
+int mpu3050_get_config_accel(struct mldl_cfg *mldl_cfg,
+                             void *accel_handle,
+                             struct ext_slave_config *data);
+int mpu3050_get_config_compass(struct mldl_cfg *mldl_cfg,
+                               void *compass_handle,
+                               struct ext_slave_config *data);
+int mpu3050_get_config_pressure(struct mldl_cfg *mldl_cfg,
+                                void *pressure_handle,
+                                struct ext_slave_config *data);
+
+
+#endif                          /* __MLDL_CFG_H__ */
+
+/**
+ *@}
+ */
diff --git a/drivers/misc/mpu3050/mlos-kernel.c b/drivers/misc/mpu3050/mlos-kernel.c
new file mode 100644
index 00000000000..ced9ba4f6f3
--- /dev/null
+++ b/drivers/misc/mpu3050/mlos-kernel.c
@@ -0,0 +1,89 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+/**
+ * @defgroup
+ * @brief
+ *
+ * @{
+ * @file     mlos-kernel.c
+ * @brief
+ *
+ *
+ */
+
+#include "mlos.h"
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+void *MLOSMalloc(unsigned int numBytes)
+{
+        return kmalloc(numBytes, GFP_KERNEL);
+}
+
+tMLError MLOSFree(void *ptr)
+{
+        kfree(ptr);
+        return ML_SUCCESS;
+}
+
+tMLError MLOSCreateMutex(HANDLE *mutex)
+{
+        /* @todo implement if needed */
+        return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+tMLError MLOSLockMutex(HANDLE mutex)
+{
+        /* @todo implement if needed */
+        return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+tMLError MLOSUnlockMutex(HANDLE mutex)
+{
+        /* @todo implement if needed */
+        return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+tMLError MLOSDestroyMutex(HANDLE handle)
+{
+        /* @todo implement if needed */
+        return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+FILE *MLOSFOpen(char *filename)
+{
+        /* @todo implement if needed */
+        return NULL;
+}
+
+void MLOSFClose(FILE *fp)
+{
+        /* @todo implement if needed */
+}
+
+void MLOSSleep(int mSecs)
+{
+        msleep(mSecs);
+}
+
+unsigned long MLOSGetTickCount(void)
+{
+        /* @todo implement if needed */
+        return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
diff --git a/drivers/misc/mpu3050/mlos.h b/drivers/misc/mpu3050/mlos.h
new file mode 100644
index 00000000000..4ebb86c9fa5
--- /dev/null
+++ b/drivers/misc/mpu3050/mlos.h
@@ -0,0 +1,73 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+#ifndef _MLOS_H
+#define _MLOS_H
+
+#ifndef __KERNEL__
+#include <stdio.h>
+#endif
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+        /* ------------ */
+        /* - Defines. - */
+        /* ------------ */
+
+        /* - MLOSCreateFile defines. - */
+
+#define MLOS_GENERIC_READ         ((unsigned int)0x80000000)
+#define MLOS_GENERIC_WRITE        ((unsigned int)0x40000000)
+#define MLOS_FILE_SHARE_READ      ((unsigned int)0x00000001)
+#define MLOS_FILE_SHARE_WRITE     ((unsigned int)0x00000002)
+#define MLOS_OPEN_EXISTING        ((unsigned int)0x00000003)
+
+        /* ---------- */
+        /* - Enums. - */
+        /* ---------- */
+
+        /* --------------- */
+        /* - Structures. - */
+        /* --------------- */
+
+        /* --------------------- */
+        /* - Function p-types. - */
+        /* --------------------- */
+
+        void *MLOSMalloc(unsigned int numBytes);
+        tMLError MLOSFree(void *ptr);
+        tMLError MLOSCreateMutex(HANDLE *mutex);
+        tMLError MLOSLockMutex(HANDLE mutex);
+        tMLError MLOSUnlockMutex(HANDLE mutex);
+        FILE *MLOSFOpen(char *filename);
+        void MLOSFClose(FILE *fp);
+
+        tMLError MLOSDestroyMutex(HANDLE handle);
+
+        void MLOSSleep(int mSecs);
+        unsigned long MLOSGetTickCount(void);
+
+#ifdef __cplusplus
+}
+#endif
+#endif                          /* _MLOS_H */
diff --git a/drivers/misc/mpu3050/mlsl-kernel.c b/drivers/misc/mpu3050/mlsl-kernel.c
new file mode 100644
index 00000000000..cb1605131cb
--- /dev/null
+++ b/drivers/misc/mpu3050/mlsl-kernel.c
@@ -0,0 +1,331 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+#include "mlsl.h"
+#include "mpu-i2c.h"
+
+/* ------------ */
+/* - Defines. - */
+/* ------------ */
+
+/* ---------------------- */
+/* - Types definitions. - */
+/* ---------------------- */
+
+/* --------------------- */
+/* - Function p-types. - */
+/* --------------------- */
+
+/**
+ *  @brief  used to open the I2C or SPI serial port.
+ *          This port is used to send and receive data to the MPU device.
+ *  @param  portNum
+ *              The COM port number associated with the device in use.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+tMLError MLSLSerialOpen(char const *port, void **sl_handle)
+{
+        return ML_SUCCESS;
+}
+
+/**
+ *  @brief  used to reset any buffering the driver may be doing
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+tMLError MLSLSerialReset(void *sl_handle)
+{
+        return ML_SUCCESS;
+}
+
+/**
+ *  @brief  used to close the I2C or SPI serial port.
+ *          This port is used to send and receive data to the MPU device.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+tMLError MLSLSerialClose(void *sl_handle)
+{
+        return ML_SUCCESS;
+}
+
+/**
+ *  @brief  used to read a single byte of data.
+ *          This should be sent by I2C or SPI.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  registerAddr    Register address to read.
+ *  @param  data            Single byte of data to read.
+ *
+ *  @return ML_SUCCESS if the command is successful, an error code otherwise.
+ */
+tMLError MLSLSerialWriteSingle(void *sl_handle,
+                               unsigned char slaveAddr,
+                               unsigned char registerAddr,
+                               unsigned char data)
+{
+        return sensor_i2c_write_register((struct i2c_adapter *) sl_handle,
+                                         slaveAddr, registerAddr, data);
+}
+
+
+/**
+ *  @brief  used to write multiple bytes of data from registers.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  registerAddr    Register address to write.
+ *  @param  length          Length of burst of data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+tMLError MLSLSerialWrite(void *sl_handle,
+                         unsigned char slaveAddr,
+                         unsigned short length,
+                         unsigned char const *data)
+{
+        tMLError result;
+        const unsigned short dataLength = length - 1;
+        const unsigned char startRegAddr = data[0];
+        unsigned char i2cWrite[SERIAL_MAX_TRANSFER_SIZE + 1];
+        unsigned short bytesWritten = 0;
+
+        while (bytesWritten < dataLength) {
+                unsigned short thisLen = min(SERIAL_MAX_TRANSFER_SIZE,
+                                             dataLength - bytesWritten);
+                if (bytesWritten == 0) {
+                        result = sensor_i2c_write((struct i2c_adapter *)
+                                                  sl_handle, slaveAddr,
+                                                  1 + thisLen, data);
+                } else {
+                        /* manually increment register addr between chunks */
+                        i2cWrite[0] = startRegAddr + bytesWritten;
+                        memcpy(&i2cWrite[1], &data[1 + bytesWritten],
+                               thisLen);
+                        result = sensor_i2c_write((struct i2c_adapter *)
+                                                  sl_handle, slaveAddr,
+                                                  1 + thisLen, i2cWrite);
+                }
+                if (ML_SUCCESS != result)
+                        return result;
+                bytesWritten += thisLen;
+        }
+        return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to read multiple bytes of data from registers.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  registerAddr    Register address to read.
+ *  @param  length          Length of burst of data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialRead(void *sl_handle,
+                        unsigned char slaveAddr,
+                        unsigned char registerAddr,
+                        unsigned short length, unsigned char *data)
+{
+        tMLError result;
+        unsigned short bytesRead = 0;
+
+        if (registerAddr == MPUREG_FIFO_R_W
+            || registerAddr == MPUREG_MEM_R_W) {
+                return ML_ERROR_INVALID_PARAMETER;
+        }
+        while (bytesRead < length) {
+                unsigned short thisLen =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesRead);
+                result =
+                    sensor_i2c_read((struct i2c_adapter *) sl_handle,
+                                    slaveAddr, registerAddr + bytesRead,
+                                    thisLen, &data[bytesRead]);
+                if (ML_SUCCESS != result)
+                        return result;
+                bytesRead += thisLen;
+        }
+        return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to write multiple bytes of data to the memory.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  memAddr         The location in the memory to write to.
+ *  @param  length          Length of burst data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialWriteMem(void *sl_handle,
+                            unsigned char slaveAddr,
+                            unsigned short memAddr,
+                            unsigned short length,
+                            unsigned char const *data)
+{
+        tMLError result;
+        unsigned short bytesWritten = 0;
+
+        if ((memAddr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+                pr_err("memory read length (%d B) extends beyond its"
+                        " limits (%d) if started at location %d\n", length,
+                        MPU_MEM_BANK_SIZE, memAddr & 0xFF);
+                return ML_ERROR_INVALID_PARAMETER;
+        }
+        while (bytesWritten < length) {
+                unsigned short thisLen =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesWritten);
+                result =
+                    mpu_memory_write((struct i2c_adapter *) sl_handle,
+                                     slaveAddr, memAddr + bytesWritten,
+                                     thisLen, &data[bytesWritten]);
+                if (ML_SUCCESS != result)
+                        return result;
+                bytesWritten += thisLen;
+        }
+        return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to read multiple bytes of data from the memory.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  memAddr         The location in the memory to read from.
+ *  @param  length          Length of burst data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialReadMem(void *sl_handle,
+                           unsigned char slaveAddr,
+                           unsigned short memAddr,
+                           unsigned short length, unsigned char *data)
+{
+        tMLError result;
+        unsigned short bytesRead = 0;
+
+        if ((memAddr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+                printk
+                    ("memory read length (%d B) extends beyond its limits (%d) "
+                     "if started at location %d\n", length,
+                     MPU_MEM_BANK_SIZE, memAddr & 0xFF);
+                return ML_ERROR_INVALID_PARAMETER;
+        }
+        while (bytesRead < length) {
+                unsigned short thisLen =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesRead);
+                result =
+                    mpu_memory_read((struct i2c_adapter *) sl_handle,
+                                    slaveAddr, memAddr + bytesRead,
+                                    thisLen, &data[bytesRead]);
+                if (ML_SUCCESS != result)
+                        return result;
+                bytesRead += thisLen;
+        }
+        return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to write multiple bytes of data to the fifo.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  length          Length of burst of data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialWriteFifo(void *sl_handle,
+                             unsigned char slaveAddr,
+                             unsigned short length,
+                             unsigned char const *data)
+{
+        tMLError result;
+        unsigned char i2cWrite[SERIAL_MAX_TRANSFER_SIZE + 1];
+        unsigned short bytesWritten = 0;
+
+        if (length > FIFO_HW_SIZE) {
+                printk(KERN_ERR
+                       "maximum fifo write length is %d\n", FIFO_HW_SIZE);
+                return ML_ERROR_INVALID_PARAMETER;
+        }
+        while (bytesWritten < length) {
+                unsigned short thisLen =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesWritten);
+                i2cWrite[0] = MPUREG_FIFO_R_W;
+                memcpy(&i2cWrite[1], &data[bytesWritten], thisLen);
+                result = sensor_i2c_write((struct i2c_adapter *) sl_handle,
+                                          slaveAddr, thisLen + 1,
+                                          i2cWrite);
+                if (ML_SUCCESS != result)
+                        return result;
+                bytesWritten += thisLen;
+        }
+        return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to read multiple bytes of data from the fifo.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  length          Length of burst of data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialReadFifo(void *sl_handle,
+                            unsigned char slaveAddr,
+                            unsigned short length, unsigned char *data)
+{
+        tMLError result;
+        unsigned short bytesRead = 0;
+
+        if (length > FIFO_HW_SIZE) {
+                printk(KERN_ERR
+                       "maximum fifo read length is %d\n", FIFO_HW_SIZE);
+                return ML_ERROR_INVALID_PARAMETER;
+        }
+        while (bytesRead < length) {
+                unsigned short thisLen =
+                    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesRead);
+                result =
+                    sensor_i2c_read((struct i2c_adapter *) sl_handle,
+                                    slaveAddr, MPUREG_FIFO_R_W, thisLen,
+                                    &data[bytesRead]);
+                if (ML_SUCCESS != result)
+                        return result;
+                bytesRead += thisLen;
+        }
+
+        return ML_SUCCESS;
+}
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/mlsl.h b/drivers/misc/mpu3050/mlsl.h
new file mode 100644
index 00000000000..76f69c77ba9
--- /dev/null
+++ b/drivers/misc/mpu3050/mlsl.h
@@ -0,0 +1,103 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+#ifndef __MSSL_H__
+#define __MSSL_H__
+
+#include "mltypes.h"
+#include "mpu.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* ------------ */
+/* - Defines. - */
+/* ------------ */
+
+/*
+ * NOTE : to properly support Yamaha compass reads,
+ * the max transfer size should be at least 9 B.
+ * Length in bytes, typically a power of 2 >= 2
+ */
+#define SERIAL_MAX_TRANSFER_SIZE 128
+
+/* ---------------------- */
+/* - Types definitions. - */
+/* ---------------------- */
+
+/* --------------------- */
+/* - Function p-types. - */
+/* --------------------- */
+
+        tMLError MLSLSerialOpen(char const *port,
+                                void **sl_handle);
+        tMLError MLSLSerialReset(void *sl_handle);
+        tMLError MLSLSerialClose(void *sl_handle);
+
+        tMLError MLSLSerialWriteSingle(void *sl_handle,
+                                       unsigned char slaveAddr,
+                                       unsigned char registerAddr,
+                                       unsigned char data);
+
+        tMLError MLSLSerialRead(void *sl_handle,
+                                unsigned char slaveAddr,
+                                unsigned char registerAddr,
+                                unsigned short length,
+                                unsigned char *data);
+
+        tMLError MLSLSerialWrite(void *sl_handle,
+                                 unsigned char slaveAddr,
+                                 unsigned short length,
+                                 unsigned char const *data);
+
+        tMLError MLSLSerialReadMem(void *sl_handle,
+                                   unsigned char slaveAddr,
+                                   unsigned short memAddr,
+                                   unsigned short length,
+                                   unsigned char *data);
+
+        tMLError MLSLSerialWriteMem(void *sl_handle,
+                                    unsigned char slaveAddr,
+                                    unsigned short memAddr,
+                                    unsigned short length,
+                                    unsigned char const *data);
+
+        tMLError MLSLSerialReadFifo(void *sl_handle,
+                                    unsigned char slaveAddr,
+                                    unsigned short length,
+                                    unsigned char *data);
+
+        tMLError MLSLSerialWriteFifo(void *sl_handle,
+                                     unsigned char slaveAddr,
+                                     unsigned short length,
+                                     unsigned char const *data);
+
+        tMLError MLSLWriteCal(unsigned char *cal, unsigned int len);
+        tMLError MLSLReadCal(unsigned char *cal, unsigned int len);
+        tMLError MLSLGetCalLength(unsigned int *len);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+#endif                          /* MLSL_H */
diff --git a/drivers/misc/mpu3050/mltypes.h b/drivers/misc/mpu3050/mltypes.h
new file mode 100644
index 00000000000..d0b27fa89e7
--- /dev/null
+++ b/drivers/misc/mpu3050/mltypes.h
@@ -0,0 +1,227 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+/**
+ *  @defgroup MLERROR
+ *  @brief  Motion Library - Error definitions.
+ *          Definition of the error codes used within the MPL and returned
+ *          to the user.
+ *          Every function tries to return a meaningful error code basing
+ *          on the occuring error condition. The error code is numeric.
+ *
+ *          The available error codes and their associated values are:
+ *          - (0)       ML_SUCCESS
+ *          - (1)       ML_ERROR
+ *          - (2)       ML_ERROR_INVALID_PARAMETER
+ *          - (3)       ML_ERROR_FEATURE_NOT_ENABLED
+ *          - (4)       ML_ERROR_FEATURE_NOT_IMPLEMENTED
+ *          - (6)       ML_ERROR_DMP_NOT_STARTED
+ *          - (7)       ML_ERROR_DMP_STARTED
+ *          - (8)       ML_ERROR_NOT_OPENED
+ *          - (9)       ML_ERROR_OPENED
+ *          - (10)      ML_ERROR_INVALID_MODULE
+ *          - (11)      ML_ERROR_MEMORY_EXAUSTED
+ *          - (12)      ML_ERROR_DIVIDE_BY_ZERO
+ *          - (13)      ML_ERROR_ASSERTION_FAILURE
+ *          - (14)      ML_ERROR_FILE_OPEN
+ *          - (15)      ML_ERROR_FILE_READ
+ *          - (16)      ML_ERROR_FILE_WRITE
+ *          - (20)      ML_ERROR_SERIAL_CLOSED
+ *          - (21)      ML_ERROR_SERIAL_OPEN_ERROR
+ *          - (22)      ML_ERROR_SERIAL_READ
+ *          - (23)      ML_ERROR_SERIAL_WRITE
+ *          - (24)      ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
+ *          - (25)      ML_ERROR_SM_TRANSITION
+ *          - (26)      ML_ERROR_SM_IMPROPER_STATE
+ *          - (30)      ML_ERROR_FIFO_OVERFLOW
+ *          - (31)      ML_ERROR_FIFO_FOOTER
+ *          - (32)      ML_ERROR_FIFO_READ_COUNT
+ *          - (33)      ML_ERROR_FIFO_READ_DATA
+ *          - (40)      ML_ERROR_MEMORY_SET
+ *          - (50)      ML_ERROR_LOG_MEMORY_ERROR
+ *          - (51)      ML_ERROR_LOG_OUTPUT_ERROR
+ *          - (60)      ML_ERROR_OS_BAD_PTR
+ *          - (61)      ML_ERROR_OS_BAD_HANDLE
+ *          - (62)      ML_ERROR_OS_CREATE_FAILED
+ *          - (63)      ML_ERROR_OS_LOCK_FAILED
+ *          - (70)      ML_ERROR_COMPASS_DATA_OVERFLOW
+ *          - (71)      ML_ERROR_COMPASS_DATA_UNDERFLOW
+ *          - (72)      ML_ERROR_COMPASS_DATA_NOT_READY
+ *          - (73)      ML_ERROR_COMPASS_DATA_ERROR
+ *          - (75)      ML_ERROR_CALIBRATION_LOAD
+ *          - (76)      ML_ERROR_CALIBRATION_STORE
+ *          - (77)      ML_ERROR_CALIBRATION_LEN
+ *          - (78)      ML_ERROR_CALIBRATION_CHECKSUM
+ *
+ *  @{
+ *      @file mltypes.h
+ *  @}
+ */
+
+#ifndef MLTYPES_H
+#define MLTYPES_H
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#else
+#include "stdint_invensense.h"
+#endif
+#include "log.h"
+
+/*---------------------------
+    ML Types
+---------------------------*/
+
+/**
+ *  @struct tMLError mltypes.h "mltypes"
+ *  @brief  The MPL Error Code return type.
+ *
+ *  @code
+ *      typedef unsigned char tMLError;
+ *  @endcode
+ */
+typedef unsigned char tMLError;
+
+#if defined(LINUX) || defined(__KERNEL__)
+typedef unsigned int HANDLE;
+#endif
+
+#ifdef __KERNEL__
+typedef HANDLE FILE;
+#endif
+
+#ifndef __cplusplus
+#ifndef __KERNEL__
+typedef int_fast8_t bool;
+#endif
+#endif
+
+/*---------------------------
+    ML Defines
+---------------------------*/
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+/* Dimension of an array */
+#ifndef DIM
+#define DIM(array) (sizeof(array)/sizeof((array)[0]))
+#endif
+
+/* - ML Errors. - */
+#define ERROR_NAME(x)   (#x)
+#define ERROR_CHECK(x)                                                  \
+        do {                                                            \
+                if (ML_SUCCESS != x) {                                  \
+                        MPL_LOGE("%s|%s|%d returning %d\n",             \
+                                __FILE__, __func__, __LINE__, x);       \
+                        return x;                                       \
+                }                                                       \
+        } while (0)
+
+#define ERROR_CHECK_FIRST(first, x)                                     \
+        { if (ML_SUCCESS == first) first = x; }
+
+#define ML_SUCCESS                       (0)
+/* Generic Error code.  Proprietary Error Codes only */
+#define ML_ERROR                         (1)
+
+/* Compatibility and other generic error codes */
+#define ML_ERROR_INVALID_PARAMETER       (2)
+#define ML_ERROR_FEATURE_NOT_ENABLED     (3)
+#define ML_ERROR_FEATURE_NOT_IMPLEMENTED (4)
+#define ML_ERROR_DMP_NOT_STARTED         (6)
+#define ML_ERROR_DMP_STARTED             (7)
+#define ML_ERROR_NOT_OPENED              (8)
+#define ML_ERROR_OPENED                  (9)
+#define ML_ERROR_INVALID_MODULE         (10)
+#define ML_ERROR_MEMORY_EXAUSTED        (11)
+#define ML_ERROR_DIVIDE_BY_ZERO         (12)
+#define ML_ERROR_ASSERTION_FAILURE      (13)
+#define ML_ERROR_FILE_OPEN              (14)
+#define ML_ERROR_FILE_READ              (15)
+#define ML_ERROR_FILE_WRITE             (16)
+#define ML_ERROR_INVALID_CONFIGURATION  (17)
+
+/* Serial Communication */
+#define ML_ERROR_SERIAL_CLOSED          (20)
+#define ML_ERROR_SERIAL_OPEN_ERROR      (21)
+#define ML_ERROR_SERIAL_READ            (22)
+#define ML_ERROR_SERIAL_WRITE           (23)
+#define ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  (24)
+
+/* SM = State Machine */
+#define ML_ERROR_SM_TRANSITION          (25)
+#define ML_ERROR_SM_IMPROPER_STATE      (26)
+
+/* Fifo */
+#define ML_ERROR_FIFO_OVERFLOW          (30)
+#define ML_ERROR_FIFO_FOOTER            (31)
+#define ML_ERROR_FIFO_READ_COUNT        (32)
+#define ML_ERROR_FIFO_READ_DATA         (33)
+
+/* Memory & Registers, Set & Get */
+#define ML_ERROR_MEMORY_SET             (40)
+
+#define ML_ERROR_LOG_MEMORY_ERROR       (50)
+#define ML_ERROR_LOG_OUTPUT_ERROR       (51)
+
+/* OS interface errors */
+#define ML_ERROR_OS_BAD_PTR             (60)
+#define ML_ERROR_OS_BAD_HANDLE          (61)
+#define ML_ERROR_OS_CREATE_FAILED       (62)
+#define ML_ERROR_OS_LOCK_FAILED         (63)
+
+/* Compass errors */
+#define ML_ERROR_COMPASS_DATA_OVERFLOW  (70)
+#define ML_ERROR_COMPASS_DATA_UNDERFLOW (71)
+#define ML_ERROR_COMPASS_DATA_NOT_READY (72)
+#define ML_ERROR_COMPASS_DATA_ERROR     (73)
+
+/* Load/Store calibration */
+#define ML_ERROR_CALIBRATION_LOAD       (75)
+#define ML_ERROR_CALIBRATION_STORE      (76)
+#define ML_ERROR_CALIBRATION_LEN        (77)
+#define ML_ERROR_CALIBRATION_CHECKSUM   (78)
+
+/* Accel errors */
+#define ML_ERROR_ACCEL_DATA_OVERFLOW    (79)
+#define ML_ERROR_ACCEL_DATA_UNDERFLOW   (80)
+#define ML_ERROR_ACCEL_DATA_NOT_READY   (81)
+#define ML_ERROR_ACCEL_DATA_ERROR       (82)
+
+/* For Linux coding compliance */
+#ifndef __KERNEL__
+#define EXPORT_SYMBOL(x)
+#endif
+
+/*---------------------------
+    p-Types
+---------------------------*/
+
+#endif                          /* MLTYPES_H */
diff --git a/drivers/misc/mpu3050/mpu-dev.c b/drivers/misc/mpu3050/mpu-dev.c
new file mode 100644
index 00000000000..4dba44d4548
--- /dev/null
+++ b/drivers/misc/mpu3050/mpu-dev.c
@@ -0,0 +1,1310 @@
+/*
+    mpu-dev.c - mpu3050 char device interface  2  $License:
+
+    Copyright (C) 1995-97 Simon G. Vogl
+    Copyright (C) 1998-99 Frodo Looijaard <frodol@dds.nl>
+    Copyright (C) 2003 Greg Kroah-Hartman <greg@kroah.com>
+
+    Copyright (C) 2010 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/>.
+ */
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/pm.h>
+#include <linux/mutex.h>
+#include <linux/suspend.h>
+#include <linux/poll.h>
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "mpuirq.h"
+#include "slaveirq.h"
+#include "mlsl.h"
+#include "mpu-i2c.h"
+#include "mldl_cfg.h"
+#include "mpu.h"
+
+#define MPU3050_EARLY_SUSPEND_IN_DRIVER 0
+
+/* Platform data for the MPU */
+struct mpu_private_data {
+        struct mldl_cfg mldl_cfg;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        struct early_suspend early_suspend;
+#endif
+        struct mutex mutex;
+        wait_queue_head_t mpu_event_wait;
+        struct completion completion;
+        struct timer_list timeout;
+        struct notifier_block nb;
+        struct mpuirq_data mpu_pm_event;
+        int response_timeout;   /* In seconds */
+        unsigned long event;
+        int pid;
+};
+
+static struct i2c_client *this_client;
+
+
+static void
+mpu_pm_timeout(u_long data)
+{
+        struct mpu_private_data *mpu = (struct mpu_private_data *) data;
+        dev_dbg(&this_client->adapter->dev, "%s\n", __func__);
+        complete(&mpu->completion);
+}
+
+static int mpu_pm_notifier_callback(struct notifier_block *nb,
+                                    unsigned long event,
+                                    void *unused)
+{
+        struct mpu_private_data *mpu =
+                container_of(nb, struct mpu_private_data, nb);
+        struct timeval event_time;
+        dev_dbg(&this_client->adapter->dev, "%s: %ld\n", __func__, event);
+
+        /* Prevent the file handle from being closed before we initialize
+           the completion event */
+        mutex_lock(&mpu->mutex);
+        if (!(mpu->pid) ||
+                (event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) {
+                mutex_unlock(&mpu->mutex);
+                return NOTIFY_OK;
+        }
+
+        do_gettimeofday(&event_time);
+        mpu->mpu_pm_event.interruptcount++;
+        mpu->mpu_pm_event.irqtime =
+                (((long long) event_time.tv_sec) << 32) +
+                event_time.tv_usec;
+        mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT;
+        mpu->mpu_pm_event.data_size = sizeof(unsigned long);
+        mpu->mpu_pm_event.data = &mpu->event;
+
+        if (event == PM_SUSPEND_PREPARE)
+                mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE;
+        if (event == PM_POST_SUSPEND)
+                mpu->event = MPU_PM_EVENT_POST_SUSPEND;
+
+        if (mpu->response_timeout > 0) {
+                mpu->timeout.expires = jiffies + mpu->response_timeout * HZ;
+                add_timer(&mpu->timeout);
+        }
+        INIT_COMPLETION(mpu->completion);
+        mutex_unlock(&mpu->mutex);
+
+        wake_up_interruptible(&mpu->mpu_event_wait);
+        wait_for_completion(&mpu->completion);
+        del_timer_sync(&mpu->timeout);
+        dev_dbg(&this_client->adapter->dev, "%s: %ld DONE\n", __func__, event);
+        return NOTIFY_OK;
+}
+
+static int mpu_open(struct inode *inode, struct file *file)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(this_client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        int result;
+        dev_dbg(&this_client->adapter->dev, "mpu_open\n");
+        dev_dbg(&this_client->adapter->dev, "current->pid %d\n",
+                current->pid);
+        mpu->pid = current->pid;
+        file->private_data = this_client;
+        /* we could do some checking on the flags supplied by "open" */
+        /* i.e. O_NONBLOCK */
+        /* -> set some flag to disable interruptible_sleep_on in mpu_read */
+
+        /* Reset the sensors to the default */
+        result = mutex_lock_interruptible(&mpu->mutex);
+        if (result) {
+                dev_err(&this_client->adapter->dev,
+                        "%s: mutex_lock_interruptible returned %d\n",
+                        __func__, result);
+                return result;
+        }
+        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;
+        mutex_unlock(&mpu->mutex);
+        return 0;
+}
+
+/* close function - called when the "file" /dev/mpu is closed in userspace   */
+static int mpu_release(struct inode *inode, struct file *file)
+{
+        struct i2c_client *client =
+            (struct i2c_client *) file->private_data;
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *accel_adapter;
+        struct i2c_adapter *compass_adapter;
+        struct i2c_adapter *pressure_adapter;
+        int result = 0;
+
+        accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+        compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+        pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+        mutex_lock(&mpu->mutex);
+        result = mpu3050_suspend(mldl_cfg, client->adapter,
+                                 accel_adapter, compass_adapter,
+                                 pressure_adapter,
+                                 TRUE, TRUE, TRUE, TRUE);
+        mpu->pid = 0;
+        mutex_unlock(&mpu->mutex);
+        complete(&mpu->completion);
+        dev_dbg(&this_client->adapter->dev, "mpu_release\n");
+        return result;
+}
+
+/* read function called when from /dev/mpu is read.  Read from the FIFO */
+static ssize_t mpu_read(struct file *file,
+                        char __user *buf, size_t count, loff_t *offset)
+{
+        struct mpuirq_data local_mpu_pm_event;
+        struct i2c_client *client =
+            (struct i2c_client *) file->private_data;
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(client);
+        size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long);
+        int err;
+
+        if (!mpu->event && (!(file->f_flags & O_NONBLOCK)))
+                wait_event_interruptible(mpu->mpu_event_wait, mpu->event);
+
+        if (!mpu->event || NULL == buf
+                || count < sizeof(mpu->mpu_pm_event) + sizeof(unsigned long))
+                return 0;
+
+        err = copy_from_user(&local_mpu_pm_event, buf,
+                        sizeof(mpu->mpu_pm_event));
+        if (err != 0) {
+                dev_err(&this_client->adapter->dev,
+                        "Copy from user returned %d\n", err);
+                return -EFAULT;
+        }
+
+        mpu->mpu_pm_event.data = local_mpu_pm_event.data;
+        err = copy_to_user((unsigned long __user *)local_mpu_pm_event.data,
+                        &mpu->event,
+                        sizeof(mpu->event));
+        if (err != 0) {
+                dev_err(&this_client->adapter->dev,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event));
+        if (err != 0) {
+                dev_err(&this_client->adapter->dev,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        mpu->event = 0;
+        return len;
+}
+
+static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll)
+{
+        struct i2c_client *client =
+            (struct i2c_client *) file->private_data;
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(client);
+        int mask = 0;
+
+        poll_wait(file, &mpu->mpu_event_wait, poll);
+        if (mpu->event)
+                mask |= POLLIN | POLLRDNORM;
+        return mask;
+}
+
+static int
+mpu_ioctl_set_mpu_pdata(struct i2c_client *client, unsigned long arg)
+{
+        int ii;
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(client);
+        struct mpu3050_platform_data *pdata = mpu->mldl_cfg.pdata;
+        struct mpu3050_platform_data local_pdata;
+
+        if (copy_from_user(&local_pdata, (unsigned char __user *) arg,
+                                sizeof(local_pdata)))
+                return -EFAULT;
+
+        pdata->int_config = local_pdata.int_config;
+        for (ii = 0; ii < DIM(pdata->orientation); ii++)
+                pdata->orientation[ii] = local_pdata.orientation[ii];
+        pdata->level_shifter = local_pdata.level_shifter;
+
+        pdata->accel.address = local_pdata.accel.address;
+        for (ii = 0; ii < DIM(pdata->accel.orientation); ii++)
+                pdata->accel.orientation[ii] =
+                        local_pdata.accel.orientation[ii];
+
+        pdata->compass.address = local_pdata.compass.address;
+        for (ii = 0; ii < DIM(pdata->compass.orientation); ii++)
+                pdata->compass.orientation[ii] =
+                        local_pdata.compass.orientation[ii];
+
+        pdata->pressure.address = local_pdata.pressure.address;
+        for (ii = 0; ii < DIM(pdata->pressure.orientation); ii++)
+                pdata->pressure.orientation[ii] =
+                        local_pdata.pressure.orientation[ii];
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        return ML_SUCCESS;
+}
+
+static int
+mpu_ioctl_set_mpu_config(struct i2c_client *client, unsigned long arg)
+{
+        int ii;
+        int result = ML_SUCCESS;
+        struct mpu_private_data *mpu =
+                (struct mpu_private_data *) i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct mldl_cfg *temp_mldl_cfg;
+
+        dev_dbg(&this_client->adapter->dev, "%s\n", __func__);
+
+        temp_mldl_cfg = kzalloc(sizeof(struct mldl_cfg), GFP_KERNEL);
+        if (NULL == temp_mldl_cfg)
+                return -ENOMEM;
+
+        /*
+         * User space is not allowed to modify accel compass pressure or
+         * pdata structs, as well as silicon_revision product_id or trim
+         */
+        if (copy_from_user(temp_mldl_cfg, (struct mldl_cfg __user *) arg,
+                                offsetof(struct mldl_cfg, silicon_revision))) {
+                result = -EFAULT;
+                goto out;
+        }
+
+        if (mldl_cfg->gyro_is_suspended) {
+                if (mldl_cfg->addr != temp_mldl_cfg->addr)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->int_config != temp_mldl_cfg->int_config)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->ext_sync != temp_mldl_cfg->ext_sync)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->full_scale != temp_mldl_cfg->full_scale)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->lpf != temp_mldl_cfg->lpf)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->clk_src != temp_mldl_cfg->clk_src)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->divider != temp_mldl_cfg->divider)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->dmp_enable != temp_mldl_cfg->dmp_enable)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->fifo_enable != temp_mldl_cfg->fifo_enable)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->dmp_cfg1 != temp_mldl_cfg->dmp_cfg1)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->dmp_cfg2 != temp_mldl_cfg->dmp_cfg2)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (mldl_cfg->gyro_power != temp_mldl_cfg->gyro_power)
+                        mldl_cfg->gyro_needs_reset = TRUE;
+
+                for (ii = 0; ii < MPU_NUM_AXES; ii++)
+                        if (mldl_cfg->offset_tc[ii] !=
+                            temp_mldl_cfg->offset_tc[ii])
+                                mldl_cfg->gyro_needs_reset = TRUE;
+
+                for (ii = 0; ii < MPU_NUM_AXES; ii++)
+                        if (mldl_cfg->offset[ii] != temp_mldl_cfg->offset[ii])
+                                mldl_cfg->gyro_needs_reset = TRUE;
+
+                if (memcmp(mldl_cfg->ram, temp_mldl_cfg->ram,
+                                MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE *
+                                sizeof(unsigned char)))
+                        mldl_cfg->gyro_needs_reset = TRUE;
+        }
+
+        memcpy(mldl_cfg, temp_mldl_cfg,
+                offsetof(struct mldl_cfg, silicon_revision));
+
+out:
+        kfree(temp_mldl_cfg);
+        return result;
+}
+
+static int
+mpu_ioctl_get_mpu_config(struct i2c_client *client, unsigned long arg)
+{
+        /* Have to be careful as there are 3 pointers in the mldl_cfg
+         * structure */
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct mldl_cfg *local_mldl_cfg;
+        int retval = 0;
+
+        local_mldl_cfg = kzalloc(sizeof(struct mldl_cfg), GFP_KERNEL);
+        if (NULL == local_mldl_cfg)
+                return -ENOMEM;
+
+        retval =
+            copy_from_user(local_mldl_cfg, (struct mldl_cfg __user *) arg,
+                           sizeof(struct mldl_cfg));
+        if (retval) {
+                dev_err(&this_client->adapter->dev,
+                        "%s|%s:%d: EFAULT on arg\n",
+                        __FILE__, __func__, __LINE__);
+                retval = -EFAULT;
+                goto out;
+        }
+
+        /* Fill in the accel, compass, pressure and pdata pointers */
+        if (mldl_cfg->accel) {
+                retval = copy_to_user((void __user *)local_mldl_cfg->accel,
+                                      mldl_cfg->accel,
+                                      sizeof(*mldl_cfg->accel));
+                if (retval) {
+                        dev_err(&this_client->adapter->dev,
+                                "%s|%s:%d: EFAULT on accel\n",
+                                __FILE__, __func__, __LINE__);
+                        retval = -EFAULT;
+                        goto out;
+                }
+        }
+
+        if (mldl_cfg->compass) {
+                retval = copy_to_user((void __user *)local_mldl_cfg->compass,
+                                      mldl_cfg->compass,
+                                      sizeof(*mldl_cfg->compass));
+                if (retval) {
+                        dev_err(&this_client->adapter->dev,
+                                "%s|%s:%d: EFAULT on compass\n",
+                                __FILE__, __func__, __LINE__);
+                        retval = -EFAULT;
+                        goto out;
+                }
+        }
+
+        if (mldl_cfg->pressure) {
+                retval = copy_to_user((void __user *)local_mldl_cfg->pressure,
+                                      mldl_cfg->pressure,
+                                      sizeof(*mldl_cfg->pressure));
+                if (retval) {
+                        dev_err(&this_client->adapter->dev,
+                                "%s|%s:%d: EFAULT on pressure\n",
+                                __FILE__, __func__, __LINE__);
+                        retval = -EFAULT;
+                        goto out;
+                }
+        }
+
+        if (mldl_cfg->pdata) {
+                retval = copy_to_user((void __user *)local_mldl_cfg->pdata,
+                                      mldl_cfg->pdata,
+                                      sizeof(*mldl_cfg->pdata));
+                if (retval) {
+                        dev_err(&this_client->adapter->dev,
+                                "%s|%s:%d: EFAULT on pdata\n",
+                                __FILE__, __func__, __LINE__);
+                        retval = -EFAULT;
+                        goto out;
+                }
+        }
+
+        /* Do not modify the accel, compass, pressure and pdata pointers */
+        retval = copy_to_user((struct mldl_cfg __user *) arg,
+                              mldl_cfg, offsetof(struct mldl_cfg, accel));
+
+        if (retval)
+                retval = -EFAULT;
+out:
+        kfree(local_mldl_cfg);
+        return retval;
+}
+
+/**
+ * Pass a requested slave configuration to the slave sensor
+ *
+ * @param adapter the adaptor to use to communicate with the slave
+ * @param mldl_cfg the mldl configuration structuer
+ * @param slave pointer to the slave descriptor
+ * @param usr_config The configuration to pass to the slave sensor
+ *
+ * @return 0 or non-zero error code
+ */
+static int slave_config(void *adapter,
+                        struct mldl_cfg *mldl_cfg,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_config __user *usr_config)
+{
+        int retval = ML_SUCCESS;
+        struct ext_slave_config config;
+        if ((!slave) || (!slave->config))
+                return retval;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        if (config.len && config.data) {
+                int *data;
+                data = kzalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return ML_ERROR_MEMORY_EXAUSTED;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data,
+                                        config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = slave->config(adapter, slave, pdata, &config);
+        kfree(config.data);
+        return retval;
+}
+
+/**
+ * Get a requested slave configuration from the slave sensor
+ *
+ * @param adapter the adaptor to use to communicate with the slave
+ * @param mldl_cfg the mldl configuration structuer
+ * @param slave pointer to the slave descriptor
+ * @param usr_config The configuration for the slave to fill out
+ *
+ * @return 0 or non-zero error code
+ */
+static int slave_get_config(void *adapter,
+                        struct mldl_cfg *mldl_cfg,
+                        struct ext_slave_descr *slave,
+                        struct ext_slave_platform_data *pdata,
+                        struct ext_slave_config __user *usr_config)
+{
+        int retval = ML_SUCCESS;
+        struct ext_slave_config config;
+        void *user_data;
+        if (!(slave) || !(slave->get_config))
+                return ML_SUCCESS;
+
+        retval = copy_from_user(&config, usr_config, sizeof(config));
+        if (retval)
+                return -EFAULT;
+
+        user_data = config.data;
+        if (config.len && config.data) {
+                int *data;
+                data = kzalloc(config.len, GFP_KERNEL);
+                if (!data)
+                        return ML_ERROR_MEMORY_EXAUSTED;
+
+                retval = copy_from_user(data,
+                                        (void __user *)config.data,
+                                        config.len);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                config.data = data;
+        }
+        retval = slave->get_config(adapter, slave, pdata, &config);
+        if (retval) {
+                kfree(config.data);
+                return retval;
+        }
+        retval = copy_to_user((unsigned char __user *) user_data,
+                        config.data,
+                        config.len);
+        kfree(config.data);
+        return retval;
+}
+
+static int mpu_handle_mlsl(void *sl_handle,
+                        unsigned char addr,
+                        unsigned int cmd,
+                        struct mpu_read_write __user *usr_msg)
+{
+        int retval = ML_SUCCESS;
+        struct mpu_read_write msg;
+        unsigned char *user_data;
+        retval = copy_from_user(&msg, usr_msg, sizeof(msg));
+        if (retval)
+                return -EFAULT;
+
+        user_data = msg.data;
+        if (msg.length && msg.data) {
+                unsigned char *data;
+                data = kzalloc(msg.length, GFP_KERNEL);
+                if (!data)
+                        return ML_ERROR_MEMORY_EXAUSTED;
+
+                retval = copy_from_user(data,
+                                        (void __user *)msg.data,
+                                        msg.length);
+                if (retval) {
+                        retval = -EFAULT;
+                        kfree(data);
+                        return retval;
+                }
+                msg.data = data;
+        } else {
+                return ML_ERROR_INVALID_PARAMETER;
+        }
+
+        switch (cmd) {
+        case MPU_READ:
+                retval = MLSLSerialRead(sl_handle, addr,
+                                        msg.address, msg.length, msg.data);
+                break;
+        case MPU_WRITE:
+                retval = MLSLSerialWrite(sl_handle, addr,
+                                        msg.length, msg.data);
+                break;
+        case MPU_READ_MEM:
+                retval = MLSLSerialReadMem(sl_handle, addr,
+                                        msg.address, msg.length, msg.data);
+                break;
+        case MPU_WRITE_MEM:
+                retval = MLSLSerialWriteMem(sl_handle, addr,
+                                        msg.address, msg.length, msg.data);
+                break;
+        case MPU_READ_FIFO:
+                retval = MLSLSerialReadFifo(sl_handle, addr,
+                                        msg.length, msg.data);
+                break;
+        case MPU_WRITE_FIFO:
+                retval = MLSLSerialWriteFifo(sl_handle, addr,
+                                        msg.length, msg.data);
+                break;
+
+        };
+        retval = copy_to_user((unsigned char __user *) user_data,
+                        msg.data,
+                        msg.length);
+        kfree(msg.data);
+        return retval;
+}
+
+/* ioctl - I/O control */
+static long mpu_ioctl(struct file *file,
+                      unsigned int cmd, unsigned long arg)
+{
+        struct i2c_client *client =
+            (struct i2c_client *) file->private_data;
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        int retval = 0;
+        struct i2c_adapter *accel_adapter;
+        struct i2c_adapter *compass_adapter;
+        struct i2c_adapter *pressure_adapter;
+
+        accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+        compass_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+        pressure_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+        retval = mutex_lock_interruptible(&mpu->mutex);
+        if (retval) {
+                dev_err(&this_client->adapter->dev,
+                        "%s: mutex_lock_interruptible returned %d\n",
+                        __func__, retval);
+                return retval;
+        }
+
+        switch (cmd) {
+        case MPU_SET_MPU_CONFIG:
+                retval = mpu_ioctl_set_mpu_config(client, arg);
+                break;
+        case MPU_SET_PLATFORM_DATA:
+                retval = mpu_ioctl_set_mpu_pdata(client, arg);
+                break;
+        case MPU_GET_MPU_CONFIG:
+                retval = mpu_ioctl_get_mpu_config(client, arg);
+                break;
+        case MPU_READ:
+        case MPU_WRITE:
+        case MPU_READ_MEM:
+        case MPU_WRITE_MEM:
+        case MPU_READ_FIFO:
+        case MPU_WRITE_FIFO:
+                retval = mpu_handle_mlsl(client->adapter, mldl_cfg->addr, cmd,
+                                        (struct mpu_read_write __user *) arg);
+                break;
+        case MPU_CONFIG_ACCEL:
+                retval = slave_config(accel_adapter, mldl_cfg,
+                                mldl_cfg->accel,
+                                &mldl_cfg->pdata->accel,
+                                (struct ext_slave_config __user *) arg);
+                break;
+        case MPU_CONFIG_COMPASS:
+                retval = slave_config(compass_adapter, mldl_cfg,
+                                mldl_cfg->compass,
+                                &mldl_cfg->pdata->compass,
+                                (struct ext_slave_config __user *) arg);
+                break;
+        case MPU_CONFIG_PRESSURE:
+                retval = slave_config(pressure_adapter, mldl_cfg,
+                                mldl_cfg->pressure,
+                                &mldl_cfg->pdata->pressure,
+                                (struct ext_slave_config __user *) arg);
+                break;
+        case MPU_GET_CONFIG_ACCEL:
+                retval = slave_get_config(accel_adapter, mldl_cfg,
+                                        mldl_cfg->accel,
+                                        &mldl_cfg->pdata->accel,
+                                        (struct ext_slave_config __user *) arg);
+                break;
+        case MPU_GET_CONFIG_COMPASS:
+                retval = slave_get_config(compass_adapter, mldl_cfg,
+                                        mldl_cfg->compass,
+                                        &mldl_cfg->pdata->compass,
+                                        (struct ext_slave_config __user *) arg);
+                break;
+        case MPU_GET_CONFIG_PRESSURE:
+                retval = slave_get_config(pressure_adapter, mldl_cfg,
+                                        mldl_cfg->pressure,
+                                        &mldl_cfg->pdata->pressure,
+                                        (struct ext_slave_config __user *) arg);
+                break;
+        case MPU_SUSPEND:
+        {
+                unsigned long sensors;
+                sensors = ~(mldl_cfg->requested_sensors);
+                retval = mpu3050_suspend(mldl_cfg,
+                                        client->adapter,
+                                        accel_adapter,
+                                        compass_adapter,
+                                        pressure_adapter,
+                                        ((sensors & ML_THREE_AXIS_GYRO)
+                                                == ML_THREE_AXIS_GYRO),
+                                        ((sensors & ML_THREE_AXIS_ACCEL)
+                                                == ML_THREE_AXIS_ACCEL),
+                                        ((sensors & ML_THREE_AXIS_COMPASS)
+                                                == ML_THREE_AXIS_COMPASS),
+                                        ((sensors & ML_THREE_AXIS_PRESSURE)
+                                                == ML_THREE_AXIS_PRESSURE));
+        }
+        break;
+        case MPU_RESUME:
+        {
+                unsigned long sensors;
+                sensors = mldl_cfg->requested_sensors;
+                retval = mpu3050_resume(mldl_cfg,
+                                        client->adapter,
+                                        accel_adapter,
+                                        compass_adapter,
+                                        pressure_adapter,
+                                        sensors & ML_THREE_AXIS_GYRO,
+                                        sensors & ML_THREE_AXIS_ACCEL,
+                                        sensors & ML_THREE_AXIS_COMPASS,
+                                        sensors & ML_THREE_AXIS_PRESSURE);
+        }
+        break;
+        case MPU_PM_EVENT_HANDLED:
+                dev_dbg(&this_client->adapter->dev,
+                        "%s: %d\n", __func__, cmd);
+                complete(&mpu->completion);
+                break;
+        case MPU_READ_ACCEL:
+        {
+                unsigned char data[6];
+                retval = mpu3050_read_accel(mldl_cfg, client->adapter,
+                                            data);
+                if ((ML_SUCCESS == retval) &&
+                    (copy_to_user((unsigned char __user *) arg,
+                            data, sizeof(data))))
+                        retval = -EFAULT;
+        }
+        break;
+        case MPU_READ_COMPASS:
+        {
+                unsigned char data[6];
+                struct i2c_adapter *compass_adapt =
+                        i2c_get_adapter(mldl_cfg->pdata->compass.
+                                        adapt_num);
+                retval = mpu3050_read_compass(mldl_cfg, compass_adapt,
+                                                 data);
+                if ((ML_SUCCESS == retval) &&
+                        (copy_to_user((unsigned char *) arg,
+                                data, sizeof(data))))
+                        retval = -EFAULT;
+        }
+        break;
+        case MPU_READ_PRESSURE:
+        {
+                unsigned char data[3];
+                struct i2c_adapter *pressure_adapt =
+                        i2c_get_adapter(mldl_cfg->pdata->pressure.
+                                        adapt_num);
+                retval =
+                        mpu3050_read_pressure(mldl_cfg, pressure_adapt,
+                                        data);
+                if ((ML_SUCCESS == retval) &&
+                    (copy_to_user((unsigned char __user *) arg,
+                            data, sizeof(data))))
+                        retval = -EFAULT;
+        }
+        break;
+        default:
+                dev_err(&this_client->adapter->dev,
+                        "%s: Unknown cmd %x, arg %lu\n", __func__, cmd,
+                        arg);
+                retval = -EINVAL;
+        }
+
+        mutex_unlock(&mpu->mutex);
+        return retval;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+void mpu3050_early_suspend(struct early_suspend *h)
+{
+        struct mpu_private_data *mpu = container_of(h,
+                                                    struct
+                                                    mpu_private_data,
+                                                    early_suspend);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *accel_adapter;
+        struct i2c_adapter *compass_adapter;
+        struct i2c_adapter *pressure_adapter;
+
+        accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+        compass_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+        pressure_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+        dev_dbg(&this_client->adapter->dev, "%s: %d, %d\n", __func__,
+                h->level, mpu->mldl_cfg.gyro_is_suspended);
+        if (MPU3050_EARLY_SUSPEND_IN_DRIVER) {
+                mutex_lock(&mpu->mutex);
+                (void) mpu3050_suspend(mldl_cfg, this_client->adapter,
+                                accel_adapter, compass_adapter,
+                                pressure_adapter, TRUE, TRUE, TRUE, TRUE);
+                mutex_unlock(&mpu->mutex);
+        }
+}
+
+void mpu3050_early_resume(struct early_suspend *h)
+{
+        struct mpu_private_data *mpu = container_of(h,
+                                                    struct
+                                                    mpu_private_data,
+                                                    early_suspend);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *accel_adapter;
+        struct i2c_adapter *compass_adapter;
+        struct i2c_adapter *pressure_adapter;
+
+        accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+        compass_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+        pressure_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+        if (MPU3050_EARLY_SUSPEND_IN_DRIVER) {
+                if (mpu->pid) {
+                        unsigned long sensors = mldl_cfg->requested_sensors;
+                        mutex_lock(&mpu->mutex);
+                        (void) mpu3050_resume(mldl_cfg,
+                                        this_client->adapter,
+                                        accel_adapter,
+                                        compass_adapter,
+                                        pressure_adapter,
+                                        sensors & ML_THREE_AXIS_GYRO,
+                                        sensors & ML_THREE_AXIS_ACCEL,
+                                        sensors & ML_THREE_AXIS_COMPASS,
+                                        sensors & ML_THREE_AXIS_PRESSURE);
+                        mutex_unlock(&mpu->mutex);
+                        dev_dbg(&this_client->adapter->dev,
+                                "%s for pid %d\n", __func__, mpu->pid);
+                }
+        }
+        dev_dbg(&this_client->adapter->dev, "%s: %d\n", __func__, h->level);
+}
+#endif
+
+void mpu_shutdown(struct i2c_client *client)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *accel_adapter;
+        struct i2c_adapter *compass_adapter;
+        struct i2c_adapter *pressure_adapter;
+
+        accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+        compass_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+        pressure_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+        mutex_lock(&mpu->mutex);
+        (void) mpu3050_suspend(mldl_cfg, this_client->adapter,
+                               accel_adapter, compass_adapter, pressure_adapter,
+                               TRUE, TRUE, TRUE, TRUE);
+        mutex_unlock(&mpu->mutex);
+        dev_dbg(&this_client->adapter->dev, "%s\n", __func__);
+}
+
+int mpu_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *accel_adapter;
+        struct i2c_adapter *compass_adapter;
+        struct i2c_adapter *pressure_adapter;
+
+        accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+        compass_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+        pressure_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+        mutex_lock(&mpu->mutex);
+        if (!mldl_cfg->ignore_system_suspend) {
+                dev_dbg(&this_client->adapter->dev,
+                        "%s: suspending on event %d\n", __func__,
+                        mesg.event);
+                (void) mpu3050_suspend(mldl_cfg, this_client->adapter,
+                                       accel_adapter, compass_adapter,
+                                       pressure_adapter,
+                                       TRUE, TRUE, TRUE, TRUE);
+        } else {
+                dev_dbg(&this_client->adapter->dev,
+                        "%s: Already suspended %d\n", __func__,
+                        mesg.event);
+        }
+        mutex_unlock(&mpu->mutex);
+        return 0;
+}
+
+int mpu_resume(struct i2c_client *client)
+{
+        struct mpu_private_data *mpu =
+            (struct mpu_private_data *) i2c_get_clientdata(client);
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct i2c_adapter *accel_adapter;
+        struct i2c_adapter *compass_adapter;
+        struct i2c_adapter *pressure_adapter;
+
+        accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+        compass_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+        pressure_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+        mutex_lock(&mpu->mutex);
+        if (mpu->pid && !mldl_cfg->ignore_system_suspend) {
+                unsigned long sensors = mldl_cfg->requested_sensors;
+                (void) mpu3050_resume(mldl_cfg, this_client->adapter,
+                                      accel_adapter,
+                                      compass_adapter,
+                                      pressure_adapter,
+                                      sensors & ML_THREE_AXIS_GYRO,
+                                      sensors & ML_THREE_AXIS_ACCEL,
+                                      sensors & ML_THREE_AXIS_COMPASS,
+                                      sensors & ML_THREE_AXIS_PRESSURE);
+                dev_dbg(&this_client->adapter->dev,
+                        "%s for pid %d\n", __func__, mpu->pid);
+        }
+        mutex_unlock(&mpu->mutex);
+        return 0;
+}
+
+/* define which file operations are supported */
+static const struct file_operations mpu_fops = {
+        .owner = THIS_MODULE,
+        .read = mpu_read,
+        .poll = mpu_poll,
+
+#if HAVE_COMPAT_IOCTL
+        .compat_ioctl = mpu_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+        .unlocked_ioctl = mpu_ioctl,
+#endif
+        .open = mpu_open,
+        .release = mpu_release,
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 32)
+I2C_CLIENT_INSMOD;
+#endif
+
+static struct miscdevice i2c_mpu_device = {
+        .minor = MISC_DYNAMIC_MINOR,
+        .name = "mpu", /* Same for both 3050 and 6000 */
+        .fops = &mpu_fops,
+};
+
+
+int mpu3050_probe(struct i2c_client *client,
+                  const struct i2c_device_id *devid)
+{
+        struct mpu3050_platform_data *pdata;
+        struct mpu_private_data *mpu;
+        struct mldl_cfg *mldl_cfg;
+        int res = 0;
+        struct i2c_adapter *accel_adapter = NULL;
+        struct i2c_adapter *compass_adapter = NULL;
+        struct i2c_adapter *pressure_adapter = NULL;
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                res = -ENODEV;
+                goto out_check_functionality_failed;
+        }
+
+        mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL);
+        if (!mpu) {
+                res = -ENOMEM;
+                goto out_alloc_data_failed;
+        }
+
+        i2c_set_clientdata(client, mpu);
+        this_client = client;
+        mldl_cfg = &mpu->mldl_cfg;
+
+        init_waitqueue_head(&mpu->mpu_event_wait);
+
+        mutex_init(&mpu->mutex);
+        init_completion(&mpu->completion);
+
+        mpu->response_timeout = 60; /* Seconds */
+        mpu->timeout.function = mpu_pm_timeout;
+        mpu->timeout.data = (u_long) mpu;
+        init_timer(&mpu->timeout);
+
+        /* FIXME:
+         * Do not register the pm_notifier as it causes
+         * issues with resume sequence as there is no response
+         * from user-space for power notifications for approx
+         * 60 sec. Refer NV bug 858630 for more details.
+         */
+#if 0
+        mpu->nb.notifier_call = mpu_pm_notifier_callback;
+        mpu->nb.priority = 0;
+        register_pm_notifier(&mpu->nb);
+#endif
+
+        pdata = (struct mpu3050_platform_data *) client->dev.platform_data;
+        if (!pdata) {
+                dev_warn(&this_client->adapter->dev,
+                         "Missing platform data for mpu3050\n");
+        } else {
+                mldl_cfg->pdata = pdata;
+
+#if defined(CONFIG_MPU_SENSORS_MPU3050_MODULE) || \
+    defined(CONFIG_MPU_SENSORS_MPU6000_MODULE)
+                pdata->accel.get_slave_descr = get_accel_slave_descr;
+                pdata->compass.get_slave_descr = get_compass_slave_descr;
+                pdata->pressure.get_slave_descr = get_pressure_slave_descr;
+#endif
+
+                if (pdata->accel.get_slave_descr) {
+                        mldl_cfg->accel =
+                            pdata->accel.get_slave_descr();
+                        dev_info(&this_client->adapter->dev,
+                                 "%s: +%s\n", MPU_NAME,
+                                 mldl_cfg->accel->name);
+                        accel_adapter =
+                                i2c_get_adapter(pdata->accel.adapt_num);
+                        if (pdata->accel.irq > 0) {
+                                dev_info(&this_client->adapter->dev,
+                                        "Installing Accel irq using %d\n",
+                                        pdata->accel.irq);
+                                res = slaveirq_init(accel_adapter,
+                                                &pdata->accel,
+                                                "accelirq");
+                                if (res)
+                                        goto out_accelirq_failed;
+                        } else {
+                                dev_warn(&this_client->adapter->dev,
+                                        "WARNING: Accel irq not assigned\n");
+                        }
+                } else {
+                        dev_warn(&this_client->adapter->dev,
+                                 "%s: No Accel Present\n", MPU_NAME);
+                }
+
+                if (pdata->compass.get_slave_descr) {
+                        mldl_cfg->compass =
+                            pdata->compass.get_slave_descr();
+                        dev_info(&this_client->adapter->dev,
+                                 "%s: +%s\n", MPU_NAME,
+                                 mldl_cfg->compass->name);
+                        compass_adapter =
+                                i2c_get_adapter(pdata->compass.adapt_num);
+                        if (pdata->compass.irq > 0) {
+                                dev_info(&this_client->adapter->dev,
+                                        "Installing Compass irq using %d\n",
+                                        pdata->compass.irq);
+                                res = slaveirq_init(compass_adapter,
+                                                &pdata->compass,
+                                                "compassirq");
+                                if (res)
+                                        goto out_compassirq_failed;
+                        } else {
+                                dev_warn(&this_client->adapter->dev,
+                                        "WARNING: Compass irq not assigned\n");
+                        }
+                } else {
+                        dev_warn(&this_client->adapter->dev,
+                                 "%s: No Compass Present\n", MPU_NAME);
+                }
+
+                if (pdata->pressure.get_slave_descr) {
+                        mldl_cfg->pressure =
+                            pdata->pressure.get_slave_descr();
+                        dev_info(&this_client->adapter->dev,
+                                 "%s: +%s\n", MPU_NAME,
+                                 mldl_cfg->pressure->name);
+                        pressure_adapter =
+                                i2c_get_adapter(pdata->pressure.adapt_num);
+
+                        if (pdata->pressure.irq > 0) {
+                                dev_info(&this_client->adapter->dev,
+                                        "Installing Pressure irq using %d\n",
+                                        pdata->pressure.irq);
+                                res = slaveirq_init(pressure_adapter,
+                                                &pdata->pressure,
+                                                "pressureirq");
+                                if (res)
+                                        goto out_pressureirq_failed;
+                        } else {
+                                dev_warn(&this_client->adapter->dev,
+                                        "WARNING: Pressure irq not assigned\n");
+                        }
+                } else {
+                        dev_warn(&this_client->adapter->dev,
+                                 "%s: No Pressure Present\n", MPU_NAME);
+                }
+        }
+
+        mldl_cfg->addr = client->addr;
+        res = mpu3050_open(&mpu->mldl_cfg, client->adapter,
+                        accel_adapter, compass_adapter, pressure_adapter);
+
+        if (res) {
+                dev_err(&this_client->adapter->dev,
+                        "Unable to open %s %d\n", MPU_NAME, res);
+                res = -ENODEV;
+                goto out_whoami_failed;
+        }
+
+        res = misc_register(&i2c_mpu_device);
+        if (res < 0) {
+                dev_err(&this_client->adapter->dev,
+                        "ERROR: misc_register returned %d\n", res);
+                goto out_misc_register_failed;
+        }
+
+        if (this_client->irq > 0) {
+                dev_info(&this_client->adapter->dev,
+                         "Installing irq using %d\n", this_client->irq);
+                res = mpuirq_init(this_client);
+                if (res)
+                        goto out_mpuirq_failed;
+        } else {
+                dev_warn(&this_client->adapter->dev,
+                        "Missing %s IRQ\n", MPU_NAME);
+        }
+
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        mpu->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+        mpu->early_suspend.suspend = mpu3050_early_suspend;
+        mpu->early_suspend.resume = mpu3050_early_resume;
+        register_early_suspend(&mpu->early_suspend);
+#endif
+        return res;
+
+out_mpuirq_failed:
+        misc_deregister(&i2c_mpu_device);
+out_misc_register_failed:
+        mpu3050_close(&mpu->mldl_cfg, client->adapter,
+                accel_adapter, compass_adapter, pressure_adapter);
+out_whoami_failed:
+        if (pdata &&
+            pdata->pressure.get_slave_descr &&
+            pdata->pressure.irq)
+                slaveirq_exit(&pdata->pressure);
+out_pressureirq_failed:
+        if (pdata &&
+            pdata->compass.get_slave_descr &&
+            pdata->compass.irq)
+                slaveirq_exit(&pdata->compass);
+out_compassirq_failed:
+        if (pdata &&
+            pdata->accel.get_slave_descr &&
+            pdata->accel.irq)
+                slaveirq_exit(&pdata->accel);
+out_accelirq_failed:
+        kfree(mpu);
+out_alloc_data_failed:
+out_check_functionality_failed:
+        dev_err(&this_client->adapter->dev, "%s failed %d\n", __func__,
+                res);
+        return res;
+
+}
+
+static int mpu3050_remove(struct i2c_client *client)
+{
+        struct mpu_private_data *mpu = i2c_get_clientdata(client);
+        struct i2c_adapter *accel_adapter;
+        struct i2c_adapter *compass_adapter;
+        struct i2c_adapter *pressure_adapter;
+        struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+        struct mpu3050_platform_data *pdata = mldl_cfg->pdata;
+
+        accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+        compass_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+        pressure_adapter =
+            i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        unregister_early_suspend(&mpu->early_suspend);
+#endif
+        mpu3050_close(mldl_cfg, client->adapter,
+                accel_adapter, compass_adapter, pressure_adapter);
+
+        if (client->irq)
+                mpuirq_exit();
+
+        if (pdata &&
+            pdata->pressure.get_slave_descr &&
+            pdata->pressure.irq)
+                slaveirq_exit(&pdata->pressure);
+
+        if (pdata &&
+            pdata->compass.get_slave_descr &&
+            pdata->compass.irq)
+                slaveirq_exit(&pdata->compass);
+
+        if (pdata &&
+            pdata->accel.get_slave_descr &&
+            pdata->accel.irq)
+                slaveirq_exit(&pdata->accel);
+
+        misc_deregister(&i2c_mpu_device);
+        kfree(mpu);
+
+        return 0;
+}
+
+static const struct i2c_device_id mpu3050_id[] = {
+        {MPU_NAME, 0},
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, mpu3050_id);
+
+static struct i2c_driver mpu3050_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = mpu3050_probe,
+        .remove = mpu3050_remove,
+        .id_table = mpu3050_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = MPU_NAME,
+                   },
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 32)
+        .address_data = &addr_data,
+#else
+        .address_list = normal_i2c,
+#endif
+
+        .shutdown = mpu_shutdown,       /* optional */
+        .suspend = mpu_suspend, /* optional */
+        .resume = mpu_resume,   /* optional */
+
+};
+
+static int __init mpu_init(void)
+{
+        int res = i2c_add_driver(&mpu3050_driver);
+        pr_debug("%s\n", __func__);
+        if (res)
+                pr_err("%s failed\n",
+                        __func__);
+        return res;
+}
+
+static void __exit mpu_exit(void)
+{
+        pr_debug("%s\n", __func__);
+        i2c_del_driver(&mpu3050_driver);
+}
+
+module_init(mpu_init);
+module_exit(mpu_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("User space character device interface for MPU3050");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS(MPU_NAME);
diff --git a/drivers/misc/mpu3050/mpu-i2c.c b/drivers/misc/mpu3050/mpu-i2c.c
new file mode 100644
index 00000000000..b1298d313ab
--- /dev/null
+++ b/drivers/misc/mpu3050/mpu-i2c.c
@@ -0,0 +1,196 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+/**
+ *  @defgroup
+ *  @brief
+ *
+ *  @{
+ *      @file   mpu-i2c.c
+ *      @brief
+ *
+ */
+
+#include <linux/i2c.h>
+#include "mpu.h"
+
+int sensor_i2c_write(struct i2c_adapter *i2c_adap,
+                     unsigned char address,
+                     unsigned int len, unsigned char const *data)
+{
+        struct i2c_msg msgs[1];
+        int res;
+
+        if (NULL == data || NULL == i2c_adap)
+                return -EINVAL;
+
+        msgs[0].addr = address;
+        msgs[0].flags = 0;      /* write */
+        msgs[0].buf = (unsigned char *) data;
+        msgs[0].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 1);
+        if (res < 1)
+                return res;
+        else
+                return 0;
+}
+
+int sensor_i2c_write_register(struct i2c_adapter *i2c_adap,
+                              unsigned char address,
+                              unsigned char reg, unsigned char value)
+{
+        unsigned char data[2];
+
+        data[0] = reg;
+        data[1] = value;
+        return sensor_i2c_write(i2c_adap, address, 2, data);
+}
+
+int sensor_i2c_read(struct i2c_adapter *i2c_adap,
+                    unsigned char address,
+                    unsigned char reg,
+                    unsigned int len, unsigned char *data)
+{
+        struct i2c_msg msgs[2];
+        int res;
+
+        if (NULL == data || NULL == i2c_adap)
+                return -EINVAL;
+
+        msgs[0].addr = address;
+        msgs[0].flags = 0;      /* write */
+        msgs[0].buf = &reg;
+        msgs[0].len = 1;
+
+        msgs[1].addr = address;
+        msgs[1].flags = I2C_M_RD;
+        msgs[1].buf = data;
+        msgs[1].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 2);
+        if (res < 2)
+                return res;
+        else
+                return 0;
+}
+
+int mpu_memory_read(struct i2c_adapter *i2c_adap,
+                    unsigned char mpu_addr,
+                    unsigned short mem_addr,
+                    unsigned int len, unsigned char *data)
+{
+        unsigned char bank[2];
+        unsigned char addr[2];
+        unsigned char buf;
+
+        struct i2c_msg msgs[4];
+        int ret;
+
+        if (NULL == data || NULL == i2c_adap)
+                return -EINVAL;
+
+        bank[0] = MPUREG_BANK_SEL;
+        bank[1] = mem_addr >> 8;
+
+        addr[0] = MPUREG_MEM_START_ADDR;
+        addr[1] = mem_addr & 0xFF;
+
+        buf = MPUREG_MEM_R_W;
+
+        /* Write Message */
+        msgs[0].addr = mpu_addr;
+        msgs[0].flags = 0;
+        msgs[0].buf = bank;
+        msgs[0].len = sizeof(bank);
+
+        msgs[1].addr = mpu_addr;
+        msgs[1].flags = 0;
+        msgs[1].buf = addr;
+        msgs[1].len = sizeof(addr);
+
+        msgs[2].addr = mpu_addr;
+        msgs[2].flags = 0;
+        msgs[2].buf = &buf;
+        msgs[2].len = 1;
+
+        msgs[3].addr = mpu_addr;
+        msgs[3].flags = I2C_M_RD;
+        msgs[3].buf = data;
+        msgs[3].len = len;
+
+        ret = i2c_transfer(i2c_adap, msgs, 4);
+        if (ret != 4)
+                return ret;
+        else
+                return 0;
+}
+
+int mpu_memory_write(struct i2c_adapter *i2c_adap,
+                     unsigned char mpu_addr,
+                     unsigned short mem_addr,
+                     unsigned int len, unsigned char const *data)
+{
+        unsigned char bank[2];
+        unsigned char addr[2];
+        unsigned char buf[513];
+
+        struct i2c_msg msgs[3];
+        int ret;
+
+        if (NULL == data || NULL == i2c_adap)
+                return -EINVAL;
+        if (len >= (sizeof(buf) - 1))
+                return -ENOMEM;
+
+        bank[0] = MPUREG_BANK_SEL;
+        bank[1] = mem_addr >> 8;
+
+        addr[0] = MPUREG_MEM_START_ADDR;
+        addr[1] = mem_addr & 0xFF;
+
+        buf[0] = MPUREG_MEM_R_W;
+        memcpy(buf + 1, data, len);
+
+        /* Write Message */
+        msgs[0].addr = mpu_addr;
+        msgs[0].flags = 0;
+        msgs[0].buf = bank;
+        msgs[0].len = sizeof(bank);
+
+        msgs[1].addr = mpu_addr;
+        msgs[1].flags = 0;
+        msgs[1].buf = addr;
+        msgs[1].len = sizeof(addr);
+
+        msgs[2].addr = mpu_addr;
+        msgs[2].flags = 0;
+        msgs[2].buf = (unsigned char *) buf;
+        msgs[2].len = len + 1;
+
+        ret = i2c_transfer(i2c_adap, msgs, 3);
+        if (ret != 3)
+                return ret;
+        else
+                return 0;
+}
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/mpu-i2c.h b/drivers/misc/mpu3050/mpu-i2c.h
new file mode 100644
index 00000000000..0bbc8c64594
--- /dev/null
+++ b/drivers/misc/mpu3050/mpu-i2c.h
@@ -0,0 +1,58 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+/**
+ * @defgroup
+ * @brief
+ *
+ * @{
+ * @file     mpu-i2c.c
+ * @brief
+ *
+ *
+ */
+
+#ifndef __MPU_I2C_H__
+#define __MPU_I2C_H__
+
+#include <linux/i2c.h>
+
+int sensor_i2c_write(struct i2c_adapter *i2c_adap,
+                     unsigned char address,
+                     unsigned int len, unsigned char const *data);
+
+int sensor_i2c_write_register(struct i2c_adapter *i2c_adap,
+                              unsigned char address,
+                              unsigned char reg, unsigned char value);
+
+int sensor_i2c_read(struct i2c_adapter *i2c_adap,
+                    unsigned char address,
+                    unsigned char reg,
+                    unsigned int len, unsigned char *data);
+
+int mpu_memory_read(struct i2c_adapter *i2c_adap,
+                    unsigned char mpu_addr,
+                    unsigned short mem_addr,
+                    unsigned int len, unsigned char *data);
+
+int mpu_memory_write(struct i2c_adapter *i2c_adap,
+                     unsigned char mpu_addr,
+                     unsigned short mem_addr,
+                     unsigned int len, unsigned char const *data);
+
+#endif  /* __MPU_I2C_H__ */
diff --git a/drivers/misc/mpu3050/mpuirq.c b/drivers/misc/mpu3050/mpuirq.c
new file mode 100644
index 00000000000..ce1ad409cbf
--- /dev/null
+++ b/drivers/misc/mpu3050/mpuirq.c
@@ -0,0 +1,319 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/workqueue.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "mpu.h"
+#include "mpuirq.h"
+#include "mldl_cfg.h"
+#include "mpu-i2c.h"
+
+#define MPUIRQ_NAME "mpuirq"
+
+/* function which gets accel data and sends it to MPU */
+
+DECLARE_WAIT_QUEUE_HEAD(mpuirq_wait);
+
+struct mpuirq_dev_data {
+        struct work_struct work;
+        struct i2c_client *mpu_client;
+        struct miscdevice *dev;
+        int irq;
+        int pid;
+        int accel_divider;
+        int data_ready;
+        int timeout;
+};
+
+static struct mpuirq_dev_data mpuirq_dev_data;
+static struct mpuirq_data mpuirq_data;
+static char *interface = MPUIRQ_NAME;
+
+static void mpu_accel_data_work_fcn(struct work_struct *work);
+
+static int mpuirq_open(struct inode *inode, struct file *file)
+{
+        dev_dbg(mpuirq_dev_data.dev->this_device,
+                "%s current->pid %d\n", __func__, current->pid);
+        mpuirq_dev_data.pid = current->pid;
+        file->private_data = &mpuirq_dev_data;
+        return 0;
+}
+
+/* close function - called when the "file" /dev/mpuirq is closed in userspace */
+static int mpuirq_release(struct inode *inode, struct file *file)
+{
+        dev_dbg(mpuirq_dev_data.dev->this_device, "mpuirq_release\n");
+        return 0;
+}
+
+/* read function called when from /dev/mpuirq is read */
+static ssize_t mpuirq_read(struct file *file,
+                           char *buf, size_t count, loff_t *ppos)
+{
+        int len, err;
+        struct mpuirq_dev_data *p_mpuirq_dev_data = file->private_data;
+
+        if (!mpuirq_dev_data.data_ready &&
+                mpuirq_dev_data.timeout &&
+                (!(file->f_flags & O_NONBLOCK))) {
+                wait_event_interruptible_timeout(mpuirq_wait,
+                                                 mpuirq_dev_data.
+                                                 data_ready,
+                                                 mpuirq_dev_data.timeout);
+        }
+
+        if (mpuirq_dev_data.data_ready && NULL != buf
+            && count >= sizeof(mpuirq_data)) {
+                err = copy_to_user(buf, &mpuirq_data, sizeof(mpuirq_data));
+                mpuirq_data.data_type = 0;
+        } else {
+                return 0;
+        }
+        if (err != 0) {
+                dev_err(p_mpuirq_dev_data->dev->this_device,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        mpuirq_dev_data.data_ready = 0;
+        len = sizeof(mpuirq_data);
+        return len;
+}
+
+unsigned int mpuirq_poll(struct file *file, struct poll_table_struct *poll)
+{
+        int mask = 0;
+
+        poll_wait(file, &mpuirq_wait, poll);
+        if (mpuirq_dev_data.data_ready)
+                mask |= POLLIN | POLLRDNORM;
+        return mask;
+}
+
+/* ioctl - I/O control */
+static long mpuirq_ioctl(struct file *file,
+                         unsigned int cmd, unsigned long arg)
+{
+        int retval = 0;
+        int data;
+
+        switch (cmd) {
+        case MPUIRQ_SET_TIMEOUT:
+                mpuirq_dev_data.timeout = arg;
+                break;
+
+        case MPUIRQ_GET_INTERRUPT_CNT:
+                data = mpuirq_data.interruptcount - 1;
+                if (mpuirq_data.interruptcount > 1)
+                        mpuirq_data.interruptcount = 1;
+
+                if (copy_to_user((int *) arg, &data, sizeof(int)))
+                        return -EFAULT;
+                break;
+        case MPUIRQ_GET_IRQ_TIME:
+                if (copy_to_user((int *) arg, &mpuirq_data.irqtime,
+                                 sizeof(mpuirq_data.irqtime)))
+                        return -EFAULT;
+                mpuirq_data.irqtime = 0;
+                break;
+        case MPUIRQ_SET_FREQUENCY_DIVIDER:
+                mpuirq_dev_data.accel_divider = arg;
+                break;
+        default:
+                retval = -EINVAL;
+        }
+        return retval;
+}
+
+static void mpu_accel_data_work_fcn(struct work_struct *work)
+{
+        struct mpuirq_dev_data *mpuirq_dev_data =
+            (struct mpuirq_dev_data *) work;
+        struct mldl_cfg *mldl_cfg =
+            (struct mldl_cfg *)
+            i2c_get_clientdata(mpuirq_dev_data->mpu_client);
+        struct i2c_adapter *accel_adapter;
+        unsigned char wbuff[16];
+        unsigned char rbuff[16];
+        int ii;
+
+        accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+        mldl_cfg->accel->read(accel_adapter,
+                              mldl_cfg->accel,
+                              &mldl_cfg->pdata->accel, rbuff);
+
+
+        /* @todo add other data formats here as well */
+        if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) {
+                for (ii = 0; ii < 3; ii++) {
+                        wbuff[2 * ii + 1] = rbuff[2 * ii + 1];
+                        wbuff[2 * ii + 2] = rbuff[2 * ii + 0];
+                }
+        } else {
+                memcpy(wbuff + 1, rbuff, mldl_cfg->accel->len);
+        }
+
+        wbuff[7] = 0;
+        wbuff[8] = 1;           /*set semaphore */
+
+        mpu_memory_write(mpuirq_dev_data->mpu_client->adapter,
+                         mldl_cfg->addr, 0x0108, 8, wbuff);
+}
+
+static irqreturn_t mpuirq_handler(int irq, void *dev_id)
+{
+        static int mycount;
+        struct timeval irqtime;
+        mycount++;
+
+        mpuirq_data.interruptcount++;
+
+        /* wake up (unblock) for reading data from userspace */
+        /* and ignore first interrupt generated in module init */
+        mpuirq_dev_data.data_ready = 1;
+
+        do_gettimeofday(&irqtime);
+        mpuirq_data.irqtime = (((long long) irqtime.tv_sec) << 32);
+        mpuirq_data.irqtime += irqtime.tv_usec;
+
+        if ((mpuirq_dev_data.accel_divider >= 0) &&
+                (0 == (mycount % (mpuirq_dev_data.accel_divider + 1)))) {
+                schedule_work((struct work_struct
+                                *) (&mpuirq_dev_data));
+        }
+
+        wake_up_interruptible(&mpuirq_wait);
+
+        return IRQ_HANDLED;
+
+}
+
+/* define which file operations are supported */
+const struct file_operations mpuirq_fops = {
+        .owner = THIS_MODULE,
+        .read = mpuirq_read,
+        .poll = mpuirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+        .compat_ioctl = mpuirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+        .unlocked_ioctl = mpuirq_ioctl,
+#endif
+        .open = mpuirq_open,
+        .release = mpuirq_release,
+};
+
+static struct miscdevice mpuirq_device = {
+        .minor = MISC_DYNAMIC_MINOR,
+        .name = MPUIRQ_NAME,
+        .fops = &mpuirq_fops,
+};
+
+int mpuirq_init(struct i2c_client *mpu_client)
+{
+
+        int res;
+        struct mldl_cfg *mldl_cfg =
+            (struct mldl_cfg *) i2c_get_clientdata(mpu_client);
+
+        /* work_struct initialization */
+        INIT_WORK((struct work_struct *) &mpuirq_dev_data,
+                  mpu_accel_data_work_fcn);
+        mpuirq_dev_data.mpu_client = mpu_client;
+
+        dev_info(&mpu_client->adapter->dev,
+                 "Module Param interface = %s\n", interface);
+
+        mpuirq_dev_data.irq = mpu_client->irq;
+        mpuirq_dev_data.pid = 0;
+        mpuirq_dev_data.accel_divider = -1;
+        mpuirq_dev_data.data_ready = 0;
+        mpuirq_dev_data.timeout = 0;
+        mpuirq_dev_data.dev = &mpuirq_device;
+
+        if (mpuirq_dev_data.irq) {
+                unsigned long flags;
+                if (BIT_ACTL_LOW ==
+                    ((mldl_cfg->pdata->int_config) & BIT_ACTL))
+                        flags = IRQF_TRIGGER_FALLING;
+                else
+                        flags = IRQF_TRIGGER_RISING;
+
+                res =
+                    request_irq(mpuirq_dev_data.irq, mpuirq_handler, flags,
+                                interface, &mpuirq_dev_data.irq);
+                if (res) {
+                        dev_err(&mpu_client->adapter->dev,
+                                "myirqtest: cannot register IRQ %d\n",
+                                mpuirq_dev_data.irq);
+                } else {
+                        res = misc_register(&mpuirq_device);
+                        if (res < 0) {
+                                dev_err(&mpu_client->adapter->dev,
+                                        "misc_register returned %d\n",
+                                        res);
+                                free_irq(mpuirq_dev_data.irq,
+                                         &mpuirq_dev_data.irq);
+                        }
+                }
+
+        } else {
+                res = 0;
+        }
+
+        return res;
+}
+
+void mpuirq_exit(void)
+{
+        /* Free the IRQ first before flushing the work */
+        if (mpuirq_dev_data.irq > 0)
+                free_irq(mpuirq_dev_data.irq, &mpuirq_dev_data.irq);
+
+        flush_scheduled_work();
+
+        dev_info(mpuirq_device.this_device, "Unregistering %s\n",
+                 MPUIRQ_NAME);
+        misc_deregister(&mpuirq_device);
+
+        return;
+}
+
+module_param(interface, charp, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(interface, "The Interface name");
diff --git a/drivers/misc/mpu3050/mpuirq.h b/drivers/misc/mpu3050/mpuirq.h
new file mode 100644
index 00000000000..a71c79c75e8
--- /dev/null
+++ b/drivers/misc/mpu3050/mpuirq.h
@@ -0,0 +1,42 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+#ifndef __MPUIRQ__
+#define __MPUIRQ__
+
+#ifdef __KERNEL__
+#include <linux/i2c-dev.h>
+#include <linux/time.h>
+#else
+#include <sys/time.h>
+#endif
+
+#define MPUIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x40, unsigned long)
+#define MPUIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x41, unsigned long)
+#define MPUIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x42, struct timeval)
+#define MPUIRQ_SET_FREQUENCY_DIVIDER _IOW(MPU_IOCTL, 0x43, unsigned long)
+
+#ifdef __KERNEL__
+
+void mpuirq_exit(void);
+int mpuirq_init(struct i2c_client *mpu_client);
+
+#endif
+
+#endif
diff --git a/drivers/misc/mpu3050/slaveirq.c b/drivers/misc/mpu3050/slaveirq.c
new file mode 100644
index 00000000000..a3c7bfec4b4
--- /dev/null
+++ b/drivers/misc/mpu3050/slaveirq.c
@@ -0,0 +1,273 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/wait.h>
+#include <linux/slab.h>
+
+#include "mpu.h"
+#include "slaveirq.h"
+#include "mldl_cfg.h"
+#include "mpu-i2c.h"
+
+/* function which gets slave data and sends it to SLAVE */
+
+struct slaveirq_dev_data {
+        struct miscdevice dev;
+        struct i2c_client *slave_client;
+        struct mpuirq_data data;
+        wait_queue_head_t slaveirq_wait;
+        int irq;
+        int pid;
+        int data_ready;
+        int timeout;
+};
+
+/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28
+ * drivers: misc: pass miscdevice pointer via file private data
+ */
+static int slaveirq_open(struct inode *inode, struct file *file)
+{
+        /* Device node is availabe in the file->private_data, this is
+         * exactly what we want so we leave it there */
+        struct slaveirq_dev_data *data =
+                container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+        dev_dbg(data->dev.this_device,
+                "%s current->pid %d\n", __func__, current->pid);
+        data->pid = current->pid;
+        return 0;
+}
+
+static int slaveirq_release(struct inode *inode, struct file *file)
+{
+        struct slaveirq_dev_data *data =
+                container_of(file->private_data, struct slaveirq_dev_data, dev);
+        dev_dbg(data->dev.this_device, "slaveirq_release\n");
+        return 0;
+}
+
+/* read function called when from /dev/slaveirq is read */
+static ssize_t slaveirq_read(struct file *file,
+                           char *buf, size_t count, loff_t *ppos)
+{
+        int len, err;
+        struct slaveirq_dev_data *data =
+                container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+        if (!data->data_ready &&
+                data->timeout &&
+                !(file->f_flags & O_NONBLOCK)) {
+                wait_event_interruptible_timeout(data->slaveirq_wait,
+                                                 data->data_ready,
+                                                 data->timeout);
+        }
+
+        if (data->data_ready && NULL != buf
+            && count >= sizeof(data->data)) {
+                err = copy_to_user(buf, &data->data, sizeof(data->data));
+                data->data.data_type = 0;
+        } else {
+                return 0;
+        }
+        if (err != 0) {
+                dev_err(data->dev.this_device,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        data->data_ready = 0;
+        len = sizeof(data->data);
+        return len;
+}
+
+static unsigned int slaveirq_poll(struct file *file,
+                                struct poll_table_struct *poll)
+{
+        int mask = 0;
+        struct slaveirq_dev_data *data =
+                container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+        poll_wait(file, &data->slaveirq_wait, poll);
+        if (data->data_ready)
+                mask |= POLLIN | POLLRDNORM;
+        return mask;
+}
+
+/* ioctl - I/O control */
+static long slaveirq_ioctl(struct file *file,
+                           unsigned int cmd, unsigned long arg)
+{
+        int retval = 0;
+        int tmp;
+        struct slaveirq_dev_data *data =
+                container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+        switch (cmd) {
+        case SLAVEIRQ_SET_TIMEOUT:
+                data->timeout = arg;
+                break;
+
+        case SLAVEIRQ_GET_INTERRUPT_CNT:
+                tmp = data->data.interruptcount - 1;
+                if (data->data.interruptcount > 1)
+                        data->data.interruptcount = 1;
+
+                if (copy_to_user((int *) arg, &tmp, sizeof(int)))
+                        return -EFAULT;
+                break;
+        case SLAVEIRQ_GET_IRQ_TIME:
+                if (copy_to_user((int *) arg, &data->data.irqtime,
+                                 sizeof(data->data.irqtime)))
+                        return -EFAULT;
+                data->data.irqtime = 0;
+                break;
+        default:
+                retval = -EINVAL;
+        }
+        return retval;
+}
+
+static irqreturn_t slaveirq_handler(int irq, void *dev_id)
+{
+        struct slaveirq_dev_data *data = (struct slaveirq_dev_data *)dev_id;
+        static int mycount;
+        struct timeval irqtime;
+        mycount++;
+
+        data->data.interruptcount++;
+
+        /* wake up (unblock) for reading data from userspace */
+        data->data_ready = 1;
+
+        do_gettimeofday(&irqtime);
+        data->data.irqtime = (((long long) irqtime.tv_sec) << 32);
+        data->data.irqtime += irqtime.tv_usec;
+        data->data.data_type |= 1;
+
+        wake_up_interruptible(&data->slaveirq_wait);
+
+        return IRQ_HANDLED;
+
+}
+
+/* define which file operations are supported */
+static const struct file_operations slaveirq_fops = {
+        .owner = THIS_MODULE,
+        .read = slaveirq_read,
+        .poll = slaveirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+        .compat_ioctl = slaveirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+        .unlocked_ioctl = slaveirq_ioctl,
+#endif
+        .open = slaveirq_open,
+        .release = slaveirq_release,
+};
+
+int slaveirq_init(struct i2c_adapter *slave_adapter,
+                  struct ext_slave_platform_data *pdata,
+                  char *name)
+{
+
+        int res;
+        struct slaveirq_dev_data *data;
+
+        if (!pdata->irq)
+                return -EINVAL;
+
+        pdata->irq_data = kzalloc(sizeof(*data),
+                                GFP_KERNEL);
+        data = (struct slaveirq_dev_data *) pdata->irq_data;
+        if (!data)
+                return -ENOMEM;
+
+        data->dev.minor = MISC_DYNAMIC_MINOR;
+        data->dev.name = name;
+        data->dev.fops = &slaveirq_fops;
+        data->irq = pdata->irq;
+        data->pid = 0;
+        data->data_ready = 0;
+        data->timeout = 0;
+
+        init_waitqueue_head(&data->slaveirq_wait);
+
+        res = request_irq(data->irq, slaveirq_handler, IRQF_TRIGGER_RISING,
+                          data->dev.name, data);
+
+        if (res) {
+                dev_err(&slave_adapter->dev,
+                        "myirqtest: cannot register IRQ %d\n",
+                        data->irq);
+                goto out_request_irq;
+        }
+
+        res = misc_register(&data->dev);
+        if (res < 0) {
+                dev_err(&slave_adapter->dev,
+                        "misc_register returned %d\n",
+                        res);
+                goto out_misc_register;
+        }
+
+        return res;
+
+out_misc_register:
+        free_irq(data->irq, data);
+out_request_irq:
+        kfree(pdata->irq_data);
+        pdata->irq_data = NULL;
+
+        return res;
+}
+
+void slaveirq_exit(struct ext_slave_platform_data *pdata)
+{
+        struct slaveirq_dev_data *data = pdata->irq_data;
+
+        if (!pdata->irq_data || data->irq <= 0)
+                return;
+
+        dev_info(data->dev.this_device, "Unregistering %s\n",
+                 data->dev.name);
+
+        free_irq(data->irq, data);
+        misc_deregister(&data->dev);
+        kfree(pdata->irq_data);
+        pdata->irq_data = NULL;
+}
diff --git a/drivers/misc/mpu3050/slaveirq.h b/drivers/misc/mpu3050/slaveirq.h
new file mode 100644
index 00000000000..b4e1115f1b0
--- /dev/null
+++ b/drivers/misc/mpu3050/slaveirq.h
@@ -0,0 +1,43 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+#ifndef __SLAVEIRQ__
+#define __SLAVEIRQ__
+
+#ifdef __KERNEL__
+#include <linux/i2c-dev.h>
+#endif
+
+#include "mpu.h"
+#include "mpuirq.h"
+
+#define SLAVEIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x50, unsigned long)
+#define SLAVEIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x51, unsigned long)
+#define SLAVEIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x52, unsigned long)
+
+#ifdef __KERNEL__
+
+void slaveirq_exit(struct ext_slave_platform_data *pdata);
+int slaveirq_init(struct i2c_adapter *slave_adapter,
+                struct ext_slave_platform_data *pdata,
+                char *name);
+
+#endif
+
+#endif
diff --git a/drivers/misc/mpu3050/timerirq.c b/drivers/misc/mpu3050/timerirq.c
new file mode 100644
index 00000000000..41c3ac98101
--- /dev/null
+++ b/drivers/misc/mpu3050/timerirq.c
@@ -0,0 +1,299 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/timer.h>
+#include <linux/slab.h>
+
+#include "mpu.h"
+#include "mltypes.h"
+#include "timerirq.h"
+
+/* function which gets timer data and sends it to TIMER */
+struct timerirq_data {
+        int pid;
+        int data_ready;
+        int run;
+        int timeout;
+        unsigned long period;
+        struct mpuirq_data data;
+        struct completion timer_done;
+        wait_queue_head_t timerirq_wait;
+        struct timer_list timer;
+        struct miscdevice *dev;
+};
+
+static struct miscdevice *timerirq_dev_data;
+
+static void timerirq_handler(unsigned long arg)
+{
+        struct timerirq_data *data = (struct timerirq_data *)arg;
+        struct timeval irqtime;
+
+        data->data.interruptcount++;
+
+        data->data_ready = 1;
+
+        do_gettimeofday(&irqtime);
+        data->data.irqtime = (((long long) irqtime.tv_sec) << 32);
+        data->data.irqtime += irqtime.tv_usec;
+        data->data.data_type |= 1;
+
+        dev_dbg(data->dev->this_device,
+                "%s, %lld, %ld\n", __func__, data->data.irqtime,
+                (unsigned long)data);
+
+        wake_up_interruptible(&data->timerirq_wait);
+
+        if (data->run)
+                mod_timer(&data->timer,
+                        jiffies + msecs_to_jiffies(data->period));
+        else
+                complete(&data->timer_done);
+}
+
+static int start_timerirq(struct timerirq_data *data)
+{
+        dev_dbg(data->dev->this_device,
+                "%s current->pid %d\n", __func__, current->pid);
+
+        /* Timer already running... success */
+        if (data->run)
+                return 0;
+
+        /* Don't allow a period of 0 since this would fire constantly */
+        if (!data->period)
+                return -EINVAL;
+
+        data->run = TRUE;
+        data->data_ready = FALSE;
+
+        init_completion(&data->timer_done);
+        setup_timer(&data->timer, timerirq_handler, (unsigned long)data);
+
+        return mod_timer(&data->timer,
+                        jiffies + msecs_to_jiffies(data->period));
+}
+
+static int stop_timerirq(struct timerirq_data *data)
+{
+        dev_dbg(data->dev->this_device,
+                "%s current->pid %lx\n", __func__, (unsigned long)data);
+
+        if (data->run) {
+                data->run = FALSE;
+                mod_timer(&data->timer, jiffies + 1);
+                wait_for_completion(&data->timer_done);
+        }
+        return 0;
+}
+
+/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28
+ * drivers: misc: pass miscdevice pointer via file private data
+ */
+static int timerirq_open(struct inode *inode, struct file *file)
+{
+        /* Device node is availabe in the file->private_data, this is
+         * exactly what we want so we leave it there */
+        struct miscdevice *dev_data = file->private_data;
+        struct timerirq_data *data = kzalloc(sizeof(*data), GFP_KERNEL);
+        if (!data)
+                return -ENOMEM;
+
+        data->dev = dev_data;
+        file->private_data = data;
+        data->pid = current->pid;
+        init_waitqueue_head(&data->timerirq_wait);
+
+        dev_dbg(data->dev->this_device,
+                "%s current->pid %d\n", __func__, current->pid);
+        return 0;
+}
+
+static int timerirq_release(struct inode *inode, struct file *file)
+{
+        struct timerirq_data *data = file->private_data;
+        dev_dbg(data->dev->this_device, "timerirq_release\n");
+        if (data->run)
+                stop_timerirq(data);
+        kfree(data);
+        return 0;
+}
+
+/* read function called when from /dev/timerirq is read */
+static ssize_t timerirq_read(struct file *file,
+                           char *buf, size_t count, loff_t *ppos)
+{
+        int len, err;
+        struct timerirq_data *data = file->private_data;
+
+        if (!data->data_ready &&
+                data->timeout &&
+                !(file->f_flags & O_NONBLOCK)) {
+                wait_event_interruptible_timeout(data->timerirq_wait,
+                                                 data->data_ready,
+                                                 data->timeout);
+        }
+
+        if (data->data_ready && NULL != buf
+            && count >= sizeof(data->data)) {
+                err = copy_to_user(buf, &data->data, sizeof(data->data));
+                data->data.data_type = 0;
+        } else {
+                return 0;
+        }
+        if (err != 0) {
+                dev_err(data->dev->this_device,
+                        "Copy to user returned %d\n", err);
+                return -EFAULT;
+        }
+        data->data_ready = 0;
+        len = sizeof(data->data);
+        return len;
+}
+
+static unsigned int timerirq_poll(struct file *file,
+                                struct poll_table_struct *poll)
+{
+        int mask = 0;
+        struct timerirq_data *data = file->private_data;
+
+        poll_wait(file, &data->timerirq_wait, poll);
+        if (data->data_ready)
+                mask |= POLLIN | POLLRDNORM;
+        return mask;
+}
+
+/* ioctl - I/O control */
+static long timerirq_ioctl(struct file *file,
+                           unsigned int cmd, unsigned long arg)
+{
+        int retval = 0;
+        int tmp;
+        struct timerirq_data *data = file->private_data;
+
+        dev_dbg(data->dev->this_device,
+                "%s current->pid %d, %d, %ld\n",
+                __func__, current->pid, cmd, arg);
+
+        if (!data)
+                return -EFAULT;
+
+        switch (cmd) {
+        case TIMERIRQ_SET_TIMEOUT:
+                data->timeout = arg;
+                break;
+        case TIMERIRQ_GET_INTERRUPT_CNT:
+                tmp = data->data.interruptcount - 1;
+                if (data->data.interruptcount > 1)
+                        data->data.interruptcount = 1;
+
+                if (copy_to_user((int *) arg, &tmp, sizeof(int)))
+                        return -EFAULT;
+                break;
+        case TIMERIRQ_START:
+                data->period = arg;
+                retval = start_timerirq(data);
+                break;
+        case TIMERIRQ_STOP:
+                retval = stop_timerirq(data);
+                break;
+        default:
+                retval = -EINVAL;
+        }
+        return retval;
+}
+
+/* define which file operations are supported */
+static const struct file_operations timerirq_fops = {
+        .owner = THIS_MODULE,
+        .read = timerirq_read,
+        .poll = timerirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+        .compat_ioctl = timerirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+        .unlocked_ioctl = timerirq_ioctl,
+#endif
+        .open = timerirq_open,
+        .release = timerirq_release,
+};
+
+static int __init timerirq_init(void)
+{
+
+        int res;
+        static struct miscdevice *data;
+
+        data = kzalloc(sizeof(*data), GFP_KERNEL);
+        if (!data)
+                return -ENOMEM;
+        timerirq_dev_data = data;
+        data->minor = MISC_DYNAMIC_MINOR;
+        data->name = "timerirq";
+        data->fops = &timerirq_fops;
+
+        res = misc_register(data);
+        if (res < 0) {
+                dev_err(data->this_device,
+                        "misc_register returned %d\n",
+                        res);
+                return res;
+        }
+
+        return res;
+}
+module_init(timerirq_init);
+
+static void __exit timerirq_exit(void)
+{
+        struct miscdevice *data = timerirq_dev_data;
+
+        dev_info(data->this_device, "Unregistering %s\n",
+                 data->name);
+
+        misc_deregister(data);
+        kfree(data);
+
+        timerirq_dev_data = NULL;
+}
+module_exit(timerirq_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Timer IRQ device driver.");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("timerirq");
diff --git a/drivers/misc/mpu3050/timerirq.h b/drivers/misc/mpu3050/timerirq.h
new file mode 100644
index 00000000000..ec2c1e29f08
--- /dev/null
+++ b/drivers/misc/mpu3050/timerirq.h
@@ -0,0 +1,30 @@
+/*
+ $License:
+    Copyright (C) 2010 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/>.
+  $
+ */
+
+#ifndef __TIMERIRQ__
+#define __TIMERIRQ__
+
+#include "mpu.h"
+
+#define TIMERIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x60, unsigned long)
+#define TIMERIRQ_GET_INTERRUPT_CNT     _IOW(MPU_IOCTL, 0x61, unsigned long)
+#define TIMERIRQ_START                 _IOW(MPU_IOCTL, 0x62, unsigned long)
+#define TIMERIRQ_STOP                  _IO(MPU_IOCTL, 0x63)
+
+#endif
diff --git a/drivers/misc/nct1008.c b/drivers/misc/nct1008.c
new file mode 100644
index 00000000000..3a8aa6b9dbd
--- /dev/null
+++ b/drivers/misc/nct1008.c
@@ -0,0 +1,944 @@
+/*
+ * drivers/misc/nct1008.c
+ *
+ * Driver for NCT1008, temperature monitoring device from ON Semiconductors
+ *
+ * Copyright (c) 2010-2011, NVIDIA Corporation.
+ *
+ * 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, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/device.h>
+#include <linux/nct1008.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+
+#define DRIVER_NAME "nct1008"
+
+/* Register Addresses */
+#define LOCAL_TEMP_RD                   0x00
+#define EXT_TEMP_RD_HI                  0x01
+#define EXT_TEMP_RD_LO                  0x10
+#define STATUS_RD                       0x02
+#define CONFIG_RD                       0x03
+
+#define LOCAL_TEMP_HI_LIMIT_RD          0x05
+#define LOCAL_TEMP_LO_LIMIT_RD          0x06
+
+#define EXT_TEMP_HI_LIMIT_HI_BYTE_RD    0x07
+#define EXT_TEMP_LO_LIMIT_HI_BYTE_RD    0x08
+
+#define CONFIG_WR                       0x09
+#define CONV_RATE_WR                    0x0A
+#define LOCAL_TEMP_HI_LIMIT_WR          0x0B
+#define LOCAL_TEMP_LO_LIMIT_WR          0x0C
+#define EXT_TEMP_HI_LIMIT_HI_BYTE_WR    0x0D
+#define EXT_TEMP_LO_LIMIT_HI_BYTE_WR    0x0E
+#define ONE_SHOT                        0x0F
+#define OFFSET_WR                       0x11
+#define OFFSET_QUARTER_WR               0x12
+#define EXT_THERM_LIMIT_WR              0x19
+#define LOCAL_THERM_LIMIT_WR            0x20
+#define THERM_HYSTERESIS_WR             0x21
+
+/* Configuration Register Bits */
+#define EXTENDED_RANGE_BIT              BIT(2)
+#define THERM2_BIT                      BIT(5)
+#define STANDBY_BIT                     BIT(6)
+#define ALERT_BIT                       BIT(7)
+
+/* Max Temperature Measurements */
+#define EXTENDED_RANGE_OFFSET           64U
+#define STANDARD_RANGE_MAX              127U
+#define EXTENDED_RANGE_MAX              (150U + EXTENDED_RANGE_OFFSET)
+
+#define NCT1008_MIN_TEMP -64
+#define NCT1008_MAX_TEMP 191
+
+#define MAX_STR_PRINT 50
+
+#define MAX_CONV_TIME_ONESHOT_MS (52)
+#define CELSIUS_TO_MILLICELSIUS(x) ((x)*1000)
+#define MILLICELSIUS_TO_CELSIUS(x) ((x)/1000)
+
+
+static int conv_period_ms_table[] =
+        {16000, 8000, 4000, 2000, 1000, 500, 250, 125, 63, 32, 16};
+
+static inline s8 value_to_temperature(bool extended, u8 value)
+{
+        return extended ? (s8)(value - EXTENDED_RANGE_OFFSET) : (s8)value;
+}
+
+static inline u8 temperature_to_value(bool extended, s8 temp)
+{
+        return extended ? (u8)(temp + EXTENDED_RANGE_OFFSET) : (u8)temp;
+}
+
+static int nct1008_get_temp(struct device *dev, long *pTemp)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct nct1008_platform_data *pdata = client->dev.platform_data;
+        s8 temp_local;
+        u8 temp_ext_lo;
+        s8 temp_ext_hi;
+        long temp_ext_milli;
+        long temp_local_milli;
+        u8 value;
+
+        /* Read Local Temp */
+        value = i2c_smbus_read_byte_data(client, LOCAL_TEMP_RD);
+        if (value < 0)
+                goto error;
+        temp_local = value_to_temperature(pdata->ext_range, value);
+        temp_local_milli = CELSIUS_TO_MILLICELSIUS(temp_local);
+
+        /* Read External Temp */
+        value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_LO);
+        if (value < 0)
+                goto error;
+        temp_ext_lo = (value >> 6);
+
+        value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_HI);
+        if (value < 0)
+                goto error;
+        temp_ext_hi = value_to_temperature(pdata->ext_range, value);
+
+        temp_ext_milli = CELSIUS_TO_MILLICELSIUS(temp_ext_hi) +
+                                temp_ext_lo * 250;
+
+        /* Return max between Local and External Temp */
+        *pTemp = max(temp_local_milli, temp_ext_milli);
+
+        dev_dbg(dev, "\n %s: ret temp=%ldC ", __func__, *pTemp);
+        return 0;
+error:
+        dev_err(&client->dev, "\n error in file=: %s %s() line=%d: "
+                "error=%d ", __FILE__, __func__, __LINE__, value);
+        return value;
+}
+
+static ssize_t nct1008_show_temp(struct device *dev,
+        struct device_attribute *attr, char *buf)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct nct1008_platform_data *pdata = client->dev.platform_data;
+        s8 temp1 = 0;
+        s8 temp = 0;
+        u8 temp2 = 0;
+        int value = 0;
+
+        if (!dev || !buf || !attr)
+                return -EINVAL;
+
+        value = i2c_smbus_read_byte_data(client, LOCAL_TEMP_RD);
+        if (value < 0)
+                goto error;
+        temp1 = value_to_temperature(pdata->ext_range, value);
+
+        value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_LO);
+        if (value < 0)
+                goto error;
+        temp2 = (value >> 6);
+        value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_HI);
+        if (value < 0)
+                goto error;
+        temp = value_to_temperature(pdata->ext_range, value);
+
+        return snprintf(buf, MAX_STR_PRINT, "%d %d.%d\n",
+                temp1, temp, temp2 * 25);
+
+error:
+        return snprintf(buf, MAX_STR_PRINT,
+                "Error read local/ext temperature\n");
+}
+
+static ssize_t nct1008_show_temp_overheat(struct device *dev,
+                                struct device_attribute *attr,
+                                char *buf)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct nct1008_platform_data *pdata = client->dev.platform_data;
+        int value;
+        s8 temp, temp2;
+
+        /* Local temperature h/w shutdown limit */
+        value = i2c_smbus_read_byte_data(client, LOCAL_THERM_LIMIT_WR);
+        if (value < 0)
+                goto error;
+        temp = value_to_temperature(pdata->ext_range, value);
+
+        /* External temperature h/w shutdown limit */
+        value = i2c_smbus_read_byte_data(client, EXT_THERM_LIMIT_WR);
+        if (value < 0)
+                goto error;
+        temp2 = value_to_temperature(pdata->ext_range, value);
+
+        return snprintf(buf, MAX_STR_PRINT, "%d %d\n", temp, temp2);
+error:
+        dev_err(dev, "%s: failed to read temperature-overheat "
+                "\n", __func__);
+        return snprintf(buf, MAX_STR_PRINT, " Rd overheat Error\n");
+}
+
+static ssize_t nct1008_set_temp_overheat(struct device *dev,
+                        struct device_attribute *attr,
+                        const char *buf, size_t count)
+{
+        long int num;
+        int err;
+        u8 temp;
+        long currTemp;
+        struct i2c_client *client = to_i2c_client(dev);
+        struct nct1008_platform_data *pdata = client->dev.platform_data;
+        char bufTemp[MAX_STR_PRINT];
+        char bufOverheat[MAX_STR_PRINT];
+        unsigned int ret;
+
+        if (strict_strtol(buf, 0, &num)) {
+                dev_err(dev, "\n file: %s, line=%d return %s() ", __FILE__,
+                        __LINE__, __func__);
+                return -EINVAL;
+        }
+        if (((int)num < NCT1008_MIN_TEMP) || ((int)num >= NCT1008_MAX_TEMP)) {
+                dev_err(dev, "\n file: %s, line=%d return %s() ", __FILE__,
+                        __LINE__, __func__);
+                return -EINVAL;
+        }
+        /* check for system power down */
+        err = nct1008_get_temp(dev, &currTemp);
+        if (err)
+                goto error;
+
+        currTemp = MILLICELSIUS_TO_CELSIUS(currTemp);
+
+        if (currTemp >= (int)num) {
+                ret = nct1008_show_temp(dev, attr, bufTemp);
+                ret = nct1008_show_temp_overheat(dev, attr, bufOverheat);
+                dev_err(dev, "\nCurrent temp: %s ", bufTemp);
+                dev_err(dev, "\nOld overheat limit: %s ", bufOverheat);
+                dev_err(dev, "\nReset from overheat: curr temp=%ld, "
+                        "new overheat temp=%d\n\n", currTemp, (int)num);
+        }
+
+        /* External temperature h/w shutdown limit */
+        temp = temperature_to_value(pdata->ext_range, (s8)num);
+        err = i2c_smbus_write_byte_data(client, EXT_THERM_LIMIT_WR, temp);
+        if (err < 0)
+                goto error;
+
+        /* Local temperature h/w shutdown limit */
+        temp = temperature_to_value(pdata->ext_range, (s8)num);
+        err = i2c_smbus_write_byte_data(client, LOCAL_THERM_LIMIT_WR, temp);
+        if (err < 0)
+                goto error;
+        return count;
+error:
+        dev_err(dev, " %s: failed to set temperature-overheat\n", __func__);
+        return err;
+}
+
+static ssize_t nct1008_show_temp_alert(struct device *dev,
+                                struct device_attribute *attr,
+                                char *buf)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct nct1008_platform_data *pdata = client->dev.platform_data;
+        int value;
+        s8 temp_hi, temp_lo;
+        /* External Temperature Throttling hi-limit */
+        value = i2c_smbus_read_byte_data(client, EXT_TEMP_HI_LIMIT_HI_BYTE_RD);
+        if (value < 0)
+                goto error;
+        temp_hi = value_to_temperature(pdata->ext_range, value);
+
+        /* External Temperature Throttling lo-limit */
+        value = i2c_smbus_read_byte_data(client, EXT_TEMP_LO_LIMIT_HI_BYTE_RD);
+        if (value < 0)
+                goto error;
+        temp_lo = value_to_temperature(pdata->ext_range, value);
+
+        return snprintf(buf, MAX_STR_PRINT, "lo:%d hi:%d\n", temp_lo, temp_hi);
+error:
+        dev_err(dev, "%s: failed to read temperature-alert\n", __func__);
+        return snprintf(buf, MAX_STR_PRINT, " Rd alert Error\n");
+}
+
+static ssize_t nct1008_set_temp_alert(struct device *dev,
+                        struct device_attribute *attr,
+                        const char *buf, size_t count)
+{
+        long int num;
+        int value;
+        int err;
+        struct i2c_client *client = to_i2c_client(dev);
+        struct nct1008_platform_data *pdata = client->dev.platform_data;
+
+        if (strict_strtol(buf, 0, &num)) {
+                dev_err(dev, "\n file: %s, line=%d return %s() ", __FILE__,
+                        __LINE__, __func__);
+                return -EINVAL;
+        }
+        if (((int)num < NCT1008_MIN_TEMP) || ((int)num >= NCT1008_MAX_TEMP)) {
+                dev_err(dev, "\n file: %s, line=%d return %s() ", __FILE__,
+                        __LINE__, __func__);
+                return -EINVAL;
+        }
+
+        /* External Temperature Throttling limit */
+        value = temperature_to_value(pdata->ext_range, (s8)num);
+        err = i2c_smbus_write_byte_data(client, EXT_TEMP_HI_LIMIT_HI_BYTE_WR,
+                value);
+        if (err < 0)
+                goto error;
+
+        /* Local Temperature Throttling limit */
+        err = i2c_smbus_write_byte_data(client, LOCAL_TEMP_HI_LIMIT_WR,
+                value);
+        if (err < 0)
+                goto error;
+
+        return count;
+error:
+        dev_err(dev, "%s: failed to set temperature-alert "
+                "\n", __func__);
+        return err;
+}
+
+static ssize_t nct1008_show_ext_temp(struct device *dev,
+        struct device_attribute *attr, char *buf)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct nct1008_platform_data *pdata = client->dev.platform_data;
+        s8 temp_value;
+        int data = 0;
+        int data_lo;
+
+        if (!dev || !buf || !attr)
+                return -EINVAL;
+
+        /* When reading the full external temperature value, read the
+         * LSB first. This causes the MSB to be locked (that is, the
+         * ADC does not write to it) until it is read */
+        data_lo = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_LO);
+        if (data_lo < 0) {
+                dev_err(&client->dev, "%s: failed to read "
+                        "ext_temperature, i2c error=%d\n", __func__, data_lo);
+                goto error;
+        }
+
+        data = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_HI);
+        if (data < 0) {
+                dev_err(&client->dev, "%s: failed to read "
+                        "ext_temperature, i2c error=%d\n", __func__, data);
+                goto error;
+        }
+
+        temp_value = value_to_temperature(pdata->ext_range, data);
+
+        return snprintf(buf, MAX_STR_PRINT, "%d.%d\n", temp_value,
+                (25 * (data_lo >> 6)));
+error:
+        return snprintf(buf, MAX_STR_PRINT, "Error read ext temperature\n");
+}
+
+static DEVICE_ATTR(temperature, S_IRUGO, nct1008_show_temp, NULL);
+static DEVICE_ATTR(temperature_overheat, (S_IRUGO | (S_IWUSR | S_IWGRP)),
+                nct1008_show_temp_overheat, nct1008_set_temp_overheat);
+static DEVICE_ATTR(temperature_alert, (S_IRUGO | (S_IWUSR | S_IWGRP)),
+                nct1008_show_temp_alert, nct1008_set_temp_alert);
+static DEVICE_ATTR(ext_temperature, S_IRUGO, nct1008_show_ext_temp, NULL);
+
+static struct attribute *nct1008_attributes[] = {
+        &dev_attr_temperature.attr,
+        &dev_attr_temperature_overheat.attr,
+        &dev_attr_temperature_alert.attr,
+        &dev_attr_ext_temperature.attr,
+        NULL
+};
+
+static const struct attribute_group nct1008_attr_group = {
+        .attrs = nct1008_attributes,
+};
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+static void print_reg(const char *reg_name, struct seq_file *s,
+                int offset)
+{
+        struct nct1008_data *nct_data = s->private;
+        int ret;
+
+        ret = i2c_smbus_read_byte_data(nct_data->client,
+                offset);
+        if (ret >= 0)
+                seq_printf(s, "Reg %s Addr = 0x%02x Reg 0x%02x "
+                "Value 0x%02x\n", reg_name,
+                nct_data->client->addr,
+                        offset, ret);
+        else
+                seq_printf(s, "%s: line=%d, i2c read error=%d\n",
+                __func__, __LINE__, ret);
+}
+
+static int dbg_nct1008_show(struct seq_file *s, void *unused)
+{
+        seq_printf(s, "nct1008 Registers\n");
+        seq_printf(s, "------------------\n");
+        print_reg("Local Temp Value    ",     s, 0x00);
+        print_reg("Ext Temp Value Hi   ",     s, 0x01);
+        print_reg("Status              ",     s, 0x02);
+        print_reg("Configuration       ",     s, 0x03);
+        print_reg("Conversion Rate     ",     s, 0x04);
+        print_reg("Local Temp Hi Limit ",     s, 0x05);
+        print_reg("Local Temp Lo Limit ",     s, 0x06);
+        print_reg("Ext Temp Hi Limit Hi",     s, 0x07);
+        print_reg("Ext Temp Hi Limit Lo",     s, 0x13);
+        print_reg("Ext Temp Lo Limit Hi",     s, 0x08);
+        print_reg("Ext Temp Lo Limit Lo",     s, 0x14);
+        print_reg("Ext Temp Value Lo   ",     s, 0x10);
+        print_reg("Ext Temp Offset Hi  ",     s, 0x11);
+        print_reg("Ext Temp Offset Lo  ",     s, 0x12);
+        print_reg("Ext THERM Limit     ",     s, 0x19);
+        print_reg("Local THERM Limit   ",     s, 0x20);
+        print_reg("THERM Hysteresis    ",     s, 0x21);
+        print_reg("Consecutive ALERT   ",     s, 0x22);
+        return 0;
+}
+
+static int dbg_nct1008_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, dbg_nct1008_show, inode->i_private);
+}
+
+static const struct file_operations debug_fops = {
+        .open           = dbg_nct1008_open,
+        .read           = seq_read,
+        .llseek         = seq_lseek,
+        .release        = single_release,
+};
+
+static int __init nct1008_debuginit(struct nct1008_data *nct)
+{
+        int err = 0;
+        struct dentry *d;
+        d = debugfs_create_file("nct1008", S_IRUGO, NULL,
+                        (void *)nct, &debug_fops);
+        if ((!d) || IS_ERR(d)) {
+                dev_err(&nct->client->dev, "Error: %s debugfs_create_file"
+                        " returned an error\n", __func__);
+                err = -ENOENT;
+                goto end;
+        }
+        if (d == ERR_PTR(-ENODEV)) {
+                dev_err(&nct->client->dev, "Error: %s debugfs not supported "
+                        "error=-ENODEV\n", __func__);
+                err = -ENODEV;
+        } else {
+                nct->dent = d;
+        }
+end:
+        return err;
+}
+#else
+static int __init nct1008_debuginit(struct nct1008_data *nct)
+{
+        return 0;
+}
+#endif
+
+static int nct1008_enable(struct i2c_client *client)
+{
+        struct nct1008_data *data = i2c_get_clientdata(client);
+        int err;
+
+        err = i2c_smbus_write_byte_data(client, CONFIG_WR,
+                                  data->config & ~STANDBY_BIT);
+        if (err < 0)
+                dev_err(&client->dev, "%s, line=%d, i2c write error=%d\n",
+                __func__, __LINE__, err);
+        return err;
+}
+
+static int nct1008_disable(struct i2c_client *client)
+{
+        struct nct1008_data *data = i2c_get_clientdata(client);
+        int err;
+
+        err = i2c_smbus_write_byte_data(client, CONFIG_WR,
+                                  data->config | STANDBY_BIT);
+        if (err < 0)
+                dev_err(&client->dev, "%s, line=%d, i2c write error=%d\n",
+                __func__, __LINE__, err);
+        return err;
+}
+
+static int nct1008_within_limits(struct nct1008_data *data)
+{
+        int intr_status;
+
+        intr_status = i2c_smbus_read_byte_data(data->client, STATUS_RD);
+
+        return !(intr_status & (BIT(3) | BIT(4)));
+}
+
+static void nct1008_work_func(struct work_struct *work)
+{
+        struct nct1008_data *data = container_of(work, struct nct1008_data,
+                                                work);
+        int intr_status;
+        struct timespec ts;
+
+        nct1008_disable(data->client);
+
+        if (data->alert_func)
+                if (!nct1008_within_limits(data))
+                        data->alert_func(data->alert_data);
+
+        /* Initiate one-shot conversion */
+        i2c_smbus_write_byte_data(data->client, ONE_SHOT, 0x1);
+
+        /* Give hardware necessary time to finish conversion */
+        ts = ns_to_timespec(MAX_CONV_TIME_ONESHOT_MS * 1000 * 1000);
+        hrtimer_nanosleep(&ts, NULL, HRTIMER_MODE_REL, CLOCK_MONOTONIC);
+
+        intr_status = i2c_smbus_read_byte_data(data->client, STATUS_RD);
+
+        nct1008_enable(data->client);
+
+        enable_irq(data->client->irq);
+}
+
+static irqreturn_t nct1008_irq(int irq, void *dev_id)
+{
+        struct nct1008_data *data = dev_id;
+
+        disable_irq_nosync(irq);
+        queue_work(data->workqueue, &data->work);
+
+        return IRQ_HANDLED;
+}
+
+static void nct1008_power_control(struct nct1008_data *data, bool is_enable)
+{
+        int ret;
+        if (!data->nct_reg) {
+                data->nct_reg = regulator_get(&data->client->dev, "vdd");
+                if (IS_ERR_OR_NULL(data->nct_reg)) {
+                        dev_warn(&data->client->dev, "Error [%d] in"
+                                "getting the regulator handle for vdd "
+                                "of %s\n", (int)data->nct_reg,
+                                dev_name(&data->client->dev));
+                        data->nct_reg = NULL;
+                        return;
+                }
+        }
+        if (is_enable)
+                ret = regulator_enable(data->nct_reg);
+        else
+                ret = regulator_disable(data->nct_reg);
+
+        if (ret < 0)
+                dev_err(&data->client->dev, "Error in %s rail vdd_nct1008, "
+                        "error %d\n", (is_enable) ? "enabling" : "disabling",
+                        ret);
+        else
+                dev_info(&data->client->dev, "success in %s rail vdd_nct1008\n",
+                        (is_enable) ? "enabling" : "disabling");
+}
+
+static int __devinit nct1008_configure_sensor(struct nct1008_data* data)
+{
+        struct i2c_client *client = data->client;
+        struct nct1008_platform_data *pdata = client->dev.platform_data;
+        u8 value;
+        s8 temp;
+        u8 temp2;
+        int err;
+
+        if (!pdata || !pdata->supported_hwrev)
+                return -ENODEV;
+
+        /* Place in Standby */
+        data->config = STANDBY_BIT;
+        err = i2c_smbus_write_byte_data(client, CONFIG_WR, data->config);
+        if (err)
+                goto error;
+
+        /* External temperature h/w shutdown limit */
+        value = temperature_to_value(pdata->ext_range, NCT1008_MAX_TEMP);
+        err = i2c_smbus_write_byte_data(client, EXT_THERM_LIMIT_WR, value);
+        if (err)
+                goto error;
+
+        /* Local temperature h/w shutdown limit */
+        value = temperature_to_value(pdata->ext_range, NCT1008_MAX_TEMP);
+        err = i2c_smbus_write_byte_data(client, LOCAL_THERM_LIMIT_WR, value);
+        if (err)
+                goto error;
+
+        /* set extended range mode if needed */
+        if (pdata->ext_range)
+                data->config |= EXTENDED_RANGE_BIT;
+        data->config &= ~(THERM2_BIT | ALERT_BIT);
+
+        err = i2c_smbus_write_byte_data(client, CONFIG_WR, data->config);
+        if (err)
+                goto error;
+
+        /* Temperature conversion rate */
+        err = i2c_smbus_write_byte_data(client, CONV_RATE_WR, pdata->conv_rate);
+        if (err)
+                goto error;
+
+        data->conv_period_ms = conv_period_ms_table[pdata->conv_rate];
+
+        /* Setup local hi and lo limits */
+        err = i2c_smbus_write_byte_data(client,
+                LOCAL_TEMP_HI_LIMIT_WR, NCT1008_MAX_TEMP);
+        if (err)
+                goto error;
+
+        err = i2c_smbus_write_byte_data(client,
+                LOCAL_TEMP_LO_LIMIT_WR, 0);
+        if (err)
+                goto error;
+
+        /* Setup external hi and lo limits */
+        err = i2c_smbus_write_byte_data(client,
+                EXT_TEMP_LO_LIMIT_HI_BYTE_WR, 0);
+        if (err)
+                goto error;
+        err = i2c_smbus_write_byte_data(client, EXT_TEMP_HI_LIMIT_HI_BYTE_WR,
+                        NCT1008_MAX_TEMP);
+        if (err)
+                goto error;
+
+        /* read initial temperature */
+        value = i2c_smbus_read_byte_data(client, LOCAL_TEMP_RD);
+        if (value < 0) {
+                err = value;
+                goto error;
+        }
+        temp = value_to_temperature(pdata->ext_range, value);
+        dev_dbg(&client->dev, "\n initial local temp = %d ", temp);
+
+        value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_LO);
+        if (value < 0) {
+                err = value;
+                goto error;
+        }
+        temp2 = (value >> 6);
+        value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_HI);
+        if (value < 0) {
+                err = value;
+                goto error;
+        }
+        temp = value_to_temperature(pdata->ext_range, value);
+
+        if (temp2 > 0)
+                dev_dbg(&client->dev, "\n initial ext temp = %d.%d deg",
+                                temp, temp2 * 25);
+        else
+                dev_dbg(&client->dev, "\n initial ext temp = %d.0 deg", temp);
+
+        /* Remote channel offset */
+        err = i2c_smbus_write_byte_data(client, OFFSET_WR, pdata->offset / 4);
+        if (err < 0)
+                goto error;
+
+        /* Remote channel offset fraction (quarters) */
+        err = i2c_smbus_write_byte_data(client, OFFSET_QUARTER_WR,
+                                        (pdata->offset % 4) << 6);
+        if (err < 0)
+                goto error;
+
+        /* register sysfs hooks */
+        err = sysfs_create_group(&client->dev.kobj, &nct1008_attr_group);
+        if (err < 0) {
+                dev_err(&client->dev, "\n sysfs create err=%d ", err);
+                goto error;
+        }
+
+        return 0;
+error:
+        dev_err(&client->dev, "\n exit %s, err=%d ", __func__, err);
+        return err;
+}
+
+static int __devinit nct1008_configure_irq(struct nct1008_data *data)
+{
+        data->workqueue = create_singlethread_workqueue("nct1008");
+
+        INIT_WORK(&data->work, nct1008_work_func);
+
+        if (data->client->irq < 0)
+                return 0;
+        else
+                return request_irq(data->client->irq, nct1008_irq,
+                        IRQF_TRIGGER_LOW,
+                        DRIVER_NAME, data);
+}
+
+int nct1008_thermal_get_temp(struct nct1008_data *data, long *temp)
+{
+        return nct1008_get_temp(&data->client->dev, temp);
+}
+
+int nct1008_thermal_get_temp_low(struct nct1008_data *data, long *temp)
+{
+        *temp = 0;
+        return 0;
+}
+
+int nct1008_thermal_set_limits(struct nct1008_data *data,
+                                long lo_limit_milli,
+                                long hi_limit_milli)
+{
+        int err;
+        u8 value;
+        bool extended_range = data->plat_data.ext_range;
+        long lo_limit = MILLICELSIUS_TO_CELSIUS(lo_limit_milli);
+        long hi_limit = MILLICELSIUS_TO_CELSIUS(hi_limit_milli);
+
+        if (lo_limit >= hi_limit)
+                return -EINVAL;
+
+        if (data->current_lo_limit != lo_limit) {
+                value = temperature_to_value(extended_range, lo_limit);
+                pr_debug("%s: set lo_limit %ld\n", __func__, lo_limit);
+                err = i2c_smbus_write_byte_data(data->client,
+                                EXT_TEMP_LO_LIMIT_HI_BYTE_WR, value);
+                if (err)
+                        return err;
+
+                data->current_lo_limit = lo_limit;
+        }
+
+        if (data->current_hi_limit != hi_limit) {
+                value = temperature_to_value(extended_range, hi_limit);
+                pr_debug("%s: set hi_limit %ld\n", __func__, hi_limit);
+                err = i2c_smbus_write_byte_data(data->client,
+                                EXT_TEMP_HI_LIMIT_HI_BYTE_WR, value);
+                if (err)
+                        return err;
+
+                data->current_hi_limit = hi_limit;
+        }
+
+        return 0;
+}
+
+int nct1008_thermal_set_alert(struct nct1008_data *data,
+                                void (*alert_func)(void *),
+                                void *alert_data)
+{
+        data->alert_data = alert_data;
+        data->alert_func = alert_func;
+
+        return 0;
+}
+
+int nct1008_thermal_set_shutdown_temp(struct nct1008_data *data,
+                                        long shutdown_temp_milli)
+{
+        struct i2c_client *client = data->client;
+        struct nct1008_platform_data *pdata = client->dev.platform_data;
+        int err;
+        u8 value;
+        long shutdown_temp;
+
+        shutdown_temp = MILLICELSIUS_TO_CELSIUS(shutdown_temp_milli);
+
+        /* External temperature h/w shutdown limit */
+        value = temperature_to_value(pdata->ext_range, shutdown_temp);
+        err = i2c_smbus_write_byte_data(client, EXT_THERM_LIMIT_WR, value);
+        if (err)
+                return err;
+
+        /* Local temperature h/w shutdown limit */
+        value = temperature_to_value(pdata->ext_range, shutdown_temp);
+        err = i2c_smbus_write_byte_data(client, LOCAL_THERM_LIMIT_WR, value);
+        if (err)
+                return err;
+
+        return 0;
+}
+
+/*
+ * Manufacturer(OnSemi) recommended sequence for
+ * Extended Range mode is as follows
+ * 1. Place in Standby
+ * 2. Scale the THERM and ALERT limits
+ *      appropriately(for Extended Range mode).
+ * 3. Enable Extended Range mode.
+ *      ALERT mask/THERM2 mode may be done here
+ *      as these are not critical
+ * 4. Set Conversion Rate as required
+ * 5. Take device out of Standby
+ */
+
+/*
+ * function nct1008_probe takes care of initial configuration
+ */
+static int __devinit nct1008_probe(struct i2c_client *client,
+                                const struct i2c_device_id *id)
+{
+        struct nct1008_data *data;
+        int err;
+        unsigned int delay;
+
+        data = kzalloc(sizeof(struct nct1008_data), GFP_KERNEL);
+        if (!data)
+                return -ENOMEM;
+
+        data->client = client;
+        memcpy(&data->plat_data, client->dev.platform_data,
+                sizeof(struct nct1008_platform_data));
+        i2c_set_clientdata(client, data);
+
+        nct1008_power_control(data, true);
+        /* extended range recommended steps 1 through 4 taken care
+         * in nct1008_configure_sensor function */
+        err = nct1008_configure_sensor(data);   /* sensor is in standby */
+        if (err < 0) {
+                dev_err(&client->dev, "\n error file: %s : %s(), line=%d ",
+                        __FILE__, __func__, __LINE__);
+                goto error;
+        }
+
+        err = nct1008_configure_irq(data);
+        if (err < 0) {
+                dev_err(&client->dev, "\n error file: %s : %s(), line=%d ",
+                        __FILE__, __func__, __LINE__);
+                goto error;
+        }
+        dev_info(&client->dev, "%s: initialized\n", __func__);
+
+        /* extended range recommended step 5 is in nct1008_enable function */
+        err = nct1008_enable(client);           /* sensor is running */
+        if (err < 0) {
+                dev_err(&client->dev, "Error: %s, line=%d, error=%d\n",
+                        __func__, __LINE__, err);
+                goto error;
+        }
+
+        err = nct1008_debuginit(data);
+        if (err < 0)
+                err = 0; /* without debugfs we may continue */
+
+        /* notify callback that probe is done */
+        if (data->plat_data.probe_callback)
+                data->plat_data.probe_callback(data);
+
+        return 0;
+
+error:
+        dev_err(&client->dev, "\n exit %s, err=%d ", __func__, err);
+        nct1008_power_control(data, false);
+        if (data->nct_reg)
+                regulator_put(data->nct_reg);
+        kfree(data);
+        return err;
+}
+
+static int __devexit nct1008_remove(struct i2c_client *client)
+{
+        struct nct1008_data *data = i2c_get_clientdata(client);
+
+        if (data->dent)
+                debugfs_remove(data->dent);
+
+        free_irq(data->client->irq, data);
+        cancel_work_sync(&data->work);
+        sysfs_remove_group(&client->dev.kobj, &nct1008_attr_group);
+        nct1008_power_control(data, false);
+        if (data->nct_reg)
+                regulator_put(data->nct_reg);
+
+        kfree(data);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int nct1008_suspend(struct i2c_client *client, pm_message_t state)
+{
+        int err;
+
+        disable_irq(client->irq);
+        err = nct1008_disable(client);
+        return err;
+}
+
+static int nct1008_resume(struct i2c_client *client)
+{
+        struct nct1008_data *data = i2c_get_clientdata(client);
+        int err;
+
+        err = nct1008_enable(client);
+        if (err < 0) {
+                dev_err(&client->dev, "Error: %s, error=%d\n",
+                        __func__, err);
+                return err;
+        }
+        enable_irq(client->irq);
+
+        return 0;
+}
+#endif
+
+static const struct i2c_device_id nct1008_id[] = {
+        { DRIVER_NAME, 0 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, nct1008_id);
+
+static struct i2c_driver nct1008_driver = {
+        .driver = {
+                .name   = DRIVER_NAME,
+        },
+        .probe          = nct1008_probe,
+        .remove         = __devexit_p(nct1008_remove),
+        .id_table       = nct1008_id,
+#ifdef CONFIG_PM
+        .suspend        = nct1008_suspend,
+        .resume         = nct1008_resume,
+#endif
+};
+
+static int __init nct1008_init(void)
+{
+        return i2c_add_driver(&nct1008_driver);
+}
+
+static void __exit nct1008_exit(void)
+{
+        i2c_del_driver(&nct1008_driver);
+}
+
+MODULE_DESCRIPTION("Temperature sensor driver for OnSemi NCT1008");
+MODULE_LICENSE("GPL");
+
+module_init(nct1008_init);
+module_exit(nct1008_exit);
diff --git a/drivers/misc/tegra-baseband/Kconfig b/drivers/misc/tegra-baseband/Kconfig
new file mode 100644
index 00000000000..1f116918296
--- /dev/null
+++ b/drivers/misc/tegra-baseband/Kconfig
@@ -0,0 +1,32 @@
+menuconfig TEGRA_BB_SUPPORT
+        bool "Tegra baseband support"
+        depends on ARCH_TEGRA
+        ---help---
+          Say Y here to get to see options for tegra baseband support.
+          This option alone does not add any kernel code.
+
+          If you say N, all options in this submenu will be skipped and disabled.
+
+if TEGRA_BB_SUPPORT
+
+config TEGRA_BB_POWER
+        bool "Enable tegra baseband power driver"
+        ---help---
+         Adds power management driver for managing different baseband
+         modems with tegra processor.
+
+         This driver should work with at least the following devices:
+
+            * STE M7400
+            * ...
+
+         Disabled by default. Choose Y here if you want to build the driver.
+
+config TEGRA_BB_M7400
+        bool "Enable driver for M7400 modem"
+        ---help---
+         Enables driver for M7400 modem.
+
+         Disabled by default. Choose Y here if you want to build the driver.
+
+endif # TEGRA_BB_SUPPORT
diff --git a/drivers/misc/tegra-baseband/Makefile b/drivers/misc/tegra-baseband/Makefile
new file mode 100644
index 00000000000..a95d84dbf11
--- /dev/null
+++ b/drivers/misc/tegra-baseband/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for tegra baseband support.
+#
+
+obj-$(CONFIG_TEGRA_BB_POWER)    += bb-power.o
+obj-$(CONFIG_TEGRA_BB_M7400)    += bb-m7400.o
diff --git a/drivers/misc/tegra-baseband/bb-m7400.c b/drivers/misc/tegra-baseband/bb-m7400.c
new file mode 100644
index 00000000000..5808a6e321c
--- /dev/null
+++ b/drivers/misc/tegra-baseband/bb-m7400.c
@@ -0,0 +1,340 @@
+/*
+ * drivers/misc/tegra-baseband/bb-m7400.c
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/resource.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/err.h>
+#include <linux/device.h>
+#include <linux/usb.h>
+#include <linux/wakelock.h>
+#include <asm/mach-types.h>
+#include <asm/mach/arch.h>
+#include <mach/tegra-bb-power.h>
+#include <mach/usb_phy.h>
+#include "bb-power.h"
+
+static struct tegra_bb_gpio_data m7400_gpios[] = {
+        { { GPIO_INVALID, GPIOF_OUT_INIT_LOW, "MDM_PWR_ON" }, true },
+        { { GPIO_INVALID, GPIOF_IN, "MDM_PWRSTATUS" }, true },
+        { { GPIO_INVALID, GPIOF_OUT_INIT_HIGH, "MDM_SERVICE" }, true },
+        { { GPIO_INVALID, GPIOF_OUT_INIT_LOW, "MDM_USB_AWR" }, false },
+        { { GPIO_INVALID, GPIOF_IN, "MDM_USB_CWR" }, false },
+        { { GPIO_INVALID, GPIOF_IN, "MDM_RESOUT2" }, true },
+        { { GPIO_INVALID, GPIOF_OUT_INIT_LOW, "MDM_USB_ARR" }, false },
+        { { GPIO_INVALID, 0, NULL }, false },   /* End of table */
+};
+static bool ehci_registered;
+static int modem_status;
+static int gpio_awr;
+static int gpio_cwr;
+static int gpio_arr;
+static struct usb_device *m7400_usb_device;
+
+static int gpio_wait_timeout(int gpio, int value, int timeout_msec)
+{
+        int count;
+        for (count = 0; count < timeout_msec; ++count) {
+                if (gpio_get_value(gpio) == value)
+                        return 0;
+                mdelay(1);
+        }
+        return -1;
+}
+
+static int m7400_enum_handshake(void)
+{
+        int retval = 0;
+
+        /* Wait for CP to indicate ready - by driving CWR high. */
+        if (gpio_wait_timeout(gpio_cwr, 1, 10) != 0) {
+                        pr_info("%s: Error: timeout waiting for modem resume.\n",
+                                                        __func__);
+                        retval = -1;
+        }
+
+        /* Signal AP ready - Drive AWR and ARR high. */
+        gpio_set_value(gpio_awr, 1);
+        gpio_set_value(gpio_arr, 1);
+
+        return retval;
+}
+
+static int m7400_apup_handshake(bool checkresponse)
+{
+        int retval = 0;
+
+        /* Signal AP ready - Drive AWR and ARR high. */
+        gpio_set_value(gpio_awr, 1);
+        gpio_set_value(gpio_arr, 1);
+
+        if (checkresponse) {
+                /* Wait for CP ack - by driving CWR high. */
+                if (gpio_wait_timeout(gpio_cwr, 1, 10) != 0) {
+                        pr_info("%s: Error: timeout waiting for modem ack.\n",
+                                                        __func__);
+                        retval = -1;
+                }
+        }
+        return retval;
+}
+
+static void m7400_apdown_handshake(void)
+{
+        /* Signal AP going down to modem - Drive AWR low. */
+        /* No need to wait for a CP response */
+        gpio_set_value(gpio_awr, 0);
+}
+
+static int m7400_l2_suspend(void)
+{
+        /* Gets called for two cases :
+                a) Port suspend.
+                b) Bus suspend. */
+        if (modem_status == BBSTATE_L2)
+                return 0;
+
+        /* Post bus suspend: Drive ARR low. */
+        gpio_set_value(gpio_arr, 0);
+        modem_status = BBSTATE_L2;
+        return 0;
+}
+
+static int m7400_l2_resume(void)
+{
+        /* Gets called for two cases :
+                a) L2 resume.
+                b) bus resume phase of L3 resume. */
+        if (modem_status == BBSTATE_L0)
+                return 0;
+
+        /* Pre bus resume: Drive ARR high. */
+        gpio_set_value(gpio_arr, 1);
+
+        /* If host initiated resume - Wait for CP ack (CWR goes high). */
+        /* If device initiated resume - CWR will be already high. */
+        if (gpio_wait_timeout(gpio_cwr, 1, 10) != 0) {
+                pr_info("%s: Error: timeout waiting for modem ack.\n",
+                                                __func__);
+                return -1;
+        }
+        modem_status = BBSTATE_L0;
+        return 0;
+}
+
+static void m7400_l3_suspend(void)
+{
+        m7400_apdown_handshake();
+        modem_status = BBSTATE_L3;
+}
+
+static void m7400_l3_resume(void)
+{
+        m7400_apup_handshake(true);
+        modem_status = BBSTATE_L0;
+}
+
+static irqreturn_t m7400_wake_irq(int irq, void *dev_id)
+{
+        struct usb_interface *intf;
+
+        switch (modem_status) {
+        case BBSTATE_L2:
+                /* Resume usb host activity. */
+                if (m7400_usb_device) {
+                        usb_lock_device(m7400_usb_device);
+                        intf = usb_ifnum_to_if(m7400_usb_device, 0);
+                        usb_autopm_get_interface(intf);
+                        usb_autopm_put_interface(intf);
+                        usb_unlock_device(m7400_usb_device);
+                }
+                break;
+        default:
+                break;
+        }
+
+        return IRQ_HANDLED;
+}
+
+static int m7400_power(int code)
+{
+        switch (code) {
+        case PWRSTATE_L2L3:
+        m7400_l3_suspend();
+        break;
+        case PWRSTATE_L3L0:
+        m7400_l3_resume();
+        break;
+        default:
+        break;
+        }
+        return 0;
+}
+
+static void m7400_ehci_customize(struct platform_device *pdev)
+{
+        struct tegra_ehci_platform_data *ehci_pdata;
+        struct tegra_uhsic_config *hsic_config;
+
+        ehci_pdata = (struct tegra_ehci_platform_data *)
+                        pdev->dev.platform_data;
+        hsic_config = (struct tegra_uhsic_config *)
+                        ehci_pdata->phy_config;
+
+        /* Register PHY callbacks */
+        hsic_config->postsuspend = m7400_l2_suspend;
+        hsic_config->preresume = m7400_l2_resume;
+
+        /* Override required settings */
+        ehci_pdata->power_down_on_bus_suspend = 0;
+}
+
+static int m7400_attrib_write(struct device *dev, int value)
+{
+        struct tegra_bb_pdata *pdata;
+        static struct platform_device *ehci_device;
+        static bool first_enum = true;
+
+        if (value > 1 || (!ehci_registered && !value)) {
+                /* Supported values are 0/1. */
+                return -1;
+        }
+
+        pdata = (struct tegra_bb_pdata *) dev->platform_data;
+        if (value) {
+
+                /* Check readiness for enumeration */
+                if (first_enum)
+                        first_enum = false;
+                else
+                        m7400_enum_handshake();
+
+                /* Register ehci controller */
+                ehci_device = pdata->ehci_register();
+                if (ehci_device == NULL) {
+                        pr_info("%s - Error: ehci register failed.\n",
+                                                         __func__);
+                        return -1;
+                }
+
+                /* Customize PHY setup/callbacks */
+                m7400_ehci_customize(ehci_device);
+
+                ehci_registered = true;
+        } else {
+                /* Unregister ehci controller */
+                if (ehci_device != NULL)
+                        pdata->ehci_unregister(ehci_device);
+
+                /* Signal AP going down */
+                m7400_apdown_handshake();
+                ehci_registered = false;
+        }
+
+        return 0;
+}
+
+static int m7400_registered(struct usb_device *udev)
+{
+        m7400_usb_device = udev;
+        modem_status = BBSTATE_L0;
+        return 0;
+}
+
+static struct tegra_bb_gpio_irqdata m7400_gpioirqs[] = {
+        { GPIO_INVALID, "tegra_bb_wake", m7400_wake_irq,
+                                IRQF_TRIGGER_RISING, true, NULL },
+        { GPIO_INVALID, NULL, NULL, 0, NULL },  /* End of table */
+};
+
+static struct tegra_bb_power_gdata m7400_gdata = {
+        .gpio = m7400_gpios,
+        .gpioirq = m7400_gpioirqs,
+};
+
+static struct tegra_bb_power_mdata m7400_mdata = {
+        .vid = 0x04cc,
+        .pid = 0x230f,
+        .wake_capable = true,
+        .autosuspend_ready = true,
+        .reg_cb = m7400_registered,
+};
+
+static struct tegra_bb_power_data m7400_data = {
+        .gpio_data = &m7400_gdata,
+        .modem_data = &m7400_mdata,
+};
+
+static void *m7400_init(void *pdata)
+{
+        struct tegra_bb_pdata *platdata = (struct tegra_bb_pdata *) pdata;
+        union tegra_bb_gpio_id *id = platdata->id;
+
+        /* Fill the gpio ids allocated by hardware */
+        m7400_gpios[0].data.gpio = id->m7400.pwr_on;
+        m7400_gpios[1].data.gpio = id->m7400.pwr_status;
+        m7400_gpios[2].data.gpio = id->m7400.service;
+        m7400_gpios[3].data.gpio = id->m7400.usb_awr;
+        m7400_gpios[4].data.gpio = id->m7400.usb_cwr;
+        m7400_gpios[5].data.gpio = id->m7400.resout2;
+        m7400_gpios[6].data.gpio = id->m7400.uart_awr;
+        m7400_gpioirqs[0].id = id->m7400.usb_cwr;
+
+        if (!platdata->ehci_register || !platdata->ehci_unregister) {
+                pr_info("%s - Error: ehci reg/unreg functions missing.\n"
+                                                        , __func__);
+                return 0;
+        }
+
+        gpio_awr = m7400_gpios[3].data.gpio;
+        gpio_cwr = m7400_gpios[4].data.gpio;
+        gpio_arr = m7400_gpios[6].data.gpio;
+        if (gpio_awr == GPIO_INVALID || gpio_cwr == GPIO_INVALID
+                        || gpio_arr == GPIO_INVALID) {
+                pr_info("%s - Error: Invalid gpio data.\n", __func__);
+                return 0;
+        }
+
+        ehci_registered = false;
+        modem_status = BBSTATE_UNKNOWN;
+        return (void *) &m7400_data;
+}
+
+static void *m7400_deinit(void)
+{
+        return (void *) &m7400_data;
+}
+
+static struct tegra_bb_callback m7400_callbacks = {
+        .init = m7400_init,
+        .deinit = m7400_deinit,
+        .attrib = m7400_attrib_write,
+#ifdef CONFIG_PM
+        .power = m7400_power,
+#endif
+};
+
+void *m7400_get_cblist(void)
+{
+        return (void *) &m7400_callbacks;
+}
diff --git a/drivers/misc/tegra-baseband/bb-power.c b/drivers/misc/tegra-baseband/bb-power.c
new file mode 100644
index 00000000000..9210a8f3e84
--- /dev/null
+++ b/drivers/misc/tegra-baseband/bb-power.c
@@ -0,0 +1,337 @@
+/*
+ * drivers/misc/tegra-baseband/bb-power.c
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/usb.h>
+#include <linux/uaccess.h>
+#include <linux/platform_data/tegra_usb.h>
+#include <mach/usb_phy.h>
+#include <mach/tegra-bb-power.h>
+#include "bb-power.h"
+
+static struct tegra_bb_callback *callback;
+static int attr_load_val;
+static struct tegra_bb_power_mdata *mdata;
+static bb_get_cblist get_cblist[] = {
+        NULL,
+        NULL,
+        NULL,
+        M7400_CB,
+};
+
+static int tegra_bb_power_gpio_init(struct tegra_bb_power_gdata *gdata)
+{
+        int ret;
+        int irq;
+        unsigned gpio_id;
+        const char *gpio_label;
+        unsigned long gpio_flags;
+        struct tegra_bb_gpio_data *gpiolist;
+        struct tegra_bb_gpio_irqdata *gpioirq;
+
+        gpiolist = gdata->gpio;
+        for (; gpiolist->data.gpio != GPIO_INVALID; ++gpiolist) {
+                gpio_id = (gpiolist->data.gpio);
+                gpio_label = (gpiolist->data.label);
+                gpio_flags = (gpiolist->data.flags);
+
+                /* Request the gpio */
+                ret = gpio_request(gpio_id, gpio_label);
+                if (ret) {
+                        pr_err("%s: Error: gpio_request for gpio %d failed.\n",
+                                                         __func__, gpio_id);
+                        return ret;
+                }
+
+                /* Set gpio direction, as requested */
+                if (gpio_flags == GPIOF_IN)
+                        gpio_direction_input(gpio_id);
+                else
+                        gpio_direction_output(gpio_id, (!gpio_flags ? 0 : 1));
+
+                /* Enable the gpio */
+                tegra_gpio_enable(gpio_id);
+
+                /* Create a sysfs node, if requested */
+                if (gpiolist->doexport)
+                        gpio_export(gpio_id, false);
+        }
+
+        gpioirq = gdata->gpioirq;
+        for (; gpioirq->id != GPIO_INVALID; ++gpioirq) {
+
+                /* Create interrupt handler, if requested */
+                if (gpioirq->handler != NULL) {
+                        irq = gpio_to_irq(gpioirq->id);
+                        ret = request_threaded_irq(irq, NULL, gpioirq->handler,
+                                gpioirq->flags, gpioirq->name, gpioirq->cookie);
+                        if (ret < 0) {
+                                pr_err("%s: Error: threaded_irq req fail.\n"
+                                                                , __func__);
+                                return ret;
+                        }
+
+                        if (gpioirq->wake_capable) {
+                                ret = enable_irq_wake(irq);
+                                if (ret) {
+                                        pr_err("%s: Error: irqwake req fail.\n",
+                                                                __func__);
+                                        return ret;
+                                }
+                        }
+                }
+        }
+        return 0;
+}
+
+static int tegra_bb_power_gpio_deinit(struct tegra_bb_power_gdata *gdata)
+{
+        struct tegra_bb_gpio_data *gpiolist;
+        struct tegra_bb_gpio_irqdata *gpioirq;
+
+        gpiolist = gdata->gpio;
+        for (; gpiolist->data.gpio != GPIO_INVALID; ++gpiolist) {
+
+                /* Free the gpio */
+                gpio_free(gpiolist->data.gpio);
+        }
+
+        gpioirq = gdata->gpioirq;
+        for (; gpioirq->id != GPIO_INVALID; ++gpioirq) {
+
+                /* Free the irq */
+                free_irq(gpio_to_irq(gpioirq->id), gpioirq->cookie);
+        }
+        return 0;
+}
+
+static ssize_t tegra_bb_attr_write(struct device *dev,
+                        struct device_attribute *attr,
+                        const char *buf, size_t count)
+{
+        int val;
+
+        if (sscanf(buf, "%d", &val) != 1)
+                return -EINVAL;
+
+        if (callback && callback->attrib) {
+                if (!callback->attrib(dev, val))
+                        attr_load_val = val;
+        }
+        return count;
+}
+
+static ssize_t tegra_bb_attr_read(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        return sprintf(buf, "%d", attr_load_val);
+}
+
+static DEVICE_ATTR(load, S_IRUSR | S_IWUSR | S_IRGRP,
+                tegra_bb_attr_read, tegra_bb_attr_write);
+
+static void tegra_usbdevice_added(struct usb_device *udev)
+{
+        const struct usb_device_descriptor *desc = &udev->descriptor;
+
+        if (desc->idVendor == mdata->vid &&
+            desc->idProduct == mdata->pid) {
+                pr_debug("%s: Device %s added.\n", udev->product, __func__);
+
+                if (mdata->wake_capable)
+                        device_set_wakeup_enable(&udev->dev, true);
+                if (mdata->autosuspend_ready)
+                        usb_enable_autosuspend(udev);
+                if (mdata->reg_cb)
+                        mdata->reg_cb(udev);
+        }
+}
+
+static void tegra_usbdevice_removed(struct usb_device *udev)
+{
+        const struct usb_device_descriptor *desc = &udev->descriptor;
+
+        if (desc->idVendor == mdata->vid &&
+            desc->idProduct == mdata->pid) {
+                pr_debug("%s: Device %s removed.\n", udev->product, __func__);
+        }
+}
+
+static int tegra_usb_notify(struct notifier_block *self, unsigned long action,
+                      void *dev)
+{
+        switch (action) {
+        case USB_DEVICE_ADD:
+                tegra_usbdevice_added((struct usb_device *)dev);
+                break;
+        case USB_DEVICE_REMOVE:
+                tegra_usbdevice_removed((struct usb_device *)dev);
+                break;
+        }
+        return NOTIFY_OK;
+}
+
+static struct notifier_block tegra_usb_nb = {
+        .notifier_call = tegra_usb_notify,
+};
+
+static int tegra_bb_power_probe(struct platform_device *device)
+{
+        struct device *dev = &device->dev;
+        struct tegra_bb_pdata *pdata;
+        struct tegra_bb_power_data *data;
+        struct tegra_bb_power_gdata *gdata;
+        int err;
+        unsigned int bb_id;
+
+        pdata = (struct tegra_bb_pdata *) dev->platform_data;
+        if (!pdata) {
+                pr_err("%s - Error: platform data is empty.\n", __func__);
+                return -ENODEV;
+        }
+
+        /* Obtain BB specific callback list */
+        bb_id = pdata->bb_id;
+        if (get_cblist[bb_id] != NULL) {
+                callback = (struct tegra_bb_callback *) get_cblist[bb_id]();
+                if (callback && callback->init) {
+                        data = (struct tegra_bb_power_data *)
+                        callback->init((void *)pdata);
+
+                        gdata = data->gpio_data;
+                        if (!gdata) {
+                                pr_err("%s - Error: Gpio data is empty.\n",
+                                                                __func__);
+                                return -ENODEV;
+                        }
+
+                        /* Initialize gpio as required */
+                        tegra_bb_power_gpio_init(gdata);
+
+                        mdata = data->modem_data;
+                        if (mdata && mdata->vid && mdata->pid)
+                                /* Register to notifications from usb core */
+                                usb_register_notify(&tegra_usb_nb);
+                } else {
+                        pr_err("%s - Error: init callback is empty.\n",
+                                                                __func__);
+                        return -ENODEV;
+                }
+        } else {
+                pr_err("%s - Error: callback data is empty.\n", __func__);
+                return -ENODEV;
+        }
+
+        /* Create the control sysfs node */
+        err = device_create_file(dev, &dev_attr_load);
+        if (err < 0) {
+                pr_err("%s - Error: device_create_file failed.\n", __func__);
+                return -ENODEV;
+        }
+        attr_load_val = 0;
+
+        return 0;
+}
+
+static int tegra_bb_power_remove(struct platform_device *device)
+{
+        struct device *dev = &device->dev;
+        struct tegra_bb_power_data *data;
+        struct tegra_bb_power_gdata *gdata;
+
+        /* BB specific callback */
+        if (callback && callback->deinit) {
+                data = (struct tegra_bb_power_data *)
+                callback->deinit();
+
+                /* Deinitialize gpios */
+                gdata = data->gpio_data;
+                if (gdata)
+                        tegra_bb_power_gpio_deinit(gdata);
+                else {
+                        pr_err("%s - Error: Gpio data is empty.\n", __func__);
+                        return -ENODEV;
+                }
+
+                mdata = data->modem_data;
+                if (mdata && mdata->vid && mdata->pid)
+                        /* Register to notifications from usb core */
+                        usb_unregister_notify(&tegra_usb_nb);
+        }
+
+        /* Remove the control sysfs node */
+        device_remove_file(dev, &dev_attr_load);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_bb_power_suspend(struct platform_device *device,
+        pm_message_t state)
+{
+        /* BB specific callback */
+        if (callback && callback->power)
+                callback->power(PWRSTATE_L2L3);
+        return 0;
+}
+
+static int tegra_bb_power_resume(struct platform_device *device)
+{
+        /* BB specific callback */
+        if (callback && callback->power)
+                callback->power(PWRSTATE_L3L0);
+        return 0;
+}
+#endif
+
+static struct platform_driver tegra_bb_power_driver = {
+        .probe = tegra_bb_power_probe,
+        .remove = tegra_bb_power_remove,
+#ifdef CONFIG_PM
+        .suspend = tegra_bb_power_suspend,
+        .resume = tegra_bb_power_resume,
+#endif
+        .driver = {
+                .name = "tegra_baseband_power",
+        },
+};
+
+static int __init tegra_baseband_power_init(void)
+{
+        pr_debug("%s\n", __func__);
+        return platform_driver_register(&tegra_bb_power_driver);
+}
+
+static void __exit tegra_baseband_power_exit(void)
+{
+        pr_debug("%s\n", __func__);
+        platform_driver_unregister(&tegra_bb_power_driver);
+}
+
+module_init(tegra_baseband_power_init)
+module_exit(tegra_baseband_power_exit)
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_DESCRIPTION("Tegra modem power management driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/tegra-baseband/bb-power.h b/drivers/misc/tegra-baseband/bb-power.h
new file mode 100644
index 00000000000..cdd69380203
--- /dev/null
+++ b/drivers/misc/tegra-baseband/bb-power.h
@@ -0,0 +1,99 @@
+/*
+ * drivers/misc/tegra-baseband/bb-power.h
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+enum tegra_bb_state {
+        BBSTATE_UNKNOWN,
+        /* Baseband state L0 - Running */
+        BBSTATE_L0,
+        /* Baseband state L2 - Suspended */
+        BBSTATE_L2,
+        /* Baseband state L3 - Suspended and detached */
+        BBSTATE_L3,
+};
+
+enum tegra_bb_pwrstate {
+        /* System power state - Entering suspend */
+        PWRSTATE_L2L3,
+        /* System power state - Resuming from suspend */
+        PWRSTATE_L3L0,
+        PWRSTATE_INVALID,
+};
+
+struct tegra_bb_gpio_data {
+        /* Baseband gpio data */
+        struct gpio data;
+        /* Baseband gpio - Should it be exported to sysfs ? */
+        bool doexport;
+};
+
+struct tegra_bb_gpio_irqdata {
+        /* Baseband gpio IRQ - Id */
+        int id;
+        /* Baseband gpio IRQ - Friendly name */
+        const char *name;
+        /* Baseband gpio IRQ - IRQ handler */
+        irq_handler_t handler;
+        /* Baseband gpio IRQ - IRQ trigger flags */
+        int flags;
+        /* Baseband gpio IRQ - Can the gpio wake system from sleep ? */
+        bool wake_capable;
+        void *cookie;
+};
+
+typedef void* (*bb_get_cblist)(void);
+typedef void* (*bb_init_cb)(void *pdata);
+typedef void* (*bb_deinit_cb)(void);
+typedef int (*bb_power_cb)(int code);
+typedef int (*bb_attrib_cb)(struct device *dev, int value);
+typedef int (*modem_register_cb)(struct usb_device *udev);
+
+struct tegra_bb_power_gdata {
+        struct tegra_bb_gpio_data *gpio;
+        struct tegra_bb_gpio_irqdata *gpioirq;
+};
+
+struct tegra_bb_power_mdata {
+        /* Baseband USB vendor ID */
+        int vid;
+        /* Baseband USB product ID */
+        int pid;
+        /* Baseband capability - Can it generate a wakeup ? */
+        bool wake_capable;
+        /* Baseband capability - Can it be auto/runtime suspended ? */
+        bool autosuspend_ready;
+        /* Baseband callback after a successful registration */
+        modem_register_cb reg_cb;
+};
+
+struct tegra_bb_power_data {
+        struct tegra_bb_power_gdata *gpio_data;
+        struct tegra_bb_power_mdata *modem_data;
+};
+
+struct tegra_bb_callback {
+        bb_init_cb init;
+        bb_deinit_cb deinit;
+        bb_power_cb power;
+        bb_attrib_cb attrib;
+        bool valid;
+};
+
+#ifdef CONFIG_TEGRA_BB_M7400
+extern void *m7400_get_cblist(void);
+#define M7400_CB m7400_get_cblist
+#else
+#define M7400_CB NULL
+#endif
diff --git a/drivers/misc/tegra-cryptodev.c b/drivers/misc/tegra-cryptodev.c
new file mode 100644
index 00000000000..d5ed6a22dda
--- /dev/null
+++ b/drivers/misc/tegra-cryptodev.c
@@ -0,0 +1,349 @@
+/*
+ * drivers/misc/tegra-cryptodev.c
+ *
+ * crypto dev node for NVIDIA tegra aes hardware
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/crypto.h>
+#include <linux/scatterlist.h>
+#include <linux/uaccess.h>
+#include <crypto/rng.h>
+
+#include "tegra-cryptodev.h"
+
+#define NBUFS 2
+
+struct tegra_crypto_ctx {
+        struct crypto_ablkcipher *ecb_tfm;
+        struct crypto_ablkcipher *cbc_tfm;
+        struct crypto_rng *rng;
+        u8 seed[TEGRA_CRYPTO_RNG_SEED_SIZE];
+        int use_ssk;
+};
+
+struct tegra_crypto_completion {
+        struct completion restart;
+        int req_err;
+};
+
+static int alloc_bufs(unsigned long *buf[NBUFS])
+{
+        int i;
+
+        for (i = 0; i < NBUFS; i++) {
+                buf[i] = (void *)__get_free_page(GFP_KERNEL);
+                if (!buf[i])
+                        goto err_free_buf;
+        }
+
+        return 0;
+
+err_free_buf:
+        while (i-- > 0)
+                free_page((unsigned long)buf[i]);
+
+        return -ENOMEM;
+}
+
+static void free_bufs(unsigned long *buf[NBUFS])
+{
+        int i;
+
+        for (i = 0; i < NBUFS; i++)
+                free_page((unsigned long)buf[i]);
+}
+
+static int tegra_crypto_dev_open(struct inode *inode, struct file *filp)
+{
+        struct tegra_crypto_ctx *ctx;
+        int ret = 0;
+
+        ctx = kzalloc(sizeof(struct tegra_crypto_ctx), GFP_KERNEL);
+        if (!ctx) {
+                pr_err("no memory for context\n");
+                return -ENOMEM;
+        }
+
+        ctx->ecb_tfm = crypto_alloc_ablkcipher("ecb-aes-tegra",
+                CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC, 0);
+        if (IS_ERR(ctx->ecb_tfm)) {
+                pr_err("Failed to load transform for ecb-aes-tegra: %ld\n",
+                        PTR_ERR(ctx->ecb_tfm));
+                ret = PTR_ERR(ctx->ecb_tfm);
+                goto fail_ecb;
+        }
+
+        ctx->cbc_tfm = crypto_alloc_ablkcipher("cbc-aes-tegra",
+                CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC, 0);
+        if (IS_ERR(ctx->cbc_tfm)) {
+                pr_err("Failed to load transform for cbc-aes-tegra: %ld\n",
+                        PTR_ERR(ctx->cbc_tfm));
+                ret = PTR_ERR(ctx->cbc_tfm);
+                goto fail_cbc;
+        }
+
+        ctx->rng = crypto_alloc_rng("rng-aes-tegra", CRYPTO_ALG_TYPE_RNG, 0);
+        if (IS_ERR(ctx->rng)) {
+                pr_err("Failed to load transform for tegra rng: %ld\n",
+                        PTR_ERR(ctx->rng));
+                ret = PTR_ERR(ctx->rng);
+                goto fail_rng;
+        }
+
+        filp->private_data = ctx;
+        return ret;
+
+fail_rng:
+        crypto_free_ablkcipher(ctx->cbc_tfm);
+
+fail_cbc:
+        crypto_free_ablkcipher(ctx->ecb_tfm);
+
+fail_ecb:
+        kfree(ctx);
+        return ret;
+}
+
+static int tegra_crypto_dev_release(struct inode *inode, struct file *filp)
+{
+        struct tegra_crypto_ctx *ctx = filp->private_data;
+
+        crypto_free_ablkcipher(ctx->ecb_tfm);
+        crypto_free_ablkcipher(ctx->cbc_tfm);
+        crypto_free_rng(ctx->rng);
+        kfree(ctx);
+        filp->private_data = NULL;
+        return 0;
+}
+
+static void tegra_crypt_complete(struct crypto_async_request *req, int err)
+{
+        struct tegra_crypto_completion *done = req->data;
+
+        if (err != -EINPROGRESS) {
+                done->req_err = err;
+                complete(&done->restart);
+        }
+}
+
+static int process_crypt_req(struct tegra_crypto_ctx *ctx, struct tegra_crypt_req *crypt_req)
+{
+        struct crypto_ablkcipher *tfm;
+        struct ablkcipher_request *req = NULL;
+        struct scatterlist in_sg;
+        struct scatterlist out_sg;
+        unsigned long *xbuf[NBUFS];
+        int ret = 0, size = 0;
+        unsigned long total = 0;
+        struct tegra_crypto_completion tcrypt_complete;
+        const u8 *key = NULL;
+
+        if (crypt_req->op & TEGRA_CRYPTO_ECB) {
+                req = ablkcipher_request_alloc(ctx->ecb_tfm, GFP_KERNEL);
+                tfm = ctx->ecb_tfm;
+        } else {
+                req = ablkcipher_request_alloc(ctx->cbc_tfm, GFP_KERNEL);
+                tfm = ctx->cbc_tfm;
+        }
+        if (!req) {
+                pr_err("%s: Failed to allocate request\n", __func__);
+                return -ENOMEM;
+        }
+
+        if ((crypt_req->keylen < 0) || (crypt_req->keylen > AES_MAX_KEY_SIZE))
+                return -EINVAL;
+
+        crypto_ablkcipher_clear_flags(tfm, ~0);
+
+        if (!ctx->use_ssk)
+                key = crypt_req->key;
+
+        ret = crypto_ablkcipher_setkey(tfm, key, crypt_req->keylen);
+        if (ret < 0) {
+                pr_err("setkey failed");
+                goto process_req_out;
+        }
+
+        ret = alloc_bufs(xbuf);
+        if (ret < 0) {
+                pr_err("alloc_bufs failed");
+                goto process_req_out;
+        }
+
+        init_completion(&tcrypt_complete.restart);
+
+        ablkcipher_request_set_callback(req, CRYPTO_TFM_REQ_MAY_BACKLOG,
+                tegra_crypt_complete, &tcrypt_complete);
+
+        total = crypt_req->plaintext_sz;
+        while (total > 0) {
+                size = min(total, PAGE_SIZE);
+                ret = copy_from_user((void *)xbuf[0],
+                        (void __user *)crypt_req->plaintext, size);
+                if (ret < 0) {
+                        pr_debug("%s: copy_from_user failed (%d)\n", __func__, ret);
+                        goto process_req_buf_out;
+                }
+                sg_init_one(&in_sg, xbuf[0], size);
+                sg_init_one(&out_sg, xbuf[1], size);
+
+                ablkcipher_request_set_crypt(req, &in_sg,
+                        &out_sg, size, crypt_req->iv);
+
+                INIT_COMPLETION(tcrypt_complete.restart);
+                tcrypt_complete.req_err = 0;
+                ret = crypt_req->encrypt ?
+                        crypto_ablkcipher_encrypt(req) :
+                        crypto_ablkcipher_decrypt(req);
+
+                if ((ret == -EINPROGRESS) || (ret == -EBUSY)) {
+                        /* crypto driver is asynchronous */
+                        ret = wait_for_completion_interruptible(&tcrypt_complete.restart);
+
+                        if (ret < 0)
+                                goto process_req_buf_out;
+
+                        if (tcrypt_complete.req_err < 0) {
+                                ret = tcrypt_complete.req_err;
+                                goto process_req_buf_out;
+                        }
+                } else if (ret < 0) {
+                        pr_debug("%scrypt failed (%d)\n",
+                                crypt_req->encrypt ? "en" : "de", ret);
+                        goto process_req_buf_out;
+                }
+
+                ret = copy_to_user((void __user *)crypt_req->result,
+                        (const void *)xbuf[1], size);
+                if (ret < 0)
+                        goto process_req_buf_out;
+
+                total -= size;
+                crypt_req->result += size;
+                crypt_req->plaintext += size;
+        }
+
+process_req_buf_out:
+        free_bufs(xbuf);
+process_req_out:
+        ablkcipher_request_free(req);
+
+        return ret;
+}
+
+static long tegra_crypto_dev_ioctl(struct file *filp,
+        unsigned int ioctl_num, unsigned long arg)
+{
+        struct tegra_crypto_ctx *ctx = filp->private_data;
+        struct tegra_crypt_req crypt_req;
+        struct tegra_rng_req rng_req;
+        char *rng;
+        int ret = 0;
+
+        switch (ioctl_num) {
+        case TEGRA_CRYPTO_IOCTL_NEED_SSK:
+                ctx->use_ssk = (int)arg;
+                break;
+
+        case TEGRA_CRYPTO_IOCTL_PROCESS_REQ:
+                ret = copy_from_user(&crypt_req, (void __user *)arg, sizeof(crypt_req));
+                if (ret < 0) {
+                        pr_err("%s: copy_from_user fail(%d)\n", __func__, ret);
+                        break;
+                }
+
+                ret = process_crypt_req(ctx, &crypt_req);
+                break;
+
+        case TEGRA_CRYPTO_IOCTL_SET_SEED:
+                if (copy_from_user(&rng_req, (void __user *)arg, sizeof(rng_req)))
+                        return -EFAULT;
+
+                memcpy(ctx->seed, rng_req.seed, TEGRA_CRYPTO_RNG_SEED_SIZE);
+
+                ret = crypto_rng_reset(ctx->rng, ctx->seed,
+                        crypto_rng_seedsize(ctx->rng));
+                break;
+
+        case TEGRA_CRYPTO_IOCTL_GET_RANDOM:
+                if (copy_from_user(&rng_req, (void __user *)arg, sizeof(rng_req)))
+                        return -EFAULT;
+
+                rng = kzalloc(rng_req.nbytes, GFP_KERNEL);
+                if (!rng) {
+                        pr_err("mem alloc for rng fail");
+                        ret = -ENODATA;
+                        goto rng_out;
+                }
+
+                ret = crypto_rng_get_bytes(ctx->rng, rng,
+                        rng_req.nbytes);
+
+                if (ret != rng_req.nbytes) {
+                        pr_err("rng failed");
+                        ret = -ENODATA;
+                        goto rng_out;
+                }
+
+                ret = copy_to_user((void __user *)rng_req.rdata,
+                        (const void *)rng, rng_req.nbytes);
+                ret = (ret < 0) ? -ENODATA : 0;
+rng_out:
+                if (rng)
+                        kfree(rng);
+                break;
+
+        default:
+                pr_debug("invalid ioctl code(%d)", ioctl_num);
+                ret = -EINVAL;
+        }
+
+        return ret;
+}
+
+struct file_operations tegra_crypto_fops = {
+        .owner = THIS_MODULE,
+        .open = tegra_crypto_dev_open,
+        .release = tegra_crypto_dev_release,
+        .unlocked_ioctl = tegra_crypto_dev_ioctl,
+};
+
+struct miscdevice tegra_crypto_device = {
+        .minor = MISC_DYNAMIC_MINOR,
+        .name = "tegra-crypto",
+        .fops = &tegra_crypto_fops,
+};
+
+static int __init tegra_crypto_dev_init(void)
+{
+        return misc_register(&tegra_crypto_device);
+}
+
+late_initcall(tegra_crypto_dev_init);
+
+MODULE_DESCRIPTION("Tegra AES hw device node.");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPLv2");
diff --git a/drivers/misc/tegra-cryptodev.h b/drivers/misc/tegra-cryptodev.h
new file mode 100644
index 00000000000..ed62a52eca0
--- /dev/null
+++ b/drivers/misc/tegra-cryptodev.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __TEGRA_CRYPTODEV_H
+#define __TEGRA_CRYPTODEV_H
+
+#include <crypto/aes.h>
+
+#include <asm-generic/ioctl.h>
+
+/* ioctl arg = 1 if you want to use ssk. arg = 0 to use normal key */
+#define TEGRA_CRYPTO_IOCTL_NEED_SSK     _IOWR(0x98, 100, int)
+#define TEGRA_CRYPTO_IOCTL_PROCESS_REQ  _IOWR(0x98, 101, int*)
+#define TEGRA_CRYPTO_IOCTL_SET_SEED     _IOWR(0x98, 102, int*)
+#define TEGRA_CRYPTO_IOCTL_GET_RANDOM   _IOWR(0x98, 103, int*)
+
+#define TEGRA_CRYPTO_MAX_KEY_SIZE       AES_MAX_KEY_SIZE
+#define TEGRA_CRYPTO_IV_SIZE    AES_BLOCK_SIZE
+#define DEFAULT_RNG_BLK_SZ      16
+
+/* the seed consists of 16 bytes of key + 16 bytes of init vector */
+#define TEGRA_CRYPTO_RNG_SEED_SIZE      AES_KEYSIZE_128 + DEFAULT_RNG_BLK_SZ
+#define TEGRA_CRYPTO_RNG_SIZE   SZ_16
+
+/* encrypt/decrypt operations */
+#define TEGRA_CRYPTO_ECB        BIT(0)
+#define TEGRA_CRYPTO_CBC        BIT(1)
+#define TEGRA_CRYPTO_RNG        BIT(2)
+
+/* a pointer to this struct needs to be passed to:
+ * TEGRA_CRYPTO_IOCTL_PROCESS_REQ
+ */
+struct tegra_crypt_req {
+        int op; /* e.g. TEGRA_CRYPTO_ECB */
+        bool encrypt;
+        char key[TEGRA_CRYPTO_MAX_KEY_SIZE];
+        int keylen;
+        char iv[TEGRA_CRYPTO_IV_SIZE];
+        int ivlen;
+        u8 *plaintext;
+        int plaintext_sz;
+        u8 *result;
+};
+
+/* pointer to this struct should be passed to:
+ * TEGRA_CRYPTO_IOCTL_SET_SEED
+ * TEGRA_CRYPTO_IOCTL_GET_RANDOM
+ */
+struct tegra_rng_req {
+        u8 seed[TEGRA_CRYPTO_RNG_SEED_SIZE];
+        u8 *rdata; /* random generated data */
+        int nbytes; /* random data length */
+};
+
+#endif
diff --git a/drivers/misc/ti-st/gps_drv.c b/drivers/misc/ti-st/gps_drv.c
new file mode 100644
index 00000000000..17ca92dd177
--- /dev/null
+++ b/drivers/misc/ti-st/gps_drv.c
@@ -0,0 +1,804 @@
+/*
+ *   GPS Char Driver for Texas Instrument's Connectivity Chip.
+ *   Copyright (C) 2009 Texas Instruments
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License version 2 as
+ *   published by the Free Software Foundation.
+ *
+ *   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, write to the Free Software
+ *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+
+#include <linux/uaccess.h>
+#include <linux/tty.h>
+#include <linux/sched.h>
+
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/skbuff.h>
+#include <linux/interrupt.h>
+
+#include <linux/ti_wilink_st.h>
+
+#undef VERBOSE
+#undef DEBUG
+
+/* Debug macros*/
+#if defined(DEBUG)              /* limited debug messages */
+#define GPSDRV_DBG(fmt, arg...)  \
+        printk(KERN_INFO "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#define GPSDRV_VER(fmt, arg...)
+#elif defined(VERBOSE)          /* very verbose */
+#define GPSDRV_DBG(fmt, arg...)  \
+        printk(KERN_INFO "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#define GPSDRV_VER(fmt, arg...)  \
+        printk(KERN_INFO "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#define GPSDRV_ERR(fmt, arg...)  \
+        printk(KERN_ERR "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#else /* Error msgs only */
+#define GPSDRV_ERR(fmt, arg...)  \
+        printk(KERN_ERR "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#define GPSDRV_VER(fmt, arg...)
+#define GPSDRV_DBG(fmt, arg...)
+#endif
+
+static void gpsdrv_tsklet_write(unsigned long data);
+
+/* List of error codes returned by the gps driver*/
+enum {
+        GPS_ERR_FAILURE = -1,   /* check struct */
+        GPS_SUCCESS,
+        GPS_ERR_CLASS = -15,
+        GPS_ERR_CPY_TO_USR,
+        GPS_ERR_CPY_FRM_USR,
+        GPS_ERR_UNKNOWN,
+};
+
+/* Channel-9 details for GPS */
+#define GPS_CH9_PKT_HDR_SIZE            4
+#define GPS_CH9_PKT_NUMBER              0x9
+#define GPS_CH9_OP_WRITE                0x1
+#define GPS_CH9_OP_READ                 0x2
+#define GPS_CH9_OP_COMPLETED_EVT        0x3
+
+/* Macros for Syncronising GPS registration and other R/W/ICTL operations */
+#define GPS_ST_REGISTERED       0
+#define GPS_ST_RUNNING          1
+
+/* Read time out defined to 10 seconds */
+#define GPSDRV_READ_TIMEOUT     10000
+/* Reg time out defined to 6 seconds */
+#define GPSDRV_REG_TIMEOUT      6000
+
+
+struct gpsdrv_event_hdr {
+        uint8_t opcode;
+        uint16_t plen;
+} __packed;
+
+/*
+ * struct gpsdrv_data - gps internal driver data
+ * @gpsdrv_reg_completed - completion to wait for registration
+ * @streg_cbdata - registration feedback
+ * @state - driver state
+ * @tx_count - TX throttling/unthrottling
+ * @st_write - write ptr from ST
+ * @rx_list - Rx data SKB queue
+ * @tx_list - Tx data SKB queue
+ * @gpsdrv_data_q - dataq checked up on poll/receive
+ * @lock - spin lock
+ * @gpsdrv_tx_tsklet - gps write task
+ */
+
+struct gpsdrv_data {
+        struct completion gpsdrv_reg_completed;
+        char streg_cbdata;
+        unsigned long state;
+        unsigned char tx_count;
+        long (*st_write) (struct sk_buff *skb);
+        struct sk_buff_head rx_list;
+        struct sk_buff_head tx_list;
+        wait_queue_head_t gpsdrv_data_q;
+        spinlock_t lock;
+        struct tasklet_struct gpsdrv_tx_tsklet;
+};
+
+#define DEVICE_NAME     "tigps"
+
+/***********Functions called from ST driver**********************************/
+
+/*  gpsdrv_st_recv Function
+ *  This is Called in from -- ST Core when a data is received
+ *  This is a registered callback with ST core when the gps driver registers
+ *  with ST.
+ *
+ *  Parameters:
+ *  @skb    : SKB buffer pointer which contains the incoming Ch-9 GPS data.
+ *  Returns:
+ *          GPS_SUCCESS - On Success
+ *          else suitable error code
+ */
+long gpsdrv_st_recv(void *arg, struct sk_buff *skb)
+{
+        struct gpsdrv_event_hdr gpsdrv_hdr = { 0x00, 0x0000 };
+        struct gpsdrv_data *hgps = (struct gpsdrv_data *)arg;
+
+        /* SKB is NULL */
+        if (NULL == skb) {
+                GPSDRV_ERR("Input SKB is NULL");
+                return GPS_ERR_FAILURE;
+        }
+
+        /* Sanity Check - To Check if the Rx Pkt is Channel -9 or not */
+        if (0x09 != skb->cb[0]) {
+                GPSDRV_ERR("Input SKB is not a Channel-9 packet");
+                return GPS_ERR_FAILURE;
+        }
+        /* Copy Ch-9 info to local structure */
+        memcpy(&gpsdrv_hdr, skb->data, GPS_CH9_PKT_HDR_SIZE - 1);
+        skb_pull(skb, GPS_CH9_PKT_HDR_SIZE - 1);
+
+        /* check if skb->len and gpsdrv_hdr.plen are equal */
+        if (skb->len != gpsdrv_hdr.plen) {
+                GPSDRV_ERR("Received corrupted packet - Length Mismatch");
+                return -EINVAL;
+        }
+#ifdef VERBOSE
+        printk(KERN_INFO"data start >>\n");
+        print_hex_dump(KERN_INFO, ">in>", DUMP_PREFIX_NONE,
+                        16, 1, skb->data, skb->len, 0);
+        printk(KERN_INFO"\n<< end\n");
+#endif
+        /* Check the Opcode */
+        if ((gpsdrv_hdr.opcode != GPS_CH9_OP_READ) && (gpsdrv_hdr.opcode != \
+                                GPS_CH9_OP_COMPLETED_EVT)) {
+                GPSDRV_ERR("Rec corrupt pkt opcode %x", gpsdrv_hdr.opcode);
+                return -EINVAL;
+        }
+
+        /* Strip Channel 9 packet information from SKB only
+         * if the opcode is GPS_CH9_OP_READ and get AI2 packet
+         */
+        if (GPS_CH9_OP_READ == gpsdrv_hdr.opcode) {
+                skb_queue_tail(&hgps->rx_list, skb);
+                wake_up_interruptible(&hgps->gpsdrv_data_q);
+        } else {
+                spin_lock(&hgps->lock);
+                /* The no. of Completed Packets is always 1
+                 * in case of Channel 9 as per spec.
+                 * Forcing it to 1 for precaution.
+                 */
+                hgps->tx_count = 1;
+                /* Check if Tx queue and Tx count not empty */
+                if (!skb_queue_empty(&hgps->tx_list)) {
+                        /* Schedule the Tx-task let */
+                        spin_unlock(&hgps->lock);
+                        GPSDRV_VER(" Scheduling tasklet to write");
+                        tasklet_schedule(&hgps->gpsdrv_tx_tsklet);
+                } else {
+                        spin_unlock(&hgps->lock);
+                }
+
+                GPSDRV_VER(" Tx count = %x", hgps->tx_count);
+                /* Free the received command complete SKB */
+                kfree_skb(skb);
+        }
+
+        return GPS_SUCCESS;
+
+}
+
+/*  gpsdrv_st_cb Function
+ *  This is Called in from -- ST Core when the state is pending during
+ *  st_register. This is a registered callback with ST core when the gps
+ *  driver registers with ST.
+ *
+ *  Parameters:
+ *  @data   Status update of GPS registration
+ *  Returns: NULL
+ */
+void gpsdrv_st_cb(void *arg, char data)
+{
+        struct gpsdrv_data *hgps = (struct gpsdrv_data *)arg;
+
+        GPSDRV_DBG(" Inside %s", __func__);
+        hgps->streg_cbdata = data;      /* ST registration callback  status */
+        complete_all(&hgps->gpsdrv_reg_completed);
+        return;
+}
+
+static struct st_proto_s gpsdrv_proto = {
+        .chnl_id = 0x09,
+        .max_frame_size = 1024,
+        .hdr_len = 3,
+        .offset_len_in_hdr = 1,
+        .len_size = 2,
+        .reserve = 1,
+        .recv = gpsdrv_st_recv,
+        .reg_complete_cb = gpsdrv_st_cb,
+};
+
+/** gpsdrv_tsklet_write Function
+ *  This tasklet function will be scheduled when there is a data in Tx queue
+ *  and GPS chip sent an command completion packet with non zero value.
+ *
+ *  Parameters :
+ *  @data  : data passed to tasklet function
+ *  Returns : NULL
+ */
+void gpsdrv_tsklet_write(unsigned long data)
+{
+        struct sk_buff *skb = NULL;
+        struct gpsdrv_data *hgps = (struct gpsdrv_data *)data;
+
+        GPSDRV_DBG(" Inside %s", __func__);
+
+        spin_lock(&hgps->lock);
+
+        /* Perform sanity check of verifying the status
+           to perform an st_write */
+        if (((!hgps->st_write) || (0 == hgps->tx_count))
+                        || ((skb_queue_empty(&hgps->tx_list)))) {
+                spin_unlock(&hgps->lock);
+                GPSDRV_ERR("Sanity check Failed exiting %s", __func__);
+                return;
+        }
+        /* hgps->tx_list not empty skb already present
+         * dequeue the tx-data and perform a st_write
+         */
+        hgps->tx_count--;
+        spin_unlock(&hgps->lock);
+        GPSDRV_VER(" Tx count in gpsdrv_tsklet_write = %x", hgps->tx_count);
+        skb = skb_dequeue(&hgps->tx_list);
+        hgps->st_write(skb);
+
+        return;
+}
+
+/*********Functions Called from GPS host***************************************/
+
+/** gpsdrv_open Function
+ *  This function will perform an register on ST driver.
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @inod  :
+ *  Returns  GPS_SUCCESS -  on success
+ *           else suitable error code
+ */
+int gpsdrv_open(struct inode *inod, struct file *file)
+{
+        int ret = 0;
+        unsigned long timeout = GPSDRV_REG_TIMEOUT;
+        struct gpsdrv_data *hgps;
+
+        GPSDRV_DBG(" Inside %s", __func__);
+
+        /* Allocate local resource memory */
+        hgps = kzalloc(sizeof(struct gpsdrv_data), GFP_KERNEL);
+        if (!(hgps)) {
+                GPSDRV_ERR("Can't allocate GPS data structure");
+                return -ENOMEM;
+        }
+
+        /* Initialize wait queue, skb queue head and
+         * registration complete strucuture
+         */
+        skb_queue_head_init(&hgps->rx_list);
+        skb_queue_head_init(&hgps->tx_list);
+        init_completion(&hgps->gpsdrv_reg_completed);
+        init_waitqueue_head(&hgps->gpsdrv_data_q);
+        spin_lock_init(&hgps->lock);
+
+        /* Check if GPS is already registered with ST */
+        if (test_and_set_bit(GPS_ST_REGISTERED, &hgps->state)) {
+                GPSDRV_ERR("GPS Registered/Registration in progress with ST"
+                                " ,open called again?");
+                kfree(hgps);
+                return -EAGAIN;
+        }
+
+        /* Initialize  gpsdrv_reg_completed so as to wait for completion
+         * on the same
+         * if st_register returns with a PENDING status
+         */
+        INIT_COMPLETION(hgps->gpsdrv_reg_completed);
+
+        gpsdrv_proto.priv_data = hgps;
+        /* Resgister GPS with ST */
+        ret = st_register(&gpsdrv_proto);
+        GPSDRV_VER(" st_register returned %d", ret);
+
+        /* If GPS Registration returned with error, then clear GPS_ST_REGISTERED
+         * for future open calls and return the appropriate error code
+         */
+        if (ret < 0 && ret != -EINPROGRESS) {
+                GPSDRV_ERR(" st_register failed");
+                clear_bit(GPS_ST_REGISTERED, &hgps->state);
+                if (ret == -EINPROGRESS)
+                        return -EAGAIN;
+                return GPS_ERR_FAILURE;
+        }
+
+        /* if returned status is pending, wait for the completion */
+        if (ret == -EINPROGRESS) {
+                GPSDRV_VER(" GPS Register waiting for completion ");
+                timeout = wait_for_completion_timeout \
+                (&hgps->gpsdrv_reg_completed, msecs_to_jiffies(timeout));
+                /* Check for timed out condition */
+                if (0 == timeout) {
+                        GPSDRV_ERR("GPS Device registration timed out");
+                        clear_bit(GPS_ST_REGISTERED, &hgps->state);
+                        return -ETIMEDOUT;
+                } else if (0 > hgps->streg_cbdata) {
+                        GPSDRV_ERR("GPS Device Registration Failed-ST\n");
+                        GPSDRV_ERR("RegCB called with ");
+                        GPSDRV_ERR("Invalid value %d\n", hgps->streg_cbdata);
+                        clear_bit(GPS_ST_REGISTERED, &hgps->state);
+                        return -EAGAIN;
+                }
+        }
+        GPSDRV_DBG(" gps registration complete ");
+
+        /* Assign the write callback pointer */
+        hgps->st_write = gpsdrv_proto.write;
+        hgps->tx_count = 1;
+        file->private_data = hgps;      /* set drv data */
+        tasklet_init(&hgps->gpsdrv_tx_tsklet, (void *)gpsdrv_tsklet_write,
+                        (unsigned long)hgps);
+        set_bit(GPS_ST_RUNNING, &hgps->state);
+
+        return GPS_SUCCESS;
+}
+
+/** gpsdrv_release Function
+ *  This function will un-registers from the ST driver.
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @inod  :
+ *  Returns  GPS_SUCCESS -  on success
+ *           else suitable error code
+ */
+int gpsdrv_release(struct inode *inod, struct file *file)
+{
+        struct gpsdrv_data *hgps = file->private_data;
+
+        GPSDRV_DBG(" Inside %s", __func__);
+
+        /* Disabling task-let 1st & then un-reg to avoid
+         * tasklet getting scheduled
+         */
+        tasklet_disable(&hgps->gpsdrv_tx_tsklet);
+        tasklet_kill(&hgps->gpsdrv_tx_tsklet);
+        /* Cleat registered bit if already registered */
+        if (test_and_clear_bit(GPS_ST_REGISTERED, &hgps->state)) {
+                if (st_unregister(&gpsdrv_proto) < 0) {
+                        GPSDRV_ERR(" st_unregister failed");
+                        /* Re-Enable the task-let if un-register fails */
+                        tasklet_enable(&hgps->gpsdrv_tx_tsklet);
+                        return GPS_ERR_FAILURE;
+                }
+        }
+
+        /* Reset Tx count value and st_write function pointer */
+        hgps->tx_count = 0;
+        hgps->st_write = NULL;
+        clear_bit(GPS_ST_RUNNING, &hgps->state);
+        GPSDRV_VER(" st_unregister success");
+
+        skb_queue_purge(&hgps->rx_list);
+        skb_queue_purge(&hgps->tx_list);
+        kfree(hgps);
+        file->private_data = NULL;
+
+        return GPS_SUCCESS;
+}
+
+/** gpsdrv_read Function
+ *  This function will wait till the data received from the ST driver
+ *  and then strips the GPS-Channel-9 header from the
+ *  incoming AI2 packet and then send it to GPS host application.
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @data  : Data which needs to be passed to APP
+ *  @size  : Length of the data passesd
+ *  offset :
+ *  Returns  Size of AI2 packet received -  on success
+ *           else suitable error code
+ */
+ssize_t gpsdrv_read(struct file *file, char __user *data, size_t size,
+                loff_t *offset)
+{
+        int len = 0, error = 0;
+        struct sk_buff *skb = NULL;
+        unsigned long timeout = GPSDRV_READ_TIMEOUT;
+        struct gpsdrv_data *hgps;
+
+        GPSDRV_DBG(" Inside %s", __func__);
+
+        /* Validate input parameters */
+        if ((NULL == file) || (((NULL == data) || (0 == size)))) {
+                GPSDRV_ERR("Invalid input params passed to %s", __func__);
+                return -EINVAL;
+        }
+
+        hgps = file->private_data;
+        /* Check if GPS is registered to perform read operation */
+        if (!test_bit(GPS_ST_RUNNING, &hgps->state)) {
+                GPSDRV_ERR("GPS Device is not running");
+                return -EINVAL;
+        }
+
+        /* cannot come here if poll-ed before reading
+         * if not poll-ed wait on the same wait_q
+         */
+        timeout = wait_event_interruptible_timeout(hgps->gpsdrv_data_q,
+                !skb_queue_empty(&hgps->rx_list), msecs_to_jiffies(timeout));
+        /* Check for timed out condition */
+        if (0 == timeout) {
+                GPSDRV_ERR("GPS Device Read timed out");
+                return -ETIMEDOUT;
+        }
+
+
+        /* hgps->rx_list not empty skb already present */
+        skb = skb_dequeue(&hgps->rx_list);
+
+        if (!skb) {
+                GPSDRV_ERR("Dequed SKB is NULL?");
+                return GPS_ERR_UNKNOWN;
+        } else if (skb->len > size) {
+                GPSDRV_DBG("SKB length is Greater than requested size ");
+                GPSDRV_DBG("Returning the available length of SKB\n");
+
+                error = copy_to_user(data, skb->data, size);
+                skb_pull(skb, size);
+
+                if (skb->len != 0)
+                        skb_queue_head(&hgps->rx_list, skb);
+
+                /* printk(KERN_DEBUG  "gpsdrv: total size read= %d", size);*/
+                return size;
+        }
+
+#ifdef VERBOSE
+        print_hex_dump(KERN_INFO, ">in>", DUMP_PREFIX_NONE,
+                        16, 1, skb->data, skb->len, 0);
+#endif
+
+        /* Forward the data to the user */
+        if (skb->len <= size) {
+                if (copy_to_user(data, skb->data, skb->len)) {
+                        GPSDRV_ERR(" Unable to copy to user space");
+                        /* Queue the skb back to head */
+                        skb_queue_head(&hgps->rx_list, skb);
+                        return GPS_ERR_CPY_TO_USR;
+                }
+        }
+
+        len = skb->len;
+        kfree_skb(skb);
+        /* printk(KERN_DEBUG  "gpsdrv: total size read= %d", len); */
+        return len;
+}
+/* gpsdrv_write Function
+ *  This function will pre-pend the GPS-Channel-9 header to the
+ *  incoming AI2 packet sent from the GPS host application.
+ *
+ *  Parameters :
+ *  @file   : File pointer for GPS char driver
+ *  @data   : AI2 packet data from GPS application
+ *  @size   : Size of the AI2 packet data
+ *  @offset :
+ *  Returns  Size of AI2 packet on success
+ *           else suitable error code
+ */
+ssize_t gpsdrv_write(struct file *file, const char __user *data,
+                size_t size, loff_t *offset)
+{
+        unsigned char channel = GPS_CH9_PKT_NUMBER; /* GPS Channel number */
+        /* Initialize gpsdrv_event_hdr with write opcode */
+        struct gpsdrv_event_hdr gpsdrv_hdr = { GPS_CH9_OP_WRITE, 0x0000 };
+        struct sk_buff *skb = NULL;
+        struct gpsdrv_data *hgps;
+
+        GPSDRV_DBG(" Inside %s", __func__);
+        /* Validate input parameters */
+        if ((NULL == file) || (((NULL == data) || (0 == size)))) {
+                GPSDRV_ERR("Invalid input params passed to %s", __func__);
+                return -EINVAL;
+        }
+
+        hgps = file->private_data;
+
+        /* Check if GPS is registered to perform write operation */
+        if (!test_bit(GPS_ST_RUNNING, &hgps->state)) {
+                GPSDRV_ERR("GPS Device is not running");
+                return -EINVAL;
+        }
+
+        if (!hgps->st_write) {
+                GPSDRV_ERR(" Can't write to ST, hgps->st_write null ?");
+                return -EINVAL;
+        }
+
+
+        skb = alloc_skb(size + GPS_CH9_PKT_HDR_SIZE, GFP_ATOMIC);
+        /* Validate Created SKB */
+        if (NULL == skb) {
+                GPSDRV_ERR("Error aaloacting SKB");
+                return -ENOMEM;
+        }
+
+        /* Update chnl-9 information with plen=AI2 pckt size which is "size"*/
+        gpsdrv_hdr.plen = size;
+
+        /* PrePend Channel-9 header to AI2 packet and write to SKB */
+        memcpy(skb_put(skb, 1), &channel, 1);
+        memcpy(skb_put(skb, GPS_CH9_PKT_HDR_SIZE - 1), &gpsdrv_hdr,
+                        GPS_CH9_PKT_HDR_SIZE - 1);
+
+        /* Forward the data from the user space to ST core */
+        if (copy_from_user(skb_put(skb, size), data, size)) {
+                GPSDRV_ERR(" Unable to copy from user space");
+                kfree_skb(skb);
+                return GPS_ERR_CPY_FRM_USR;
+        }
+
+#ifdef VERBOSE
+        GPSDRV_VER("start data..");
+        print_hex_dump(KERN_INFO, "<out<", DUMP_PREFIX_NONE,
+                        16, 1, skb->data, size, 0);
+        GPSDRV_VER("\n..end data");
+#endif
+
+        /* Check if data can be sent to GPS chip
+         * If not, add it to queue and that can be sent later
+         */
+        spin_lock(&hgps->lock);
+        if (0 != hgps->tx_count) {
+                /* If TX Q is empty send current SKB;
+                 *  else, queue current SKB at end of tx_list queue and
+                 *  send first SKB in tx_list queue.
+                 */
+                hgps->tx_count--;
+                spin_unlock(&hgps->lock);
+
+                GPSDRV_VER(" Tx count in gpsdrv_write = %x", hgps->tx_count);
+
+                if (skb_queue_empty(&hgps->tx_list)) {
+                        hgps->st_write(skb);
+                } else {
+                        skb_queue_tail(&hgps->tx_list, skb);
+                        hgps->st_write(skb_dequeue(&hgps->tx_list));
+                }
+        } else {
+                /* Add it to TX queue */
+                spin_unlock(&hgps->lock);
+                GPSDRV_VER(" SKB added to Tx queue ");
+                GPSDRV_VER("Tx count = %x ", hgps->tx_count);
+                skb_queue_tail(&hgps->tx_list, skb);
+                /* This is added for precaution in the case that there is a
+                 * context switch during the execution of the above lines.
+                 * Redundant otherwise.
+                 */
+                if ((0 != hgps->tx_count) && \
+                                (!skb_queue_empty(&hgps->tx_list))) {
+                        /* Schedule the Tx-task let */
+                        GPSDRV_VER("Scheduling tasklet to write\n");
+                        tasklet_schedule(&hgps->gpsdrv_tx_tsklet);
+                }
+        }
+
+        return size;
+}
+
+/** gpsdrv_ioctl Function
+ *  This will peform the functions as directed by the command and command
+ *  argument.
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @cmd   : IOCTL Command
+ *  @arg   : Command argument for IOCTL command
+ *  Returns  GPS_SUCCESS on success
+ *           else suitable error code
+ */
+static long gpsdrv_ioctl(struct file *file,
+                unsigned int cmd, unsigned long arg)
+{
+        struct sk_buff *skb = NULL;
+        int             retCode = GPS_SUCCESS;
+        struct gpsdrv_data *hgps;
+
+        GPSDRV_DBG(" Inside %s", __func__);
+
+        /* Validate input parameters */
+        if ((NULL == file) || (0 == cmd)) {
+                GPSDRV_ERR("Invalid input parameters passed to %s", __func__);
+                return -EINVAL;
+        }
+
+        hgps = file->private_data;
+
+        /* Check if GPS is registered to perform IOCTL operation */
+        if (!test_bit(GPS_ST_RUNNING, &hgps->state)) {
+                GPSDRV_ERR("GPS Device is not running");
+                return -EINVAL;
+        }
+
+        switch (cmd) {
+        case TCFLSH:
+                GPSDRV_VER(" IOCTL TCFLSH invoked with %ld argument", arg);
+                switch (arg) {
+                        /* purge Rx/Tx SKB list queues depending on arg value */
+                case TCIFLUSH:
+                        skb_queue_purge(&hgps->rx_list);
+                        break;
+                case TCOFLUSH:
+                        skb_queue_purge(&hgps->tx_list);
+                        break;
+                case TCIOFLUSH:
+                        skb_queue_purge(&hgps->rx_list);
+                        skb_queue_purge(&hgps->tx_list);
+                        break;
+                default:
+                        GPSDRV_ERR("Invalid Command passed for tcflush");
+                        retCode = 0;
+                        break;
+                }
+                break;
+        case FIONREAD:
+                /* Deque the SKB from the head if rx_list is not empty
+                 * And update the argument with skb->len to provide
+                 * the amount of data available in the available SKB
+                 */
+                skb = skb_dequeue(&hgps->rx_list);
+                if (skb != NULL) {
+                        *(unsigned int *)arg = skb->len;
+                        /* Re-Store the SKB for furtur Read operations */
+                        skb_queue_head(&hgps->rx_list, skb);
+                } else {
+                        *(unsigned int *)arg = 0;
+                }
+                GPSDRV_DBG("returning %d\n", *(unsigned int *)arg);
+
+                break;
+        default:
+                GPSDRV_DBG("Un-Identified IOCTL %d", cmd);
+                retCode = 0;
+                break;
+        }
+
+        return retCode;
+}
+
+/** gpsdrv_poll Function
+ *  This function will wait till some data is received to the gps driver from ST
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @wait  : POLL wait information
+ *  Returns  status of POLL on success
+ *           else suitable error code
+ */
+static unsigned int gpsdrv_poll(struct file *file, poll_table *wait)
+{
+        unsigned long mask = 0;
+        struct gpsdrv_data *hgps = file->private_data;
+
+        /* Check if GPS is registered to perform read operation */
+        if (!test_bit(GPS_ST_RUNNING, &hgps->state)) {
+                GPSDRV_ERR("GPS Device is not running");
+                return -EINVAL;
+        }
+
+        /* Wait till data is signalled from gpsdrv_st_recv function
+         *  with AI2 packet
+         */
+        poll_wait(file, &hgps->gpsdrv_data_q, wait);
+
+        if (!skb_queue_empty(&hgps->rx_list))
+                mask |= POLLIN; /* TODO: check app for mask */
+
+        return mask;
+}
+
+/* GPS Char driver function pointers
+ * These functions are called from USER space by pefroming File Operations
+ * on /dev/gps node exposed by this driver during init
+ */
+const struct file_operations gpsdrv_chrdev_ops = {
+        .owner = THIS_MODULE,
+        .open = gpsdrv_open,
+        .read = gpsdrv_read,
+        .write = gpsdrv_write,
+        .unlocked_ioctl = gpsdrv_ioctl,
+        .poll = gpsdrv_poll,
+        .release = gpsdrv_release,
+};
+
+/*********Functions called during insmod and delmod****************************/
+
+static int gpsdrv_major;                /* GPS major number */
+static struct class *gpsdrv_class;      /* GPS class during class_create */
+static struct device *gpsdrv_dev;       /* GPS dev during device_create */
+/** gpsdrv_init Function
+ *  This function Initializes the gps driver parametes and exposes
+ *  /dev/gps node to user space
+ *
+ *  Parameters : NULL
+ *  Returns  GPS_SUCCESS on success
+ *           else suitable error code
+ */
+static int __init gpsdrv_init(void)
+{
+
+        GPSDRV_DBG(" Inside %s", __func__);
+
+        /* Expose the device DEVICE_NAME to user space
+         * And obtain the major number for the device
+         */
+        gpsdrv_major = register_chrdev(0, DEVICE_NAME, \
+                        &gpsdrv_chrdev_ops);
+        if (0 > gpsdrv_major) {
+                GPSDRV_ERR("Error when registering to char dev");
+                return GPS_ERR_FAILURE;
+        }
+        GPSDRV_VER("allocated %d, %d", gpsdrv_major, 0);
+
+        /*  udev */
+        gpsdrv_class = class_create(THIS_MODULE, DEVICE_NAME);
+        if (IS_ERR(gpsdrv_class)) {
+                GPSDRV_ERR(" Something went wrong in class_create");
+                unregister_chrdev(gpsdrv_major, DEVICE_NAME);
+                return GPS_ERR_CLASS;
+        }
+
+        gpsdrv_dev =
+                device_create(gpsdrv_class, NULL, MKDEV(gpsdrv_major, 0),
+                                NULL, DEVICE_NAME);
+        if (IS_ERR(gpsdrv_dev)) {
+                GPSDRV_ERR(" Error in class_create");
+                unregister_chrdev(gpsdrv_major, DEVICE_NAME);
+                class_destroy(gpsdrv_class);
+                return GPS_ERR_CLASS;
+        }
+
+        return GPS_SUCCESS;
+}
+
+/** gpsdrv_exit Function
+ *  This function Destroys the gps driver parametes and /dev/gps node
+ *
+ *  Parameters : NULL
+ *  Returns   NULL
+ */
+static void __exit gpsdrv_exit(void)
+{
+        GPSDRV_DBG(" Inside %s", __func__);
+        GPSDRV_VER(" Bye.. freeing up %d", gpsdrv_major);
+
+        device_destroy(gpsdrv_class, MKDEV(gpsdrv_major, 0));
+        class_destroy(gpsdrv_class);
+        unregister_chrdev(gpsdrv_major, DEVICE_NAME);
+}
+
+module_init(gpsdrv_init);
+module_exit(gpsdrv_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/uid_stat.c b/drivers/misc/uid_stat.c
new file mode 100644
index 00000000000..2141124a6c1
--- /dev/null
+++ b/drivers/misc/uid_stat.c
@@ -0,0 +1,156 @@
+/* drivers/misc/uid_stat.c
+ *
+ * Copyright (C) 2008 - 2009 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <asm/atomic.h>
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/proc_fs.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/stat.h>
+#include <linux/uid_stat.h>
+#include <net/activity_stats.h>
+
+static DEFINE_SPINLOCK(uid_lock);
+static LIST_HEAD(uid_list);
+static struct proc_dir_entry *parent;
+
+struct uid_stat {
+        struct list_head link;
+        uid_t uid;
+        atomic_t tcp_rcv;
+        atomic_t tcp_snd;
+};
+
+static struct uid_stat *find_uid_stat(uid_t uid) {
+        unsigned long flags;
+        struct uid_stat *entry;
+
+        spin_lock_irqsave(&uid_lock, flags);
+        list_for_each_entry(entry, &uid_list, link) {
+                if (entry->uid == uid) {
+                        spin_unlock_irqrestore(&uid_lock, flags);
+                        return entry;
+                }
+        }
+        spin_unlock_irqrestore(&uid_lock, flags);
+        return NULL;
+}
+
+static int tcp_snd_read_proc(char *page, char **start, off_t off,
+                                int count, int *eof, void *data)
+{
+        int len;
+        unsigned int bytes;
+        char *p = page;
+        struct uid_stat *uid_entry = (struct uid_stat *) data;
+        if (!data)
+                return 0;
+
+        bytes = (unsigned int) (atomic_read(&uid_entry->tcp_snd) + INT_MIN);
+        p += sprintf(p, "%u\n", bytes);
+        len = (p - page) - off;
+        *eof = (len <= count) ? 1 : 0;
+        *start = page + off;
+        return len;
+}
+
+static int tcp_rcv_read_proc(char *page, char **start, off_t off,
+                                int count, int *eof, void *data)
+{
+        int len;
+        unsigned int bytes;
+        char *p = page;
+        struct uid_stat *uid_entry = (struct uid_stat *) data;
+        if (!data)
+                return 0;
+
+        bytes = (unsigned int) (atomic_read(&uid_entry->tcp_rcv) + INT_MIN);
+        p += sprintf(p, "%u\n", bytes);
+        len = (p - page) - off;
+        *eof = (len <= count) ? 1 : 0;
+        *start = page + off;
+        return len;
+}
+
+/* Create a new entry for tracking the specified uid. */
+static struct uid_stat *create_stat(uid_t uid) {
+        unsigned long flags;
+        char uid_s[32];
+        struct uid_stat *new_uid;
+        struct proc_dir_entry *entry;
+
+        /* Create the uid stat struct and append it to the list. */
+        if ((new_uid = kmalloc(sizeof(struct uid_stat), GFP_KERNEL)) == NULL)
+                return NULL;
+
+        new_uid->uid = uid;
+        /* Counters start at INT_MIN, so we can track 4GB of network traffic. */
+        atomic_set(&new_uid->tcp_rcv, INT_MIN);
+        atomic_set(&new_uid->tcp_snd, INT_MIN);
+
+        spin_lock_irqsave(&uid_lock, flags);
+        list_add_tail(&new_uid->link, &uid_list);
+        spin_unlock_irqrestore(&uid_lock, flags);
+
+        sprintf(uid_s, "%d", uid);
+        entry = proc_mkdir(uid_s, parent);
+
+        /* Keep reference to uid_stat so we know what uid to read stats from. */
+        create_proc_read_entry("tcp_snd", S_IRUGO, entry , tcp_snd_read_proc,
+                (void *) new_uid);
+
+        create_proc_read_entry("tcp_rcv", S_IRUGO, entry, tcp_rcv_read_proc,
+                (void *) new_uid);
+
+        return new_uid;
+}
+
+int uid_stat_tcp_snd(uid_t uid, int size) {
+        struct uid_stat *entry;
+        activity_stats_update();
+        if ((entry = find_uid_stat(uid)) == NULL &&
+                ((entry = create_stat(uid)) == NULL)) {
+                        return -1;
+        }
+        atomic_add(size, &entry->tcp_snd);
+        return 0;
+}
+
+int uid_stat_tcp_rcv(uid_t uid, int size) {
+        struct uid_stat *entry;
+        activity_stats_update();
+        if ((entry = find_uid_stat(uid)) == NULL &&
+                ((entry = create_stat(uid)) == NULL)) {
+                        return -1;
+        }
+        atomic_add(size, &entry->tcp_rcv);
+        return 0;
+}
+
+static int __init uid_stat_init(void)
+{
+        parent = proc_mkdir("uid_stat", NULL);
+        if (!parent) {
+                pr_err("uid_stat: failed to create proc entry\n");
+                return -1;
+        }
+        return 0;
+}
+
+__initcall(uid_stat_init);
diff --git a/drivers/misc/wl127x-rfkill.c b/drivers/misc/wl127x-rfkill.c
new file mode 100644
index 00000000000..f5b95152948
--- /dev/null
+++ b/drivers/misc/wl127x-rfkill.c
@@ -0,0 +1,121 @@
+/*
+ * Bluetooth TI wl127x rfkill power control via GPIO
+ *
+ * Copyright (C) 2009 Motorola, Inc.
+ * Copyright (C) 2008 Texas Instruments
+ * Initial code: Pavan Savoy <pavan.savoy@gmail.com> (wl127x_power.c)
+ *
+ *  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, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/rfkill.h>
+#include <linux/platform_device.h>
+#include <linux/wl127x-rfkill.h>
+
+static int wl127x_rfkill_set_power(void *data, enum rfkill_state state)
+{
+        int nshutdown_gpio = (int) data;
+
+        switch (state) {
+        case RFKILL_STATE_UNBLOCKED:
+                gpio_set_value(nshutdown_gpio, 1);
+                break;
+        case RFKILL_STATE_SOFT_BLOCKED:
+                gpio_set_value(nshutdown_gpio, 0);
+                break;
+        default:
+                printk(KERN_ERR "invalid bluetooth rfkill state %d\n", state);
+        }
+        return 0;
+}
+
+static int wl127x_rfkill_probe(struct platform_device *pdev)
+{
+        int rc = 0;
+        struct wl127x_rfkill_platform_data *pdata = pdev->dev.platform_data;
+        enum rfkill_state default_state = RFKILL_STATE_SOFT_BLOCKED;  /* off */
+
+        rc = gpio_request(pdata->nshutdown_gpio, "wl127x_nshutdown_gpio");
+        if (unlikely(rc))
+                return rc;
+
+        rc = gpio_direction_output(pdata->nshutdown_gpio, 0);
+        if (unlikely(rc))
+                return rc;
+
+        rfkill_set_default(RFKILL_TYPE_BLUETOOTH, default_state);
+        wl127x_rfkill_set_power(NULL, default_state);
+
+        pdata->rfkill = rfkill_allocate(&pdev->dev, RFKILL_TYPE_BLUETOOTH);
+        if (unlikely(!pdata->rfkill))
+                return -ENOMEM;
+
+        pdata->rfkill->name = "wl127x";
+        pdata->rfkill->state = default_state;
+        /* userspace cannot take exclusive control */
+        pdata->rfkill->user_claim_unsupported = 1;
+        pdata->rfkill->user_claim = 0;
+        pdata->rfkill->data = (void *) pdata->nshutdown_gpio;
+        pdata->rfkill->toggle_radio = wl127x_rfkill_set_power;
+
+        rc = rfkill_register(pdata->rfkill);
+
+        if (unlikely(rc))
+                rfkill_free(pdata->rfkill);
+
+        return 0;
+}
+
+static int wl127x_rfkill_remove(struct platform_device *pdev)
+{
+        struct wl127x_rfkill_platform_data *pdata = pdev->dev.platform_data;
+
+        rfkill_unregister(pdata->rfkill);
+        rfkill_free(pdata->rfkill);
+        gpio_free(pdata->nshutdown_gpio);
+
+        return 0;
+}
+
+static struct platform_driver wl127x_rfkill_platform_driver = {
+        .probe = wl127x_rfkill_probe,
+        .remove = wl127x_rfkill_remove,
+        .driver = {
+                   .name = "wl127x-rfkill",
+                   .owner = THIS_MODULE,
+                   },
+};
+
+static int __init wl127x_rfkill_init(void)
+{
+        return platform_driver_register(&wl127x_rfkill_platform_driver);
+}
+
+static void __exit wl127x_rfkill_exit(void)
+{
+        platform_driver_unregister(&wl127x_rfkill_platform_driver);
+}
+
+module_init(wl127x_rfkill_init);
+module_exit(wl127x_rfkill_exit);
+
+MODULE_ALIAS("platform:wl127x");
+MODULE_DESCRIPTION("wl127x-rfkill");
+MODULE_AUTHOR("Motorola");
+MODULE_LICENSE("GPL");