samsung update 1

158 files changed, 64561 insertions, 9 deletions

diff --git a/drivers/misc/2mic/Kconfig b/drivers/misc/2mic/Kconfig
new file mode 100644
index 0000000..16da70e
--- /dev/null
+++ b/drivers/misc/2mic/Kconfig
@@ -0,0 +1,5 @@
+config 2MIC_FM34_WE395
+	bool "Forte Media 2MIC driver"
+	default n
+	help
+	  This is Forte media voice solution devices.
\ No newline at end of file
diff --git a/drivers/misc/2mic/Makefile b/drivers/misc/2mic/Makefile
new file mode 100644
index 0000000..a05d51a
--- /dev/null
+++ b/drivers/misc/2mic/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_2MIC_FM34_WE395)	+= fm34_we395.o
diff --git a/drivers/misc/2mic/fm34_cmd/fm34_we395_c1_lgt.h b/drivers/misc/2mic/fm34_cmd/fm34_we395_c1_lgt.h
new file mode 100644
index 0000000..1b8eb6a
--- /dev/null
+++ b/drivers/misc/2mic/fm34_cmd/fm34_we395_c1_lgt.h
@@ -0,0 +1,289 @@
+/*
+ * Copyright (C) 2012 Samsung 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.
+ *
+ */
+
+
+ #ifndef __FEM34_WE395_CMD_H__
+#define __FEM34_WE395_CMD_H__
+
+unsigned char bypass_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xC0, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC1, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xC2, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC3, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xB8,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char handset_ns_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF7, 0x00, 0x80,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x03, 0x7D, 0xB9,
+0xFC, 0xF3, 0x3B, 0x23, 0x04, 0x83, 0xDE,
+0xFC, 0xF3, 0x3B, 0x23, 0x05, 0x20, 0x6C,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x08, 0x08, 0xFA,
+0xFC, 0xF3, 0x3B, 0x23, 0x09, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xB8,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x10, 0x12, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x25, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x26, 0x00, 0x38,
+0xFC, 0xF3, 0x3B, 0x23, 0x32, 0x05, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x33, 0x00, 0x0C,
+0xFC, 0xF3, 0x3B, 0x23, 0x37, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x39, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x48, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x49, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x50, 0x38, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x51, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x52, 0x00, 0xC0,
+0xFC, 0xF3, 0x3B, 0x23, 0x56, 0x50, 0x32,
+0xFC, 0xF3, 0x3B, 0x23, 0x5A, 0x06, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x5B, 0x1E, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x60, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x61, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x64, 0x00, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6E, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6F, 0x12, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x70, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x71, 0x05, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x73, 0x21, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x74, 0x1C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x75, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x77, 0x2C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7B, 0x00, 0x08,
+0xFC, 0xF3, 0x3B, 0x23, 0x7C, 0x58, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x82, 0x03, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x83, 0x03, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x84, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0x86, 0x48, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x8B, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x90, 0x85, 0x67,
+0xFC, 0xF3, 0x3B, 0x23, 0x91, 0x64, 0x44,
+0xFC, 0xF3, 0x3B, 0x23, 0x92, 0x22, 0x24,
+0xFC, 0xF3, 0x3B, 0x23, 0x93, 0x44, 0x44,
+0xFC, 0xF3, 0x3B, 0x23, 0x9C, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xA6, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x23, 0xBD, 0x30, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xC9, 0x16, 0x18,
+0xFC, 0xF3, 0x3B, 0x23, 0xCB, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xCD, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0xCF, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD0, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD1, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD2, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xDB, 0x38, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDC, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDF, 0x96, 0x64,
+0xFC, 0xF3, 0x3B, 0x23, 0xE0, 0x66, 0x65,
+0xFC, 0xF3, 0x3B, 0x23, 0xE1, 0x43, 0x33,
+0xFC, 0xF3, 0x3B, 0x23, 0xE2, 0x34, 0x66,
+0xFC, 0xF3, 0x3B, 0x23, 0xE3, 0x6F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xE4, 0xFF, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xE7, 0x16, 0x3E,
+0xFC, 0xF3, 0x3B, 0x23, 0xE9, 0x2C, 0xCC,
+0xFC, 0xF3, 0x3B, 0x23, 0xEA, 0x6C, 0xCC,
+0xFC, 0xF3, 0x3B, 0x23, 0xEB, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x23, 0xEC, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x23, 0xED, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xEE, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xF0, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE8, 0x16, 0xA2,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char loud_ns_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xB8, 0x1F, 0x40,
+0xFC, 0xF3, 0x3B, 0x22, 0xB9, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xF2, 0x00, 0x48,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x11, 0x4E, 0xA9,
+0xFC, 0xF3, 0x3B, 0x23, 0x12, 0xB1, 0x5C,
+0xFC, 0xF3, 0x3B, 0x23, 0x13, 0x4E, 0xA9,
+0xFC, 0xF3, 0x3B, 0x23, 0x14, 0x95, 0x7C,
+0xFC, 0xF3, 0x3B, 0x23, 0x15, 0x5C, 0xB7,
+0xFC, 0xF3, 0x3B, 0x23, 0x16, 0x54, 0x2A,
+0xFC, 0xF3, 0x3B, 0x23, 0x17, 0xAB, 0xEF,
+0xFC, 0xF3, 0x3B, 0x23, 0x18, 0x54, 0x2A,
+0xFC, 0xF3, 0x3B, 0x23, 0x19, 0x84, 0x8D,
+0xFC, 0xF3, 0x3B, 0x23, 0x1A, 0x79, 0x55,
+0xFC, 0xF3, 0x3B, 0x23, 0x27, 0x7F, 0x30,
+0xFC, 0xF3, 0x3B, 0x23, 0x41, 0xFF, 0xF3,
+0xFC, 0xF3, 0x3B, 0x23, 0x43, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x44, 0x00, 0x05,
+0xFC, 0xF3, 0x3B, 0x23, 0x52, 0x00, 0x60,
+0xFC, 0xF3, 0x3B, 0x23, 0x5C, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x5F, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x8D, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x8E, 0x74, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x96, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xCE, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xEF, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xF0, 0x6A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x39, 0x00, 0x10,
+0xFC, 0xF3, 0x3B, 0x23, 0x37, 0xFF, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0x2F, 0x00, 0x80,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x23, 0xE7, 0x0D, 0x45,
+0xFC, 0xF3, 0x3B, 0x23, 0xE8, 0x19, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x97, 0x01, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xE0, 0x65, 0x54,
+0xFC, 0xF3, 0x3B, 0x23, 0xE1, 0x33, 0x33,
+0xFC, 0xF3, 0x3B, 0x23, 0xE2, 0x34, 0x66,
+0xFC, 0xF3, 0x3B, 0x23, 0xE3, 0x6F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xE4, 0xFF, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xDF, 0x99, 0x97,
+0xFC, 0xF3, 0x3B, 0x23, 0xDB, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xDC, 0x0A, 0x2B,
+0xFC, 0xF3, 0x3B, 0x23, 0x3D, 0x12, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x33, 0x00, 0x08,
+0xFC, 0xF3, 0x3B, 0x23, 0x32, 0x00, 0x30,
+0xFC, 0xF3, 0x3B, 0x23, 0x81, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x80, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7C, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7D, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7F, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x91, 0x67, 0x76,
+0xFC, 0xF3, 0x3B, 0x23, 0x92, 0x65, 0x43,
+0xFC, 0xF3, 0x3B, 0x23, 0x93, 0x33, 0x33,
+0xFC, 0xF3, 0x3B, 0x23, 0x94, 0x33, 0x33,
+0xFC, 0xF3, 0x3B, 0x23, 0x95, 0x33, 0x34,
+0xFC, 0xF3, 0x3B, 0x23, 0x90, 0x44, 0x45,
+0xFC, 0xF3, 0x3B, 0x23, 0x8C, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x83, 0x05, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x82, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x05, 0x00, 0x47,
+0xFC, 0xF3, 0x3B, 0x23, 0x70, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x23, 0xEE, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xED, 0x00, 0x3A,
+0xFC, 0xF3, 0x3B, 0x23, 0x9C, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x03, 0x2D, 0xC9,
+0xFC, 0xF3, 0x3B, 0x23, 0x3F, 0x00, 0x06,
+0xFC, 0xF3, 0x3B, 0x23, 0x42, 0xFF, 0xF2,
+0xFC, 0xF3, 0x3B, 0x23, 0xBE, 0x26, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x10, 0x12, 0x48,
+0xFC, 0xF3, 0x3B, 0x23, 0x09, 0x04, 0x03,
+0xFC, 0xF3, 0x3B, 0x23, 0x7A, 0x05, 0xAA,
+0xFC, 0xF3, 0x3B, 0x23, 0x64, 0x00, 0x3C,
+0xFC, 0xF3, 0x3B, 0x23, 0x48, 0x0B, 0x4D,
+0xFC, 0xF3, 0x3B, 0x23, 0x49, 0x0B, 0x4D,
+0xFC, 0xF3, 0x3B, 0x23, 0x51, 0x31, 0x0E,
+0xFC, 0xF3, 0x3B, 0x23, 0x50, 0x31, 0x0E,
+0xFC, 0xF3, 0x3B, 0x23, 0x53, 0x20, 0x10,
+0xFC, 0xF3, 0x3B, 0x23, 0x54, 0x1F, 0xF0,
+0xFC, 0xF3, 0x3B, 0x23, 0x56, 0x6A, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x08, 0x04, 0x03,
+0xFC, 0xF3, 0x3B, 0x23, 0x71, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6E, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x01, 0xC0,
+0xFC, 0xF3, 0x3B, 0x23, 0x72, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6F, 0x10, 0x06,
+0xFC, 0xF3, 0x3B, 0x23, 0x75, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x73, 0x24, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x74, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x9E, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x9D, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xB5, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xBA, 0x06, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xBB, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xB8, 0x78, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xB9, 0x24, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xBD, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xA5, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x23, 0xB4, 0x00, 0x0F,
+0xFC, 0xF3, 0x3B, 0x23, 0xB3, 0x00, 0x10,
+0xFC, 0xF3, 0x3B, 0x23, 0xB7, 0x00, 0x0B,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x23, 0x3C, 0x00, 0x40,
+0xFC, 0xF3, 0x3B, 0x22, 0xF7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x84, 0x00, 0x0C,
+0xFC, 0xF3, 0x3B, 0x23, 0xBC, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x26, 0x00, 0x38,
+0xFC, 0xF3, 0x3B, 0x23, 0x25, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE9, 0x06, 0x66,
+0xFC, 0xF3, 0x3B, 0x23, 0xEA, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x01, 0xC0,
+0xFC, 0xF3, 0x3B, 0x23, 0x04, 0x03, 0xDE,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x23, 0xD1, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xD2, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xCF, 0x04, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0xD0, 0x01, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x5A, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x5B, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xD5, 0x34, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x2D, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char ftm_loopback_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC0, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xC1, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xC2, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x22, 0xC3, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xB8,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+#endif
diff --git a/drivers/misc/2mic/fm34_cmd/fm34_we395_default.h b/drivers/misc/2mic/fm34_cmd/fm34_we395_default.h
new file mode 100644
index 0000000..6695791
--- /dev/null
+++ b/drivers/misc/2mic/fm34_cmd/fm34_we395_default.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2012 Samsung 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.
+ *
+ */
+
+
+ #ifndef __FEM34_WE395_CMD_H__
+#define __FEM34_WE395_CMD_H__
+
+unsigned char bypass_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x00, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00,
+};
+
+unsigned char handset_ns_cmd[] = {
+};
+
+unsigned char loud_ns_cmd[] = {
+};
+
+unsigned char ftm_loopback_cmd[] = {
+};
+
+#endif
diff --git a/drivers/misc/2mic/fm34_we395.c b/drivers/misc/2mic/fm34_we395.c
new file mode 100644
index 0000000..677086d
--- /dev/null
+++ b/drivers/misc/2mic/fm34_we395.c
@@ -0,0 +1,297 @@
+/* drivers/misc/2mic/fm34_we395.c - fm34_we395 voice processor driver
+ *
+ * Copyright (C) 2012 Samsung 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/delay.h>
+#include <linux/freezer.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+#include <linux/i2c/fm34_we395.h>
+#include <linux/i2c/voice_processor.h>
+
+#include "fm34_we395.h"
+
+static struct class *fm34_class;
+static struct device *fm34_dev;
+
+struct fm34_data {
+	struct fm34_platform_data	*pdata;
+	struct device	*dev;
+	struct i2c_client	*client;
+	enum voice_processing_mode	curr_mode;
+};
+
+static void fm34_reset_parameter(struct fm34_data *fm34)
+{
+	gpio_set_value(fm34->pdata->gpio_rst, 0);
+	usleep_range(10000, 10000);
+	gpio_set_value(fm34->pdata->gpio_pwdn, 1);
+	gpio_set_value(fm34->pdata->gpio_bp, 1);
+	msleep(50);
+	gpio_set_value(fm34->pdata->gpio_rst, 1);
+	msleep(50);
+}
+
+static int fm34_set_mode(struct fm34_data *fm34,
+					enum voice_processing_mode mode)
+{
+	int ret = 0;
+	unsigned char *i2c_cmd;
+	int size = 0;
+
+	dev_dbg(fm34->dev, "%s : mode %d\n", __func__, mode);
+
+	if (fm34->curr_mode == mode) {
+		dev_dbg(fm34->dev, "skip %s\n", __func__);
+		return ret;
+	}
+
+	switch (mode) {
+	case VOICE_NS_BYPASS_MODE:
+		usleep_range(20000, 20000);
+		gpio_set_value(fm34->pdata->gpio_pwdn, 0);
+		return ret;
+	case VOICE_NS_HANDSET_MODE:
+		i2c_cmd = handset_ns_cmd;
+		size = sizeof(handset_ns_cmd);
+		break;
+	case VOICE_NS_LOUD_MODE:
+		i2c_cmd = loud_ns_cmd;
+		size = sizeof(loud_ns_cmd);
+		break;
+	case VOICE_NS_FTM_LOOPBACK_MODE:
+		i2c_cmd = ftm_loopback_cmd;
+		size = sizeof(ftm_loopback_cmd);
+		break;
+	default:
+		break;
+	}
+
+	fm34_reset_parameter(fm34);
+
+	if (size) {
+		ret = i2c_master_send(fm34->client, i2c_cmd, size);
+		if (ret < 0) {
+			dev_err(fm34->dev, "i2c_master_send failed %d\n", ret);
+			return ret;
+		}
+		fm34->curr_mode = mode;
+	}
+
+	return ret;
+}
+
+static ssize_t fm34_mode_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct fm34_data *fm34 = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", fm34->curr_mode);
+}
+
+static ssize_t fm34_mode_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct fm34_data *fm34 = dev_get_drvdata(dev);
+	long mode;
+	ssize_t status;
+
+	status = strict_strtol(buf, 0, &mode);
+	if (status == 0) {
+		dev_info(fm34->dev, "%s mode = %ld\n", __func__, mode);
+		fm34_set_mode(fm34, mode);
+	} else
+		dev_err(fm34->dev, "%s : operation is not valid\n", __func__);
+
+	return status ? : size;
+}
+
+static DEVICE_ATTR(mode, S_IRUGO | S_IWUSR | S_IWGRP,
+		fm34_mode_show, fm34_mode_store);
+
+static int fm34_init_parameter(struct fm34_data *fm34)
+{
+	int ret = 0;
+	unsigned char *i2c_cmd;
+	int size = 0;
+
+	fm34_reset_parameter(fm34);
+
+	i2c_cmd = bypass_cmd;
+	size = sizeof(bypass_cmd);
+
+	ret = i2c_master_send(fm34->client, i2c_cmd, size);
+	if (ret < 0) {
+		dev_err(fm34->dev, "i2c_master_send failed %d\n", ret);
+		return ret;
+	}
+
+	fm34->curr_mode = VOICE_NS_BYPASS_MODE;
+
+	usleep_range(20000, 20000);
+	gpio_set_value(fm34->pdata->gpio_pwdn, 0);
+
+	return ret;
+}
+
+static int __devinit fm34_probe(
+		struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct fm34_data *fm34;
+	struct fm34_platform_data *pdata;
+	int ret = 0;
+
+	fm34 = kzalloc(sizeof(*fm34), GFP_KERNEL);
+	if (fm34 == NULL) {
+		ret = -ENOMEM;
+		goto err_kzalloc;
+	}
+
+	fm34->client = client;
+	i2c_set_clientdata(client, fm34);
+
+	fm34->dev = &client->dev;
+	dev_set_drvdata(fm34->dev, fm34);
+
+	fm34->pdata = client->dev.platform_data;
+	pdata = fm34->pdata;
+
+	fm34->curr_mode = -1;
+
+	ret = gpio_request(pdata->gpio_rst, "FM34_RESET");
+	if (ret < 0) {
+		dev_err(fm34->dev, "error requesting reset gpio\n");
+		goto err_gpio_reset;
+	}
+	gpio_direction_output(pdata->gpio_rst, 1);
+
+	ret = gpio_request(pdata->gpio_pwdn, "FM34_PWDN");
+	if (ret < 0) {
+		dev_err(fm34->dev, "error requesting pwdn gpio\n");
+		goto err_gpio_pwdn;
+	}
+	gpio_direction_output(pdata->gpio_pwdn, 1);
+
+	ret = gpio_request(pdata->gpio_bp, "FM34_BYPASS");
+	if (ret < 0) {
+		dev_err(fm34->dev, "error requesting bypass gpio\n");
+		goto err_gpio_bp;
+	}
+	gpio_direction_output(pdata->gpio_bp, 1);
+
+	if (fm34->pdata->set_mclk != NULL)
+		fm34->pdata->set_mclk(true, false);
+
+	ret = fm34_init_parameter(fm34);
+	if (ret < 0)
+		dev_err(fm34->dev, "fm34_init_parameter failed %d\n", ret);
+
+	fm34_class = class_create(THIS_MODULE, "voice_processor");
+	if (IS_ERR(fm34_class)) {
+		dev_err(fm34->dev, "Failed to create class(voice_processor) %ld\n",
+				IS_ERR(fm34_class));
+		goto err_class_create;
+	}
+
+	fm34_dev = device_create(fm34_class, NULL, 0, fm34, "2mic");
+	if (IS_ERR(fm34_dev)) {
+		dev_err(fm34->dev, "Failed to create device(2mic) %ld\n",
+				IS_ERR(fm34_dev));
+		goto err_device_create;
+	}
+
+	ret = device_create_file(fm34_dev, &dev_attr_mode);
+	if (ret < 0)
+		dev_err(fm34->dev, "Failed to create device file (%s) %d\n",
+					dev_attr_mode.attr.name, ret);
+
+	return 0;
+
+err_gpio_bp:
+	gpio_free(pdata->gpio_pwdn);
+err_gpio_pwdn:
+	gpio_free(pdata->gpio_rst);
+err_gpio_reset:
+	kfree(fm34);
+err_kzalloc:
+err_class_create:
+err_device_create:
+	return ret;
+}
+
+static int __devexit fm34_remove(struct i2c_client *client)
+{
+	struct fm34_data *fm34 = i2c_get_clientdata(client);
+	i2c_set_clientdata(client, NULL);
+	gpio_free(fm34->pdata->gpio_rst);
+	gpio_free(fm34->pdata->gpio_pwdn);
+	kfree(fm34);
+	return 0;
+}
+
+static int fm34_suspend(struct device *dev)
+{
+	return 0;
+}
+
+static int fm34_resume(struct device *dev)
+{
+	return 0;
+}
+
+static const struct i2c_device_id fm34_id[] = {
+	{ "fm34_we395", 0 },
+	{ }
+};
+
+static const struct dev_pm_ops fm34_pm_ops = {
+	.suspend = fm34_suspend,
+	.resume = fm34_resume,
+};
+
+static struct i2c_driver fm34_driver = {
+	.probe = fm34_probe,
+	.remove = __devexit_p(fm34_remove),
+	.id_table = fm34_id,
+	.driver = {
+		.name = "fm34_we395",
+		.owner = THIS_MODULE,
+		.pm = &fm34_pm_ops,
+	},
+};
+
+static int __init fm34_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return i2c_add_driver(&fm34_driver);
+}
+
+static void __exit fm34_exit(void)
+{
+	i2c_del_driver(&fm34_driver);
+}
+
+module_init(fm34_init);
+module_exit(fm34_exit);
+
+MODULE_DESCRIPTION("fm34 voice processor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/2mic/fm34_we395.h b/drivers/misc/2mic/fm34_we395.h
new file mode 100644
index 0000000..03d31d4
--- /dev/null
+++ b/drivers/misc/2mic/fm34_we395.h
@@ -0,0 +1,24 @@
+/* drivers/misc/2mic/fm34_we395.h
+ *
+ * Copyright (C) 2012 Samsung 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.
+ *
+ */
+
+ #ifndef __FEM34_WE395_H__
+#define __FM34_WE395_H__
+
+#ifdef CONFIG_MACH_C1_KOR_LGT
+#include "./fm34_cmd/fm34_we395_c1_lgt.h"
+#else
+#include "./fm34_cmd/fm34_we395_default.h"
+#endif
+#endif
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 56c05ef..db4121a 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -61,12 +61,33 @@ config AD525X_DPOT_SPI
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad525x_dpot-spi.
 
+config ANDROID_PMEM
+	bool "Android pmem allocator"
+	depends on MACH_U1 || MACH_PX
+	default n
+
+if ANDROID_PMEM
+	comment "Reserved memory configurations"
+
+config ANDROID_PMEM_MEMSIZE_PMEM
+	int "Memory size in kbytes for android surface using pmem"
+	default "4096"
+
+config ANDROID_PMEM_MEMSIZE_PMEM_GPU1
+	int "Memory size in kbytes for android surface using pmem_gpu1"
+	default "10240"
+
+config ANDROID_PMEM_MEMSIZE_PMEM_CAM
+	int "Memory size in kbytes for android camera using pmem_camera"
+	default "4096"
+endif
+
 config ATMEL_PWM
 	tristate "Atmel AT32/AT91 PWM support"
 	depends on AVR32 || ARCH_AT91SAM9263 || ARCH_AT91SAM9RL || ARCH_AT91CAP9
 	help
 	  This option enables device driver support for the PWM channels
-	  on certain Atmel processors.  Pulse Width Modulation is used for
+	  on certain Atmel processors.	Pulse Width Modulation is used for
 	  purposes including software controlled power-efficient backlights
 	  on LCD displays, motor control, and waveform generation.
 
@@ -364,14 +385,14 @@ config SENSORS_BH1780
 	  will be called bh1780gli.
 
 config SENSORS_BH1770
-         tristate "BH1770GLC / SFH7770 combined ALS - Proximity sensor"
-         depends on I2C
-         ---help---
-           Say Y here if you want to build a driver for BH1770GLC (ROHM) or
+	 tristate "BH1770GLC / SFH7770 combined ALS - Proximity sensor"
+	 depends on I2C
+	 ---help---
+	   Say Y here if you want to build a driver for BH1770GLC (ROHM) or
 	   SFH7770 (Osram) combined ambient light and proximity sensor chip.
 
-           To compile this driver as a module, choose M here: the
-           module will be called bh1770glc. If unsure, say N here.
+	   To compile this driver as a module, choose M here: the
+	   module will be called bh1770glc. If unsure, say N here.
 
 config SENSORS_APDS990X
 	 tristate "APDS990X combined als and proximity sensors"
@@ -391,6 +412,14 @@ config HMC6352
 	  This driver provides support for the Honeywell HMC6352 compass,
 	  providing configuration and heading data via sysfs.
 
+config SENSORS_AK8975
+	tristate "AK8975 compass support"
+	default n
+	depends on I2C
+	help
+	  If you say yes here you get support for Asahi Kasei's
+	  orientation sensor AK8975.
+
 config EP93XX_PWM
 	tristate "EP93xx PWM support"
 	depends on ARCH_EP93XX
@@ -434,6 +463,10 @@ config TI_DAC7512
 	  This driver can also be built as a module. If so, the module
 	  will be called ti_dac7512.
 
+config UID_STAT
+	bool "UID based statistics tracking exported to /proc/uid_stat"
+	default n
+
 config VMWARE_BALLOON
 	tristate "VMware Balloon Driver"
 	depends on X86
@@ -490,12 +523,139 @@ config PCH_PHUB
 	  To compile this driver as a module, choose M here: the module will
 	  be called pch_phub.
 
+config WL127X_RFKILL
+	tristate "Bluetooth power control driver for TI wl127x"
+	depends on RFKILL
+	default n
+	---help---
+	 Creates an rfkill entry in sysfs for power control of Bluetooth
+	 TI wl127x chips.
+
+config APANIC
+	bool "Android kernel panic diagnostics driver"
+	default n
+	---help---
+	 Driver which handles kernel panics and attempts to write
+	 critical debugging data to flash.
+
+config APANIC_PLABEL
+	string "Android panic dump flash partition label"
+	depends on APANIC
+	default "kpanic"
+	---help---
+	 If your platform uses a different flash partition label for storing
+	 crashdumps, enter it here.
+
+config JACK_MON
+	bool "Jacks Monitoring Driver"
+	default n
+	help
+	 Jacks Monitoring Driver
+
+config UART_SELECT
+	bool "Uart Selection Driver"
+	default n
+	help
+	 This is uart selection driver.
+
+config SEC_DEV_JACK
+	bool "Earjack detection driver for Samsung devices"
+	depends on INPUT
+	default n
+	---help---
+	 This is Earjack detection driver for Samsung devices.
+
+config FM34_WE395
+	bool "Forte Media 2MIC driver"
+	default n
+	help
+	  This is Forte media voice solution devices.
+	  Say yes here to build support for FM34_WE395 2mic driver.
+	  If your device have this soulution, you maybe say yes to hear sounds.
+	  FM34 series (FM34-WE395, FM34-WE500) share it.
+
+config AUDIENCE_ES305
+	bool "Audience 2MIC driver"
+	default n
+	help
+	  This is Audience voice processor devices.
+	  Say yes here to build support for AUDIENCE_ES305 2mic driver.
+	  Audience A2220 series are same as ES305.
+	  A1026 also can use this driver.
+
+source "drivers/misc/2mic/Kconfig"
+
+config MUIC_MAX8997
+	tristate "MAX8997 MUIC"
+	depends on MFD_MAX8997
+	help
+	  If you say yes here you will get support for the MUIC of
+	  Maxim MAX8997 PMIC.
+	  The MAX8997 MUIC is a USB port accessory detector and switch.
+
+config MUIC_MAX8997_OVPUI
+	tristate "MAX8997 MUIC"
+	depends on MFD_MAX8997
+	default n
+	help
+	  If you say yes here you will get support for the MUIC OVP UI
+	  concept of Maxim MAX8997 PMIC. The MAX8997 MUIC OVP UI concept
+	  is used in Chinese models. When OVP is occurred, the battery UI
+	  must be changed to uncharging status.
+
+config USBHUB_USB3503
+	bool "USB3503 USB HUB Driver"
+	depends on I2C
+	help
+	 Enables USB HUB USB3503
+
+config PN544
+        bool "NXP PN544 NFC Controller Driver"
+        default n
+        help
+          NXP PN544 Near Field Communication controller support.
+
+config STMPE811_ADC
+	tristate "STMPE811 ADC driver"
+	depends on I2C
+	---help---
+	  Say yes here to build support for STMPE811 8 channel ADC driver
+
+config MPU_SENSORS_MPU3050
+    tristate "MPU3050"
+    depends on I2C
+
+config MPU_SENSORS_MPU6050
+    tristate "MPU6050"
+    depends on I2C
+
 source "drivers/misc/c2port/Kconfig"
 source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
 source "drivers/misc/iwmc3200top/Kconfig"
+if MPU_SENSORS_MPU3050
+source "drivers/misc/mpu3050/Kconfig"
+endif
+if MPU_SENSORS_MPU6050
+source "drivers/misc/inv_mpu/Kconfig"
+endif
 source "drivers/misc/ti-st/Kconfig"
 source "drivers/misc/lis3lv02d/Kconfig"
 source "drivers/misc/carma/Kconfig"
+source "drivers/misc/c2c/Kconfig"
+source "drivers/misc/modem_if/Kconfig"
+
+config SLP_LOWMEM_NOTIFY
+	bool "Driver for SLP Low Memory Notification"
+	depends on SLP
+	help
+	  Provide interface to register for low memory notifications.
+
+config SLP_PROCESS_MON
+	bool "Driver for SLP process monitoring"
+	depends on SLP
+	help
+	  Providing monitoring important processes. Users can register the process
+	  with sysfs.
 
 endif # MISC_DEVICES
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 5f03172..027da64 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -13,12 +13,13 @@ obj-$(CONFIG_ATMEL_TCLIB)	+= atmel_tclib.o
 obj-$(CONFIG_BMP085)		+= bmp085.o
 obj-$(CONFIG_ICS932S401)	+= ics932s401.o
 obj-$(CONFIG_LKDTM)		+= lkdtm.o
-obj-$(CONFIG_TIFM_CORE)       	+= tifm_core.o
-obj-$(CONFIG_TIFM_7XX1)       	+= tifm_7xx1.o
+obj-$(CONFIG_TIFM_CORE)		+= tifm_core.o
+obj-$(CONFIG_TIFM_7XX1)		+= tifm_7xx1.o
 obj-$(CONFIG_PHANTOM)		+= phantom.o
 obj-$(CONFIG_SENSORS_BH1780)	+= bh1780gli.o
 obj-$(CONFIG_SENSORS_BH1770)	+= bh1770glc.o
 obj-$(CONFIG_SENSORS_APDS990X)	+= apds990x.o
+obj-$(CONFIG_ANDROID_PMEM)	+= pmem.o
 obj-$(CONFIG_SGI_IOC4)		+= ioc4.o
 obj-$(CONFIG_ENCLOSURE_SERVICES) += enclosure.o
 obj-$(CONFIG_KGDB_TESTS)	+= kgdbts.o
@@ -33,11 +34,14 @@ obj-$(CONFIG_SENSORS_TSL2550)	+= tsl2550.o
 obj-$(CONFIG_EP93XX_PWM)	+= ep93xx_pwm.o
 obj-$(CONFIG_DS1682)		+= ds1682.o
 obj-$(CONFIG_TI_DAC7512)	+= ti_dac7512.o
+obj-$(CONFIG_UID_STAT)		+= uid_stat.o
 obj-$(CONFIG_C2PORT)		+= c2port/
 obj-$(CONFIG_IWMC3200TOP)      += iwmc3200top/
 obj-$(CONFIG_HMC6352)		+= hmc6352.o
 obj-y				+= eeprom/
 obj-y				+= cb710/
+obj-$(CONFIG_MPU_SENSORS_MPU3050) += mpu3050/
+obj-$(CONFIG_MPU_SENSORS_MPU6050) += inv_mpu/
 obj-$(CONFIG_SPEAR13XX_PCIE_GADGET)	+= spear13xx_pcie_gadget.o
 obj-$(CONFIG_VMWARE_BALLOON)	+= vmw_balloon.o
 obj-$(CONFIG_ARM_CHARLCD)	+= arm-charlcd.o
@@ -46,3 +50,24 @@ obj-y				+= ti-st/
 obj-$(CONFIG_AB8500_PWM)	+= ab8500-pwm.o
 obj-y				+= lis3lv02d/
 obj-y				+= carma/
+obj-$(CONFIG_WL127X_RFKILL)	+= wl127x-rfkill.o
+obj-$(CONFIG_APANIC)		+= apanic.o
+obj-$(CONFIG_SENSORS_AK8975)	+= akm8975.o
+obj-$(CONFIG_SAMSUNG_C2C)	+= c2c/
+obj-$(CONFIG_USBHUB_USB3503)	+= usb3503.o
+obj-$(CONFIG_SEC_MODEM)		+= modem_if/
+obj-$(CONFIG_MFD_MAX77693)	+= max77693-muic.o
+obj-$(CONFIG_STMPE811_ADC)	+= stmpe811-adc.o
+obj-$(CONFIG_JACK_MON)		+= jack.o
+obj-$(CONFIG_UART_SELECT)	+= uart_select.o
+obj-$(CONFIG_SEC_DEV_JACK)	+= sec_jack.o
+obj-$(CONFIG_MUIC_MAX8997)	+= max8997-muic.o
+obj-$(CONFIG_PN544)		+= pn544.o
+obj-$(CONFIG_FM34_WE395)	+= fm34_we395.o
+obj-$(CONFIG_AUDIENCE_ES305)	+= es305.o
+obj-y				+= 2mic/
+
+obj-$(CONFIG_MACH_M0_CTC)   += cw_tty.o
+
+# Secure OS Mobicore Interface
+obj-$(CONFIG_MOBICORE_SUPPORT)  += tzic.o
diff --git a/drivers/misc/akm8975.c b/drivers/misc/akm8975.c
new file mode 100644
index 0000000..830d289
--- /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,
+	.ioctl = akmd_ioctl,
+};
+
+static const struct file_operations akm_aot_fops = {
+	.owner = THIS_MODULE,
+	.open = akm_aot_open,
+	.release = akm_aot_release,
+	.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 0000000..ca875f8
--- /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/c2c/Kconfig b/drivers/misc/c2c/Kconfig
new file mode 100644
index 0000000..3b91fa5
--- /dev/null
+++ b/drivers/misc/c2c/Kconfig
@@ -0,0 +1,24 @@
+#
+# C2C XXX
+#
+
+menuconfig SAMSUNG_C2C
+	tristate "C2C Support"
+	depends on CPU_EXYNOS4212 || CPU_EXYNOS5250
+	default n
+	help
+	  It is for supporting C2C driver.
+
+if SAMSUNG_C2C
+config C2C_DEBUG
+	bool "C2C Debugging - Print C2C debug messages"
+	default y
+	help
+	  Print C2C debug messages.
+
+config C2C_IPC_ENABLE
+	bool "Enable C2C IPC via the Shared Memory"
+	default n
+	help
+	  Enable C2C IPC via the Shared Memory.
+endif # SAMSUNG_C2C
diff --git a/drivers/misc/c2c/Makefile b/drivers/misc/c2c/Makefile
new file mode 100644
index 0000000..ef12d10
--- /dev/null
+++ b/drivers/misc/c2c/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for C2C
+#
+
+obj-$(CONFIG_SAMSUNG_C2C)	+= samsung-c2c.o
diff --git a/drivers/misc/c2c/samsung-c2c.c b/drivers/misc/c2c/samsung-c2c.c
new file mode 100644
index 0000000..fe58c3d
--- /dev/null
+++ b/drivers/misc/c2c/samsung-c2c.c
@@ -0,0 +1,712 @@
+/*
+ * Samsung C2C driver
+ *
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Author: Kisang Lee <kisang80.lee@samsung.com>
+ *
+ * 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.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/cma.h>
+#include <linux/sysfs.h>
+#ifdef ENABLE_C2CSTATE_TIMER
+#include <linux/timer.h>
+#endif
+#ifdef CONFIG_C2C_IPC_ENABLE
+#include <linux/vmalloc.h>
+#include <asm/page.h>
+#include <asm/pgtable.h>
+#endif
+#include <asm/mach-types.h>
+
+#include <mach/c2c.h>
+#include <mach/regs-c2c.h>
+#include <mach/regs-pmu.h>
+#include <mach/regs-pmu5.h>
+#include <mach/pmu.h>
+#include <plat/cpu.h>
+
+#include "samsung-c2c.h"
+
+void (*exynos_c2c_request_pwr_mode)(enum c2c_pwr_mode mode);
+
+#ifdef ENABLE_C2CSTATE_TIMER
+struct timer_list c2c_status_timer;
+
+static void c2c_timer_func(unsigned long data)
+{
+	/* Check C2C state */
+	struct exynos_c2c_platdata *pdata = (struct exynos_c2c_platdata *)data;
+	static int old_state = 0xff;
+	int current_state = 0;
+
+	if (pdata->get_c2c_state() != NULL) {
+		current_state = pdata->get_c2c_state();
+		if (current_state != old_state) {
+			dev_info(c2c_con.c2c_dev, "C2C state is chaged (0x%x --> 0x%x)\n",
+				old_state, current_state);
+			old_state = current_state;
+		}
+	}
+	c2c_status_timer.expires = jiffies + (HZ/5);
+	add_timer(&c2c_status_timer);
+}
+#endif
+
+void c2c_reset_ops(void)
+{
+	/* This function will be only used for EVT0 or EVT0.1 */
+	u32 set_clk = 0;
+
+	if (c2c_con.opp_mode == C2C_OPP100)
+		set_clk = c2c_con.clk_opp100;
+	else if (c2c_con.opp_mode == C2C_OPP50)
+		set_clk = c2c_con.clk_opp50;
+	else if (c2c_con.opp_mode == C2C_OPP25)
+		set_clk = c2c_con.clk_opp25;
+
+	dev_info(c2c_con.c2c_dev, "c2c_reset_ops()\n");
+	clk_set_rate(c2c_con.c2c_sclk, (set_clk + 1) * MHZ);
+	c2c_set_func_clk(set_clk);
+
+	/* First phase - C2C block reset */
+	c2c_set_reset(C2C_CLEAR);
+	c2c_set_reset(C2C_SET);
+	/* Second phase - Clear clock gating */
+	c2c_set_clock_gating(C2C_CLEAR);
+	/* Third phase - Retention reg */
+	c2c_writel(c2c_con.retention_reg, EXYNOS_C2C_IRQ_EN_SET1);
+	c2c_writel(set_clk, EXYNOS_C2C_FCLK_FREQ);
+	c2c_writel(set_clk, EXYNOS_C2C_RX_MAX_FREQ);
+	/* Last phase - Set clock gating */
+	c2c_set_clock_gating(C2C_SET);
+}
+
+static ssize_t c2c_ctrl_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	int ret = 0;
+	ret = sprintf(buf, "C2C State");
+	c2c_set_clock_gating(C2C_CLEAR);
+	ret += sprintf(&buf[ret], "SysReg : 0x%x\n",
+			readl(c2c_con.c2c_sysreg));
+	ret += sprintf(&buf[ret], "Port Config : 0x%x\n",
+			c2c_readl(EXYNOS_C2C_PORTCONFIG));
+	ret += sprintf(&buf[ret], "FCLK_FREQ : %d\n",
+			c2c_readl(EXYNOS_C2C_FCLK_FREQ));
+	ret += sprintf(&buf[ret], "RX_MAX_FREQ : %d\n",
+			c2c_readl(EXYNOS_C2C_RX_MAX_FREQ));
+	ret += sprintf(&buf[ret], "IRQ_EN_SET1 : 0x%x\n",
+			c2c_readl(EXYNOS_C2C_IRQ_EN_SET1));
+	ret += sprintf(&buf[ret], "Get C2C sclk rate : %ld\n",
+			clk_get_rate(c2c_con.c2c_sclk));
+	ret += sprintf(&buf[ret], "Get C2C aclk rate : %ld\n",
+			clk_get_rate(c2c_con.c2c_aclk));
+	c2c_set_clock_gating(C2C_SET);
+
+	return ret;
+}
+
+static ssize_t c2c_ctrl_store(struct device *dev, struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	int ops_num, opp_val, req_clk;
+	sscanf(buf, "%d", &ops_num);
+
+	switch (ops_num) {
+	case 1:
+		c2c_reset_ops();
+		break;
+	case 2:
+	case 3:
+	case 4:
+		opp_val = ops_num - 1;
+		req_clk = 0;
+		dev_info(c2c_con.c2c_dev, "Set current OPP mode (%d)\n", opp_val);
+
+		if (opp_val == C2C_OPP100)
+			req_clk = c2c_con.clk_opp100;
+		else if (opp_val == C2C_OPP50)
+			req_clk = c2c_con.clk_opp50;
+		else if (opp_val == C2C_OPP25)
+			req_clk = c2c_con.clk_opp25;
+
+		if (opp_val == 0 || req_clk == 1) {
+			dev_info(c2c_con.c2c_dev, "This mode is not reserved in OPP mode.\n");
+		} else {
+			c2c_set_clock_gating(C2C_CLEAR);
+			if (c2c_con.opp_mode < opp_val) { /* increase case */
+				clk_set_rate(c2c_con.c2c_sclk, (req_clk + 1) * MHZ);
+				c2c_writel(req_clk, EXYNOS_C2C_FCLK_FREQ);
+				c2c_set_func_clk(req_clk);
+				c2c_writel(req_clk, EXYNOS_C2C_RX_MAX_FREQ);
+			} else if (c2c_con.opp_mode > opp_val) { /* decrease case */
+				c2c_writel(req_clk, EXYNOS_C2C_RX_MAX_FREQ);
+				clk_set_rate(c2c_con.c2c_sclk, (req_clk + 1) * MHZ);
+				c2c_writel(req_clk, EXYNOS_C2C_FCLK_FREQ);
+				c2c_set_func_clk(req_clk);
+			} else{
+				dev_info(c2c_con.c2c_dev, "Requested same OPP mode\n");
+			}
+			c2c_con.opp_mode = opp_val;
+			c2c_set_clock_gating(C2C_SET);
+		}
+
+		dev_info(c2c_con.c2c_dev, "Get C2C sclk rate : %ld\n",
+					clk_get_rate(c2c_con.c2c_sclk));
+		dev_info(c2c_con.c2c_dev, "Get C2C aclk rate : %ld\n",
+					clk_get_rate(c2c_con.c2c_aclk));
+		break;
+	default:
+		dev_info(c2c_con.c2c_dev, "Wrong C2C operation number\n");
+		dev_info(c2c_con.c2c_dev, "---C2C Operation Number---\n");
+		dev_info(c2c_con.c2c_dev, "1. C2C Reset\n");
+		dev_info(c2c_con.c2c_dev, "2. Set OPP25\n");
+		dev_info(c2c_con.c2c_dev, "3. Set OPP50\n");
+		dev_info(c2c_con.c2c_dev, "4. Set OPP100\n");
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(c2c_ctrl, 0644, c2c_ctrl_show, c2c_ctrl_store);
+
+int c2c_open(struct inode *inode, struct file *filp)
+{
+	/* This function is not needed.(Test Function) */
+	dev_info(c2c_con.c2c_dev, "C2C chrdrv Opened.\n");
+
+	return 0;
+}
+
+static long c2c_ioctl(struct file *filp, unsigned int cmd,
+		unsigned long arg)
+{
+	c2c_reset_ops();
+
+	return 0;
+}
+
+static const struct file_operations c2c_fops = {
+	.owner	= THIS_MODULE,
+	.unlocked_ioctl	= c2c_ioctl,
+	.open	= c2c_open,
+};
+
+static struct miscdevice char_dev = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	.name	= C2C_DEV_NAME,
+	.fops	= &c2c_fops
+};
+
+static int c2c_set_sharedmem(enum c2c_shrdmem_size size, u32 addr)
+{
+	dev_info(c2c_con.c2c_dev, "Set BaseAddr(0x%x) and Size(%d)\n",
+				addr, 1 << (2 + size));
+
+	/* Set DRAM Base Addr & Size */
+	c2c_set_shdmem_size(size);
+	c2c_set_base_addr((addr >> 22));
+
+	return 0;
+}
+
+static void c2c_set_interrupt(u32 genio_num, enum c2c_interrupt set_int)
+{
+	u32 cur_int_reg, cur_lev_reg;
+
+	cur_int_reg = c2c_readl(EXYNOS_C2C_GENO_INT);
+	cur_lev_reg = c2c_readl(EXYNOS_C2C_GENO_LEVEL);
+
+	switch (set_int) {
+	case C2C_INT_TOGGLE:
+		cur_int_reg &= ~(0x1 << genio_num);
+		c2c_writel(cur_int_reg, EXYNOS_C2C_GENO_INT);
+		break;
+	case C2C_INT_HIGH:
+		cur_int_reg |= (0x1 << genio_num);
+		cur_lev_reg |= (0x1 << genio_num);
+		c2c_writel(cur_int_reg, EXYNOS_C2C_GENO_INT);
+		c2c_writel(cur_lev_reg, EXYNOS_C2C_GENO_LEVEL);
+		break;
+	case C2C_INT_LOW:
+		cur_int_reg |= (0x1 << genio_num);
+		cur_lev_reg &= ~(0x1 << genio_num);
+		c2c_writel(cur_int_reg, EXYNOS_C2C_GENO_INT);
+		c2c_writel(cur_lev_reg, EXYNOS_C2C_GENO_LEVEL);
+		break;
+	}
+}
+
+static irqreturn_t c2c_sscm0_irq(int irq, void *data)
+{
+	/* TODO : This function will be used other type boards */
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t c2c_sscm1_irq(int irq, void *data)
+{
+	/* TODO : It is just temporary code. It will be modified. */
+	u32 raw_irq, latency_val, opp_val, req_clk;
+	raw_irq = c2c_readl(EXYNOS_C2C_IRQ_EN_STAT1);
+
+#ifdef CONFIG_C2C_IPC_ENABLE
+	if (raw_irq & 0x1) {
+		dev_info(c2c_con.c2c_dev, "IPC interrupt occured : GENO[0]\n");
+		if (c2c_con.hd.handler)
+			c2c_con.hd.handler(c2c_con.hd.data);
+
+		/* Interrupt Clear */
+		c2c_writel(0x1, EXYNOS_C2C_IRQ_EN_STAT1);
+	}
+#endif
+	if ((raw_irq >> C2C_GENIO_OPP_INT) & 1) { /* OPP Change */
+		/*
+		    OPP mode GENI/O bit definition[29:27]
+		    OPP100 GENI/O[29:28] : 1 1
+		    OPP50 GENI/O[29:28] : 1 0
+		    OPP25 GENI/O[29:28] : 0 1
+		    GENI[27] is only used for making interrupt.
+		*/
+		opp_val = (c2c_readl(EXYNOS_C2C_GENO_STATUS) >> 28) & 3;
+		req_clk = 0;
+		dev_info(c2c_con.c2c_dev, "OPP interrupt occured (%d)\n", opp_val);
+
+		if (opp_val == C2C_OPP100)
+			req_clk = c2c_con.clk_opp100;
+		else if (opp_val == C2C_OPP50)
+			req_clk = c2c_con.clk_opp50;
+		else if (opp_val == C2C_OPP25)
+			req_clk = c2c_con.clk_opp25;
+
+		if (opp_val == 0 || req_clk == 1) {
+			dev_info(c2c_con.c2c_dev, "This mode is not reserved in OPP mode.\n");
+		} else {
+			if (c2c_con.opp_mode < opp_val) { /* increase case */
+				clk_set_rate(c2c_con.c2c_sclk, (req_clk + 1) * MHZ);
+				c2c_writel(req_clk, EXYNOS_C2C_FCLK_FREQ);
+				c2c_set_func_clk(req_clk);
+				c2c_writel(req_clk, EXYNOS_C2C_RX_MAX_FREQ);
+			} else if (c2c_con.opp_mode > opp_val) { /* decrease case */
+				c2c_writel(req_clk, EXYNOS_C2C_RX_MAX_FREQ);
+				clk_set_rate(c2c_con.c2c_sclk, (req_clk + 1) * MHZ);
+				c2c_writel(req_clk, EXYNOS_C2C_FCLK_FREQ);
+				c2c_set_func_clk(req_clk);
+			} else{
+				dev_info(c2c_con.c2c_dev, "Requested same OPP mode\n");
+			}
+			c2c_con.opp_mode = opp_val;
+		}
+
+		/* Interrupt Clear */
+		c2c_writel((0x1 << C2C_GENIO_OPP_INT), EXYNOS_C2C_IRQ_EN_STAT1);
+	}
+
+	/* Memory I/F latency change */
+	if ((raw_irq >> C2C_GENIO_LATENCY_INT) & 1) {
+		latency_val = (c2c_readl(EXYNOS_C2C_GENO_STATUS) >> 30) & 3;
+		switch (latency_val) {
+		case 3:
+			dev_info(c2c_con.c2c_dev, "Set Min latency\n");
+			if (exynos_c2c_request_pwr_mode != NULL)
+				exynos_c2c_request_pwr_mode(MIN_LATENCY);
+			break;
+		case 1:
+			dev_info(c2c_con.c2c_dev, "Set Short latency\n");
+			if (exynos_c2c_request_pwr_mode != NULL)
+				exynos_c2c_request_pwr_mode(SHORT_LATENCY);
+			break;
+		case 0:
+			dev_info(c2c_con.c2c_dev, "Set Max latency\n");
+			if (exynos_c2c_request_pwr_mode != NULL)
+				exynos_c2c_request_pwr_mode(MAX_LATENCY);
+			break;
+		}
+		/* Interrupt Clear */
+		c2c_writel((0x1 << C2C_GENIO_LATENCY_INT), EXYNOS_C2C_IRQ_EN_STAT1);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void set_c2c_device(struct platform_device *pdev)
+{
+	struct exynos_c2c_platdata *pdata = pdev->dev.platform_data;
+	u32 default_clk;
+
+	c2c_con.c2c_sysreg = pdata->c2c_sysreg;
+	c2c_con.rx_width = pdata->rx_width;
+	c2c_con.tx_width = pdata->tx_width;
+	c2c_con.clk_opp100 = pdata->clk_opp100;
+	c2c_con.clk_opp50 = pdata->clk_opp50;
+	c2c_con.clk_opp25 = pdata->clk_opp25;
+	c2c_con.opp_mode = pdata->default_opp_mode;
+#ifdef CONFIG_C2C_IPC_ENABLE
+	c2c_con.shd_pages = NULL;
+	c2c_con.hd.data = NULL;
+	c2c_con.hd.handler = NULL;
+#endif
+	c2c_con.c2c_sclk = clk_get(&pdev->dev, "sclk_c2c");
+	c2c_con.c2c_aclk = clk_get(&pdev->dev, "aclk_c2c");
+
+	if (soc_is_exynos4212())
+		exynos_c2c_request_pwr_mode = exynos4_c2c_request_pwr_mode;
+	else if (soc_is_exynos4412()) {
+		exynos_c2c_request_pwr_mode = exynos4_c2c_request_pwr_mode;
+		if (samsung_rev() >= EXYNOS4412_REV_1_0)
+			writel(C2C_SYSREG_DEFAULT, c2c_con.c2c_sysreg);
+	} else if (soc_is_exynos5250())
+		exynos_c2c_request_pwr_mode = NULL;
+
+	/* Set clock to default mode */
+	if (c2c_con.opp_mode == C2C_OPP100)
+		default_clk = c2c_con.clk_opp100;
+	else if (c2c_con.opp_mode == C2C_OPP50)
+		default_clk = c2c_con.clk_opp50;
+	else if (c2c_con.opp_mode == C2C_OPP25)
+		default_clk = c2c_con.clk_opp25;
+	else {
+		dev_info(c2c_con.c2c_dev, "Default OPP mode is not selected.\n");
+		c2c_con.opp_mode = C2C_OPP50;
+		default_clk = c2c_con.clk_opp50;
+	}
+
+	clk_set_rate(c2c_con.c2c_sclk, (default_clk + 1)  * MHZ);
+	clk_set_rate(c2c_con.c2c_aclk, ((default_clk / 2) + 1) * MHZ);
+
+	dev_info(c2c_con.c2c_dev, "Get C2C sclk rate : %ld\n",
+				clk_get_rate(c2c_con.c2c_sclk));
+	dev_info(c2c_con.c2c_dev, "Get C2C aclk rate : %ld\n",
+				clk_get_rate(c2c_con.c2c_aclk));
+	if (pdata->setup_gpio)
+		pdata->setup_gpio(pdata->rx_width, pdata->tx_width);
+
+	c2c_set_sharedmem(pdata->shdmem_size, pdata->shdmem_addr);
+
+	/* Set SYSREG to memdone */
+	c2c_set_memdone(C2C_SET);
+	c2c_set_clock_gating(C2C_CLEAR);
+
+	/* Set C2C clock register to OPP50 */
+	c2c_writel(default_clk, EXYNOS_C2C_FCLK_FREQ);
+	c2c_writel(default_clk, EXYNOS_C2C_RX_MAX_FREQ);
+	c2c_set_func_clk(default_clk);
+
+	/* Set C2C buswidth */
+	c2c_writel(((pdata->rx_width << 4) | (pdata->tx_width)),
+					EXYNOS_C2C_PORTCONFIG);
+	c2c_set_tx_buswidth(pdata->tx_width);
+	c2c_set_rx_buswidth(pdata->rx_width);
+
+	/* Enable all of GENI/O Interrupt */
+	c2c_writel((0x1 << C2C_GENIO_OPP_INT), EXYNOS_C2C_IRQ_EN_SET1);
+	c2c_con.retention_reg = (0x1 << C2C_GENIO_OPP_INT);
+
+	if (exynos_c2c_request_pwr_mode != NULL)
+		exynos_c2c_request_pwr_mode(MAX_LATENCY);
+
+	c2c_set_interrupt(C2C_GENIO_OPP_INT, C2C_INT_HIGH);
+
+	dev_info(c2c_con.c2c_dev, "Port Config : 0x%x\n",
+				c2c_readl(EXYNOS_C2C_PORTCONFIG));
+	dev_info(c2c_con.c2c_dev, "FCLK_FREQ register : %d\n",
+				c2c_readl(EXYNOS_C2C_FCLK_FREQ));
+	dev_info(c2c_con.c2c_dev, "RX_MAX_FREQ register : %d\n",
+				c2c_readl(EXYNOS_C2C_RX_MAX_FREQ));
+	dev_info(c2c_con.c2c_dev, "IRQ_EN_SET1 register : 0x%x\n",
+				c2c_readl(EXYNOS_C2C_IRQ_EN_SET1));
+
+	c2c_set_clock_gating(C2C_SET);
+}
+
+#ifdef CONFIG_C2C_IPC_ENABLE
+void __iomem *c2c_request_cp_region(unsigned int cp_addr,
+		unsigned int size)
+{
+	dma_addr_t phy_cpmem;
+
+	phy_cpmem = cma_alloc(c2c_con.c2c_dev, "c2c_shdmem", size, 0);
+	if (IS_ERR_VALUE(phy_cpmem)) {
+		dev_info(c2c_con.c2c_dev, KERN_ERR "C2C CMA Alloc Error!!!");
+		return NULL;
+	}
+
+	return phys_to_virt(phy_cpmem);
+}
+EXPORT_SYMBOL(c2c_request_cp_region);
+
+void c2c_release_cp_region(void *rgn)
+{
+	dma_addr_t phy_cpmem;
+
+	phy_cpmem = virt_to_phys(rgn);
+
+	cma_free(phy_cpmem);
+}
+EXPORT_SYMBOL(c2c_release_cp_region);
+
+void __iomem *c2c_request_sh_region(unsigned int sh_addr,
+		unsigned int size)
+{
+	int i;
+	struct page **pages;
+	void *pv;
+
+	pages = kmalloc((size >> PAGE_SHIFT) * sizeof(*pages), GFP_KERNEL);
+	for (i = 0; i < (size >> PAGE_SHIFT); i++) {
+		pages[i] = phys_to_page(sh_addr);
+		sh_addr += PAGE_SIZE;
+	}
+
+	c2c_con.shd_pages = (void *)pages;
+
+	pv = vmap(pages, size >> PAGE_SHIFT, VM_MAP,
+					pgprot_noncached(PAGE_KERNEL));
+
+	return (void __iomem *)pv;
+}
+EXPORT_SYMBOL(c2c_request_sh_region);
+
+void c2c_release_sh_region(void *rgn)
+{
+	vunmap(rgn);
+	kfree(c2c_con.shd_pages);
+	c2c_con.shd_pages = NULL;
+}
+EXPORT_SYMBOL(c2c_release_sh_region);
+
+int c2c_register_handler(void (*handler)(void *), void *data)
+{
+	if (!handler)
+		return -EINVAL;
+
+	c2c_con.hd.data = data;
+	c2c_con.hd.handler = handler;
+
+	c2c_reset_interrupt();
+
+	return 0;
+}
+EXPORT_SYMBOL(c2c_register_handler);
+
+int c2c_unregister_handler(void (*handler)(void *))
+{
+	if (!handler || (c2c_con.hd.handler != handler))
+		return -EINVAL;
+
+	c2c_con.hd.data = NULL;
+	c2c_con.hd.handler = NULL;
+	return 0;
+}
+EXPORT_SYMBOL(c2c_unregister_handler);
+
+void c2c_send_interrupt(void)
+{
+	c2c_writel(c2c_readl(EXYNOS_C2C_GENI_CONTROL) ^ 0x1,
+			EXYNOS_C2C_GENI_CONTROL);
+}
+EXPORT_SYMBOL(c2c_send_interrupt);
+
+void c2c_reset_interrupt(void)
+{
+	c2c_writel(c2c_readl(EXYNOS_C2C_IRQ_EN_SET1) | 0x1,
+			EXYNOS_C2C_IRQ_EN_SET1);
+	c2c_con.retention_reg |= 0x1;
+}
+EXPORT_SYMBOL(c2c_reset_interrupt);
+#endif
+
+static int __devinit samsung_c2c_probe(struct platform_device *pdev)
+{
+	struct exynos_c2c_platdata *pdata = pdev->dev.platform_data;
+	struct resource *res = NULL;
+	struct resource *res1 = NULL;
+	int sscm_irq0, sscm_irq1;
+	int err = 0;
+
+	c2c_con.c2c_dev = &pdev->dev;
+
+	/* resource for AP's SSCM region */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no memory resource defined(AP's SSCM)\n");
+		return -ENOENT;
+	}
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (!res) {
+		dev_err(&pdev->dev, "failded to request memory resource(AP)\n");
+		return -ENOENT;
+	}
+	pdata->ap_sscm_addr = ioremap(res->start, resource_size(res));
+	if (!pdata->ap_sscm_addr) {
+		dev_err(&pdev->dev, "failded to request memory resource(AP)\n");
+		goto release_ap_sscm;
+	}
+	c2c_con.ap_sscm_addr = pdata->ap_sscm_addr;
+
+	/* resource for CP's SSCM region */
+	res1 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res1) {
+		dev_err(&pdev->dev, "no memory resource defined(AP's SSCM)\n");
+		goto unmap_ap_sscm;
+	}
+	res1 = request_mem_region(res1->start, resource_size(res1), pdev->name);
+	if (!res1) {
+		dev_err(&pdev->dev, "failded to request memory resource(AP)\n");
+		goto unmap_ap_sscm;
+	}
+	pdata->cp_sscm_addr = ioremap(res1->start, resource_size(res1));
+	if (!pdata->cp_sscm_addr) {
+		dev_err(&pdev->dev, "failded to request memory resource(CP)\n");
+		goto release_cp_sscm;
+	}
+	c2c_con.cp_sscm_addr = pdata->cp_sscm_addr;
+
+	/* Request IRQ */
+	sscm_irq0 = platform_get_irq(pdev, 0);
+	if (sscm_irq0 < 0) {
+		dev_err(&pdev->dev, "no irq specified\n");
+		goto unmap_cp_sscm;
+	}
+	err = request_irq(sscm_irq0, c2c_sscm0_irq, 0, pdev->name, pdev);
+	if (err) {
+		dev_err(&pdev->dev, "Can't request SSCM0 IRQ\n");
+		goto unmap_cp_sscm;
+	}
+	/* SSCM0 irq will be only used for master(CP) device */
+	disable_irq(sscm_irq0);
+
+	sscm_irq1 = platform_get_irq(pdev, 1);
+	if (sscm_irq1 < 0) {
+		dev_err(&pdev->dev, "no irq specified\n");
+		goto release_sscm_irq0;
+	}
+	err = request_irq(sscm_irq1, c2c_sscm1_irq, 1, pdev->name, pdev);
+	if (err) {
+		dev_err(&pdev->dev, "Can't request SSCM1 IRQ\n");
+		goto release_sscm_irq0;
+	}
+
+	err = misc_register(&char_dev);
+	if (err) {
+		dev_err(&pdev->dev, "Can't register chrdev!\n");
+		goto release_sscm_irq0;
+	}
+
+	set_c2c_device(pdev);
+
+#ifdef ENABLE_C2CSTATE_TIMER
+	/* Timer for debugging to check C2C state */
+	init_timer(&c2c_status_timer);
+	c2c_status_timer.expires = jiffies + HZ;
+	c2c_status_timer.data = (unsigned long)pdata;
+	c2c_status_timer.function = &c2c_timer_func;
+	add_timer(&c2c_status_timer);
+#endif
+
+	/* Create sysfs file for C2C debug */
+	err = device_create_file(&pdev->dev, &dev_attr_c2c_ctrl);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to create sysfs for C2C\n");
+		goto release_sscm_irq1;
+	}
+
+	return 0;
+
+release_sscm_irq1:
+	free_irq(sscm_irq1, pdev);
+
+release_sscm_irq0:
+	free_irq(sscm_irq0, pdev);
+
+unmap_cp_sscm:
+	iounmap(pdata->cp_sscm_addr);
+
+release_cp_sscm:
+	release_mem_region(res1->start, resource_size(res1));
+
+unmap_ap_sscm:
+	iounmap(pdata->ap_sscm_addr);
+
+release_ap_sscm:
+	release_mem_region(res->start, resource_size(res));
+
+	return err;
+}
+
+static int __devexit samsung_c2c_remove(struct platform_device *pdev)
+{
+	/* TODO */
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int samsung_c2c_suspend(struct platform_device *dev, pm_message_t pm)
+{
+	/* TODO */
+	return 0;
+}
+
+static int samsung_c2c_resume(struct platform_device *dev)
+{
+	struct exynos_c2c_platdata *pdata = dev->dev.platform_data;
+
+	if ((soc_is_exynos4212() || soc_is_exynos4412())
+		&& samsung_rev() == EXYNOS4412_REV_0) {
+		/* Set SYSREG */
+		c2c_set_sharedmem(pdata->shdmem_size, pdata->shdmem_addr);
+		c2c_set_memdone(C2C_SET);
+	} else if (soc_is_exynos5250()) {
+		/* Set SYSREG */
+		c2c_set_sharedmem(pdata->shdmem_size, pdata->shdmem_addr);
+		c2c_set_memdone(C2C_SET);
+	}
+
+	return 0;
+}
+#else
+#define samsung_c2c_suspend NULL
+#define samsung_c2c_resume NULL
+#endif
+
+static struct platform_driver samsung_c2c_driver = {
+	.probe		= samsung_c2c_probe,
+	.remove		= __devexit_p(samsung_c2c_remove),
+	.suspend	= samsung_c2c_suspend,
+	.resume		= samsung_c2c_resume,
+	.driver		= {
+		.name	= "samsung-c2c",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init samsung_c2c_init(void)
+{
+	return platform_driver_register(&samsung_c2c_driver);
+}
+module_init(samsung_c2c_init);
+
+static void __exit samsung_c2c_exit(void)
+{
+	platform_driver_unregister(&samsung_c2c_driver);
+}
+module_exit(samsung_c2c_exit);
+
+MODULE_DESCRIPTION("Samsung C2C driver");
+MODULE_AUTHOR("Kisang Lee <kisang80.lee@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/c2c/samsung-c2c.h b/drivers/misc/c2c/samsung-c2c.h
new file mode 100644
index 0000000..03ea601
--- /dev/null
+++ b/drivers/misc/c2c/samsung-c2c.h
@@ -0,0 +1,334 @@
+/*
+ * Samsung C2C driver
+ *
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Author: Kisang Lee <kisang80.lee@samsung.com>
+ *
+ * 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.
+ */
+#ifndef SAMSUNG_C2C_H
+#define SAMSUNG_C2C_H
+
+/* This timer will be only used for debugging
+#define ENABLE_C2CSTATE_TIMER
+*/
+#define C2C_DEV_NAME "c2c_dev"
+#define C2C_SYSREG_DEFAULT 0x832AA803
+
+#ifdef CONFIG_C2C_IPC_ENABLE
+#define C2C_CP_RGN_ADDR		0x60000000
+#define C2C_CP_RGN_SIZE		(56 * SZ_1M)
+#define C2C_SH_RGN_ADDR		(C2C_CP_RGN_ADDR + C2C_CP_RGN_SIZE)
+#define C2C_SH_RGN_SIZE		(8 * SZ_1M)
+
+extern void __iomem *c2c_request_cp_region(unsigned int cp_addr,
+		unsigned int size);
+extern void __iomem *c2c_request_sh_region(unsigned int sh_addr,
+		unsigned int size);
+extern void c2c_release_cp_region(void *rgn);
+extern void c2c_release_sh_region(void *rgn);
+
+extern int c2c_register_handler(void (*handler)(void *), void *data);
+extern int c2c_unregister_handler(void (*handler)(void *));
+extern void c2c_send_interrupt(void);
+extern void c2c_reset_interrupt(void);
+
+struct c2c_ipc_handler {
+	void *data;
+	void (*handler)(void *);
+};
+#endif
+
+enum c2c_set_clear {
+	C2C_CLEAR = 0,
+	C2C_SET = 1,
+};
+
+enum c2c_interrupt {
+	C2C_INT_TOGGLE = 0,
+	C2C_INT_HIGH = 1,
+	C2C_INT_LOW = 2,
+};
+
+struct c2c_state_control {
+	void __iomem *ap_sscm_addr;
+	void __iomem *cp_sscm_addr;
+#ifdef CONFIG_C2C_IPC_ENABLE
+	void *shd_pages;
+	struct c2c_ipc_handler hd;
+#endif
+	struct device *c2c_dev;
+
+	u32 rx_width;
+	u32 tx_width;
+
+	u32 clk_opp100;
+	u32 clk_opp50;
+	u32 clk_opp25;
+
+	struct clk *c2c_sclk;
+	struct clk *c2c_aclk;
+
+	enum c2c_opp_mode opp_mode;
+	/* Below variables are needed in reset for retention */
+	u32 retention_reg;
+	void __iomem *c2c_sysreg;
+};
+
+static struct c2c_state_control c2c_con;
+
+static inline void c2c_writel(u32 val, int reg)
+{
+	writel(val, c2c_con.ap_sscm_addr + reg);
+}
+
+static inline void c2c_writew(u16 val, int reg)
+{
+	writew(val, c2c_con.ap_sscm_addr + reg);
+}
+
+static inline void c2c_writeb(u8 val, int reg)
+{
+	writeb(val, c2c_con.ap_sscm_addr + reg);
+}
+
+static inline u32 c2c_readl(int reg)
+{
+	return readl(c2c_con.ap_sscm_addr + reg);
+}
+
+static inline u16 c2c_readw(int reg)
+{
+	return readw(c2c_con.ap_sscm_addr + reg);
+}
+
+static inline u8 c2c_readb(int reg)
+{
+	return readb(c2c_con.ap_sscm_addr + reg);
+}
+
+static inline void c2c_writel_cp(u32 val, int reg)
+{
+	writel(val, c2c_con.cp_sscm_addr + reg);
+}
+
+static inline void c2c_writew_cp(u16 val, int reg)
+{
+	writew(val, c2c_con.cp_sscm_addr + reg);
+}
+
+static inline void c2c_writeb_cp(u8 val, int reg)
+{
+	writeb(val, c2c_con.cp_sscm_addr + reg);
+}
+
+static inline u32 c2c_readl_cp(int reg)
+{
+	return readl(c2c_con.cp_sscm_addr + reg);
+}
+
+static inline u16 c2c_readw_cp(int reg)
+{
+	return readw(c2c_con.cp_sscm_addr + reg);
+}
+
+static inline u8 c2c_readb_cp(int reg)
+{
+	return readb(c2c_con.cp_sscm_addr + reg);
+}
+
+static inline enum c2c_set_clear c2c_get_clock_gating(void)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+	if (sysreg & (1 << C2C_SYSREG_CG))
+		return C2C_SET;
+	else
+		return C2C_CLEAR;
+}
+
+static inline void c2c_set_clock_gating(enum c2c_set_clear val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	if (val == C2C_SET)
+		sysreg |= (1 << C2C_SYSREG_CG);
+	else
+		sysreg &= ~(1 << C2C_SYSREG_CG);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+static inline enum c2c_set_clear c2c_get_memdone(void)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+	if (sysreg & (1 << C2C_SYSREG_MD))
+		return C2C_SET;
+	else
+		return C2C_CLEAR;
+}
+
+static inline void c2c_set_memdone(enum c2c_set_clear val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	if (val == C2C_SET)
+		sysreg |= (1 << C2C_SYSREG_MD);
+	else
+		sysreg &= ~(1 << C2C_SYSREG_MD);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+static inline enum c2c_set_clear c2c_get_master_on(void)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+	if (sysreg & (1 << C2C_SYSREG_MO))
+		return C2C_SET;
+	else
+		return C2C_CLEAR;
+}
+
+static inline void c2c_set_master_on(enum c2c_set_clear val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	if (val == C2C_SET)
+		sysreg |= (1 << C2C_SYSREG_MO);
+	else
+		sysreg &= ~(1 << C2C_SYSREG_MO);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+static inline u32 c2c_get_func_clk(void)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= (0x3ff << C2C_SYSREG_FCLK);
+
+	return sysreg >> C2C_SYSREG_FCLK;
+}
+
+static inline void c2c_set_func_clk(u32 val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= ~(0x3ff << C2C_SYSREG_FCLK);
+	sysreg |= (val << C2C_SYSREG_FCLK);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+static inline u32 c2c_get_tx_buswidth(void)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= (0x3 << C2C_SYSREG_TXW);
+
+	return sysreg >> C2C_SYSREG_TXW;
+}
+
+static inline void c2c_set_tx_buswidth(u32 val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= ~(0x3 << C2C_SYSREG_TXW);
+	sysreg |= (val << C2C_SYSREG_TXW);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+static inline u32 c2c_get_rx_buswidth(void)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= (0x3 << C2C_SYSREG_RXW);
+
+	return sysreg >> C2C_SYSREG_RXW;
+}
+
+static inline void c2c_set_rx_buswidth(u32 val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= ~(0x3 << C2C_SYSREG_RXW);
+	sysreg |= (val << C2C_SYSREG_RXW);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+static inline enum c2c_set_clear c2c_get_reset(void)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+	if (sysreg & (1 << C2C_SYSREG_RST))
+		return C2C_SET;
+	else
+		return C2C_CLEAR;
+}
+
+static inline void c2c_set_reset(enum c2c_set_clear val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	if (val == C2C_SET)
+		sysreg |= (1 << C2C_SYSREG_RST);
+	else
+		sysreg &= ~(1 << C2C_SYSREG_RST);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+static inline void c2c_set_rtrst(enum c2c_set_clear val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	if (val == C2C_SET)
+		sysreg |= (1 << C2C_SYSREG_RTRST);
+	else
+		sysreg &= ~(1 << C2C_SYSREG_RTRST);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+static inline u32 c2c_get_base_addr(void)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= (0x3ff << C2C_SYSREG_BASE_ADDR);
+
+	return sysreg >> C2C_SYSREG_BASE_ADDR;
+}
+
+static inline void c2c_set_base_addr(u32 val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= ~(0x3ff << C2C_SYSREG_BASE_ADDR);
+	sysreg |= (val << C2C_SYSREG_BASE_ADDR);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+static inline u32 c2c_get_shdmem_size(void)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= (0x7 << C2C_SYSREG_DRAM_SIZE);
+
+	return sysreg >> C2C_SYSREG_DRAM_SIZE;
+}
+
+static inline void c2c_set_shdmem_size(u32 val)
+{
+	u32 sysreg = readl(c2c_con.c2c_sysreg);
+
+	sysreg &= ~(0x7 << C2C_SYSREG_DRAM_SIZE);
+	sysreg |= (val << C2C_SYSREG_DRAM_SIZE);
+
+	writel(sysreg, c2c_con.c2c_sysreg);
+}
+
+#endif
diff --git a/drivers/misc/cw_tty.c b/drivers/misc/cw_tty.c
new file mode 100644
index 0000000..4c6367b
--- /dev/null
+++ b/drivers/misc/cw_tty.c
@@ -0,0 +1,368 @@
+/* linux/arch/arm/mach-xxxx/board-c1ctc-modems.c
+ * Copyright (C) 2010 Samsung Electronics. All rights reserved.
+ *
+ * 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/errno.h>
+#include <linux/signal.h>
+#include <linux/sched.h>
+#include <linux/timer.h>
+#include <linux/interrupt.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/major.h>
+#include <linux/string.h>
+#include <linux/fcntl.h>
+#include <linux/mm.h>
+#include <linux/kernel.h>
+#include <linux/console.h>
+#include <linux/reboot.h>
+#include <linux/keyboard.h>
+#include <linux/init.h>
+#include <linux/pm.h>
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/gfp.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+
+#include <linux/time.h>
+#include <linux/slab.h>
+#include <linux/unistd.h>
+
+#define ID_CW_TTY_CWPPP			36
+#define ID_CW_TTY_CWXAN			41
+
+#define CW_TTY_MAJOR_NUM		235
+
+/* Maximum PDP data length */
+#define MAX_PDP_DATA_LEN		1500
+
+/* Maximum PDP packet length including header and start/stop bytes */
+#define MAX_PDP_PACKET_LEN		(MAX_PDP_DATA_LEN + 4 + 2)
+
+#define MAX_CW_TTY_BUFFER_LEN		(8192*2)
+
+struct cw_tty_info {
+	/* PDP context ID */
+	u8			id;
+
+	/* Tx packet buffer */
+	u8			tx_buf[MAX_CW_TTY_BUFFER_LEN];
+
+	/* App device interface */
+	union {
+		/* Virtual serial interface */
+		struct {
+			struct tty_driver	tty_driver[2];
+			int			refcount;
+			struct tty_struct	*tty_table[1];
+			struct ktermios		*termios[1];
+			struct ktermios		*termios_locked[1];
+			char			tty_name[64];
+			struct tty_struct	*tty;
+			struct semaphore	write_lock;
+		} vs_u;
+	} dev_u;
+#define vs_dev		dev_u.vs_u
+};
+
+struct cw_tty_info *cw_tty[2];
+
+struct cw_tty_info *cw_tty_get_dev_by_id(int id)
+{
+	struct cw_tty_info *dev;
+
+	if (id == ID_CW_TTY_CWPPP)
+		dev = cw_tty[0];
+	else if (id == ID_CW_TTY_CWXAN)
+		dev = cw_tty[1];
+	else
+		dev = NULL;
+
+	return dev;
+}
+
+struct cw_tty_info *cw_tty_get_dev_by_name(const char *name)
+{
+	struct cw_tty_info *dev;
+
+	if (strcmp("ttyCWPPP", name) == 0)
+		dev = cw_tty[0];
+	else if (strcmp("ttyCWXAN", name) == 0)
+		dev = cw_tty[1];
+	else
+		dev = NULL;
+
+	return dev;
+}
+
+/* Added by Venkatesh GR SISO Received PPP Data is sent to ppp */
+static int XanCwReceivedWiFiData(int nDestChannelID,
+				u8 *pi8_RecvdBuf, int nRecvdBuflen)
+{
+	int ret = 0;
+	struct cw_tty_info *dev = NULL;
+
+	if (pi8_RecvdBuf == NULL) {
+		pr_err("[%s] input buffer is NULL\n", __func__);
+		return 0;
+	}
+
+	dev = cw_tty_get_dev_by_id(nDestChannelID);
+	if (dev == NULL) {
+		pr_err("[%s] cw_tty_get_dev_by_id is NULL\n", __func__);
+		return 0;
+	}
+
+	if (dev->vs_dev.tty != NULL && dev->vs_dev.refcount) {
+		pr_info("[%s] name: %s, len: %d\n", __func__,
+					dev->vs_dev.tty_name, nRecvdBuflen);
+
+		ret = tty_insert_flip_string(dev->vs_dev.tty,
+					(u8 *)pi8_RecvdBuf, nRecvdBuflen);
+		if (ret < nRecvdBuflen)
+			pr_err("[%s] tty_insert_flip_string fail %d\n",
+								__func__, ret);
+
+		tty_flip_buffer_push(dev->vs_dev.tty);
+	} else
+		pr_err("[%s] refcount: %d\n", __func__,
+						dev->vs_dev.refcount);
+
+	return ret;
+}
+
+/* Added by Venkatesh GR SISO Received PPP Data from pppd to Xanadu */
+static int XanCwSendToXanadu(int nSrcChannelID,
+		int nDestChannelID, u8 *pi8_RecvdBuf, int nRecvdBuflen)
+{
+	int ret = 0;
+	struct cw_tty_info *dev = NULL;
+	int nXanChannelID = nDestChannelID;
+
+	if (pi8_RecvdBuf == NULL) {
+		pr_err("[%s] input buffer is NULL\n", __func__);
+		return 0;
+	}
+
+	dev = cw_tty_get_dev_by_id(nXanChannelID);
+	if (dev == NULL) {
+		pr_err("[%s] cw_tty_get_dev_by_id is NULL\n", __func__);
+		return 0;
+	}
+
+	if (dev->vs_dev.tty != NULL && dev->vs_dev.refcount) {
+		pr_info("[%s] name: %s, len: %d\n", __func__,
+					dev->vs_dev.tty_name, nRecvdBuflen);
+
+		ret = tty_insert_flip_string(dev->vs_dev.tty,
+					(u8 *)pi8_RecvdBuf, nRecvdBuflen);
+		if (ret < nRecvdBuflen)
+			pr_err("[%s] tty_insert_flip_string returned %d\n",
+								__func__, ret);
+
+		tty_flip_buffer_push(dev->vs_dev.tty);
+	} else
+		pr_err("[%s] refcount: %d\n", __func__,
+						dev->vs_dev.refcount);
+
+	return ret;
+}
+
+
+static int cw_tty_open(struct tty_struct *tty, struct file *filp)
+{
+	struct cw_tty_info *dev;
+
+	pr_info("[%s] name: %s, current: %s\n", __func__,
+					tty->driver->name, current->comm);
+
+	dev = cw_tty_get_dev_by_name(tty->driver->name);
+	if (dev == NULL) {
+		pr_err("[%s] cw_tty_get_dev_by_name is NULL\n", __func__);
+		return -ENODEV;
+	}
+
+	tty->driver_data = (void *)dev;
+	tty->low_latency = 1;
+	dev->vs_dev.tty = tty;
+	dev->vs_dev.refcount++;
+
+	pr_info("[%s] name:%s, refcount: %d\n", __func__,
+				tty->driver->name, dev->vs_dev.refcount);
+
+	return 0;
+}
+
+static void cw_tty_close(struct tty_struct *tty, struct file *filp)
+{
+	struct cw_tty_info *dev;
+
+	dev = cw_tty_get_dev_by_name(tty->driver->name);
+	if (dev == NULL) {
+		pr_err("[%s] cw_tty_get_dev_by_name is NULL\n", __func__);
+		return;
+	}
+
+	dev->vs_dev.refcount--;
+	pr_info("[%s] name: %s, refcount: %d\n", __func__,
+				tty->driver->name, dev->vs_dev.refcount);
+
+	return;
+}
+
+static int cw_tty_write(struct tty_struct *tty,
+			const unsigned char *buf, int count)
+{
+	int ret = 0;
+	struct cw_tty_info *dev = (struct cw_tty_info *)tty->driver_data;
+	int nDestChannelID = 0;
+
+	/* Added by Venkatesh GR, SISO For CDMA+WiFi requirements -- Start*/
+	if (strcmp(dev->vs_dev.tty_name, "ttyCWPPP") == 0) {
+		pr_info("[%s] Rx PPPD, Tx CWTunnel : len = %d\n",
+							__func__, count);
+
+		nDestChannelID = ID_CW_TTY_CWXAN;
+
+		/* Send the Buffer Received From PPPD to CWTunnel */
+		ret = XanCwSendToXanadu(dev->id, nDestChannelID,
+							(u8 *)buf, count);
+
+	} else if (strcmp(dev->vs_dev.tty_name, "ttyCWXAN") == 0) {
+		pr_info("[%s] Rx CWTunnel, Tx PPPD : len = %d\n",
+							__func__, count);
+
+
+		nDestChannelID = ID_CW_TTY_CWPPP;
+
+		/* Send the Buffer Received From CWTunnel to PPPD */
+		ret = XanCwReceivedWiFiData(nDestChannelID, (u8 *)buf, count);
+	}
+
+	return ret;
+}
+
+static int cw_tty_write_room(struct tty_struct *tty)
+{
+	return MAX_CW_TTY_BUFFER_LEN;
+}
+
+static int cw_tty_chars_in_buffer(struct tty_struct *tty)
+{
+	return 0;
+}
+
+static int cw_tty_ioctl(struct tty_struct *tty,
+		    unsigned int cmd, unsigned long arg)
+{
+	pr_info("[%s] name: %s cmd: %x\n", __func__,
+					tty->driver->name, cmd);
+
+	return -ENOIOCTLCMD;
+}
+
+static const struct tty_operations cw_tty_ops = {
+	.open = cw_tty_open,
+	.close = cw_tty_close,
+	.write = cw_tty_write,
+	.write_room = cw_tty_write_room,
+	.chars_in_buffer = cw_tty_chars_in_buffer,
+	.ioctl = cw_tty_ioctl,
+};
+
+static int __init cw_tty_init(void)
+{
+	struct tty_driver	*tty_driver;
+	struct cw_tty_info  *dev;
+
+	dev = kmalloc(sizeof(struct cw_tty_info)
+			+ MAX_PDP_PACKET_LEN, GFP_KERNEL);
+	if (dev == NULL) {
+		pr_err("[%s] out of memory\n", __func__);
+		return -ENOMEM;
+	}
+	memset(dev, 0, sizeof(struct cw_tty_info));
+
+	dev->id = ID_CW_TTY_CWPPP;
+	strcpy(dev->vs_dev.tty_name, "ttyCWPPP");
+	tty_driver = &dev->vs_dev.tty_driver[0];
+
+	kref_init(&tty_driver->kref);
+
+	tty_driver->magic = TTY_DRIVER_MAGIC;
+	tty_driver->driver_name	= "ttyCW";
+	tty_driver->name = "ttyCWPPP";
+	tty_driver->major = CW_TTY_MAJOR_NUM;
+	tty_driver->minor_start = dev->id;
+	tty_driver->num		= 1;
+	tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	tty_driver->subtype = SERIAL_TYPE_NORMAL;
+	tty_driver->flags = TTY_DRIVER_REAL_RAW;
+	tty_driver->ttys	= dev->vs_dev.tty_table;
+	tty_driver->termios	= dev->vs_dev.termios;
+	tty_driver->termios_locked	= dev->vs_dev.termios_locked;
+	tty_set_operations(tty_driver, &cw_tty_ops);
+
+	if (tty_register_driver(tty_driver)) {
+		put_tty_driver(tty_driver);
+		pr_err("[%s] Couldn't register ttyCWPPP driver\n", __func__);
+		return -ENOMEM;
+	}
+	sema_init(&dev->vs_dev.write_lock, 1);
+	cw_tty[0] = dev;
+
+	dev = kmalloc(sizeof(struct cw_tty_info)
+			+ MAX_PDP_PACKET_LEN, GFP_KERNEL);
+	if (dev == NULL) {
+		pr_err("[%s] out of memory\n", __func__);
+		return -ENOMEM;
+	}
+	memset(dev, 0, sizeof(struct cw_tty_info));
+
+	dev->id = ID_CW_TTY_CWXAN;
+	strcpy(dev->vs_dev.tty_name, "ttyCWXAN");
+	tty_driver = &dev->vs_dev.tty_driver[1];
+
+	kref_init(&tty_driver->kref);
+
+	tty_driver->magic = TTY_DRIVER_MAGIC;
+	tty_driver->driver_name	= "ttyCW";
+	tty_driver->name = "ttyCWXAN";
+	tty_driver->major = CW_TTY_MAJOR_NUM;
+	tty_driver->minor_start = dev->id;
+	tty_driver->num		= 1;
+	tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	tty_driver->subtype = SERIAL_TYPE_NORMAL;
+	tty_driver->flags = TTY_DRIVER_REAL_RAW;
+	tty_driver->ttys	= dev->vs_dev.tty_table;
+	tty_driver->termios	= dev->vs_dev.termios;
+	tty_driver->termios_locked	= dev->vs_dev.termios_locked;
+	tty_set_operations(tty_driver, &cw_tty_ops);
+
+	if (tty_register_driver(tty_driver)) {
+		put_tty_driver(tty_driver);
+		pr_err("[%s] Couldn't register ttyCWXAN driver\n", __func__);
+		return -ENOMEM;
+	}
+	sema_init(&dev->vs_dev.write_lock, 1);
+	cw_tty[1] = dev;
+
+	return 0;
+}
+
+late_initcall(cw_tty_init);
+
+
diff --git a/drivers/misc/es305.c b/drivers/misc/es305.c
new file mode 100644
index 0000000..f3f3cd0
--- /dev/null
+++ b/drivers/misc/es305.c
@@ -0,0 +1,400 @@
+/* drivers/misc/es305.c - audience ES305 voice processor driver
+ *
+ * Copyright (C) 2012 Samsung 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/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/kthread.h>
+#include <linux/firmware.h>
+#include <linux/i2c/es305.h>
+
+#ifdef CONFIG_MACH_C1VZW
+#define ES305_FIRMWARE_NAME	"audience/es305_fw_c1vzw.bin"
+#else
+#define ES305_FIRMWARE_NAME	"audience/es305_fw.bin"
+#endif
+
+static struct i2c_client *this_client;
+static struct es305_platform_data *pdata;
+static struct task_struct *task;
+
+unsigned char es305_reset_cmd[] = {
+	0x80, 0x02, 0x00, 0x00,
+};
+
+unsigned char es305_sync_cmd[] = {
+	0x80, 0x00, 0x00, 0x00,
+};
+
+unsigned char es305_boot_cmd[] = {
+	0x00, 0x01,
+};
+
+unsigned char es305_sleep_cmd[] = {
+	0x80, 0x10, 0x00, 0x01,
+};
+
+unsigned char es305_bypass_data[] = {
+#ifdef CONFIG_MACH_C1VZW
+	0x80, 0x52, 0x00, 0x48,
+#else
+	0x80, 0x52, 0x00, 0x4C,
+#endif
+	0x80, 0x10, 0x00, 0x01,
+};
+
+static int es305_i2c_read(char *rxData, int length)
+{
+	int rc;
+	struct i2c_msg msgs[] = {
+		{
+			.addr = this_client->addr,
+			.flags = I2C_M_RD,
+			.len = length,
+			.buf = rxData,
+		},
+	};
+
+	rc = i2c_transfer(this_client->adapter, msgs, 1);
+	if (rc < 0) {
+		pr_err("%s: transfer error %d\n", __func__, rc);
+		return rc;
+	}
+	return 0;
+}
+
+static int es305_i2c_write(char *txData, int length)
+{
+	int rc;
+	struct i2c_msg msg[] = {
+		{
+			.addr = this_client->addr,
+			.flags = 0,
+			.len = length,
+			.buf = txData,
+		},
+	};
+
+	rc = i2c_transfer(this_client->adapter, msg, 1);
+	if (rc < 0) {
+		pr_err("%s: transfer error %d\n", __func__, rc);
+		return rc;
+	}
+
+	return 0;
+}
+
+int es305_set_cmd(enum es305_cmd cmd)
+{
+#if DEBUG
+	int i = 0;
+#endif
+	int rc = 0, size = 0;
+	unsigned char *i2c_cmds = NULL;
+
+	pr_info(MODULE_NAME "%s : cmd %d\n", __func__, cmd);
+	switch (cmd) {
+	case ES305_SW_RESET:
+		i2c_cmds = es305_reset_cmd;
+		size = sizeof(es305_reset_cmd);
+		break;
+	case ES305_SYNC:
+		i2c_cmds = es305_sync_cmd;
+		size = sizeof(es305_sync_cmd);
+		break;
+	case ES305_BOOT:
+		i2c_cmds = es305_boot_cmd;
+		size = sizeof(es305_boot_cmd);
+		break;
+	case ES305_SLEEP:
+		i2c_cmds = es305_sleep_cmd;
+		size = sizeof(es305_sleep_cmd);
+		break;
+	case ES305_BYPASS_DATA:
+		i2c_cmds = es305_bypass_data;
+		size = sizeof(es305_bypass_data);
+		break;
+	default:
+		pr_err(MODULE_NAME "%s : unknown cmd\n", __func__);
+		break;
+	}
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+
+	rc = es305_i2c_write(i2c_cmds, size);
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed %d\n", __func__, rc);
+
+	return rc;
+}
+
+
+int es305_sw_reset(void)
+{
+	int rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	i2c_cmds = es305_reset_cmd;
+	size = sizeof(es305_reset_cmd);
+
+	rc = es305_i2c_write(i2c_cmds, size);
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed %d\n", __func__, rc);
+
+	msleep(50);
+
+	return rc;
+}
+
+int es305_hw_reset(void)
+{
+	int rc = 0;
+	int retry = RETRY_CNT;
+	unsigned char msgbuf[4] = {0,};
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	while (retry--) {
+		/* Reset ES305 chip */
+		gpio_set_value(pdata->gpio_reset, 0);
+
+		/* Enable ES305 clock */
+		if (pdata->set_mclk != NULL)
+			pdata->set_mclk(true, false);
+		mdelay(1);
+
+		gpio_set_value(pdata->gpio_reset, 1);
+
+		msleep(50); /* Delay before send I2C command */
+
+		rc = es305_set_cmd(ES305_BOOT);
+		if (rc < 0) {
+			pr_err(MODULE_NAME "%s: set boot mode error\n",
+					__func__);
+			continue;
+		}
+
+		rc = es305_i2c_read(msgbuf, 1);
+		if (rc < 0) {
+			pr_err(MODULE_NAME "%s: boot mode ack error (%d retries left)\n",
+					__func__, retry);
+			continue;
+		}
+
+		mdelay(1);
+		rc = es305_load_firmware();
+		if (rc < 0) {
+			pr_err(MODULE_NAME "%s: load firmware error\n",
+					__func__);
+			continue;
+		}
+
+		msleep(20); /* Delay time before issue a Sync Cmd */
+
+		rc = es305_set_cmd(ES305_SYNC);
+		if (rc < 0) {
+			pr_err(MODULE_NAME "%s: set sync error\n",
+					__func__);
+			continue;
+		}
+
+		rc = es305_i2c_read(msgbuf, 4);
+		if (rc < 0) {
+			pr_err(MODULE_NAME "%s: boot mode ack error (%d retries left)\n",
+					__func__, retry);
+			continue;
+		}
+
+		break;
+	}
+	return rc;
+}
+
+static int es305_bootup_init(void *dummy)
+{
+	int rc = 0;
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	rc = es305_hw_reset();
+	if (rc < 0) {
+		pr_err(MODULE_NAME "%s: reset error %d\n",
+					__func__, rc);
+		return rc;
+	}
+
+	rc = es305_set_cmd(ES305_BYPASS_DATA);
+	if (rc < 0) {
+		pr_err(MODULE_NAME "%s: set bypass error %d\n",
+					__func__, rc);
+		return rc;
+	}
+	return rc;
+}
+
+static void es305_gpio_init(void)
+{
+	if (pdata->gpio_wakeup) {
+		gpio_request(pdata->gpio_wakeup, "ES305_WAKEUP");
+		gpio_direction_output(pdata->gpio_wakeup, 1);
+		gpio_free(pdata->gpio_wakeup);
+		gpio_set_value(pdata->gpio_wakeup, 1);
+	}
+
+	if (pdata->gpio_reset) {
+		gpio_request(pdata->gpio_reset, "ES305_RESET");
+		gpio_direction_output(pdata->gpio_reset, 1);
+		gpio_free(pdata->gpio_reset);
+		gpio_set_value(pdata->gpio_reset, 1);
+	}
+}
+
+int es305_load_firmware(void)
+{
+	int rc = 0, i = 0;
+	size_t size = 0;
+	const struct firmware *fw = NULL;
+	unsigned char *i2c_cmds;
+
+	pr_info(MODULE_NAME "%s : start\n", __func__);
+
+	rc = request_firmware(&fw, ES305_FIRMWARE_NAME,
+				  &this_client->dev);
+	if (rc < 0) {
+		pr_err(MODULE_NAME "%s : unable to open firmware ret %d\n",
+				__func__, rc);
+		release_firmware(fw);
+		return rc;
+	}
+	i2c_cmds = (unsigned char *)fw->data;
+	size = fw->size;
+
+	for (i = 0; i < (size/32); i++, i2c_cmds += 32) {
+		rc = es305_i2c_write(i2c_cmds, 32);
+		if (rc < 0) {
+			pr_err(MODULE_NAME "%s failed %d\n", __func__, rc);
+			break;
+		}
+	}
+
+	if (size%32) {
+		rc = es305_i2c_write(i2c_cmds, size%32);
+		if (rc < 0)
+			pr_err(MODULE_NAME "%s failed %d\n", __func__, rc);
+	}
+
+	release_firmware(fw);
+
+	pr_info(MODULE_NAME "%s : end\n", __func__);
+	return rc;
+}
+
+
+static int es305_probe(
+		struct i2c_client *client, const struct i2c_device_id *id)
+{
+	int rc = 0;
+	pdata = client->dev.platform_data;
+
+	pr_info(MODULE_NAME "%s : start\n", __func__);
+
+	if (pdata == NULL) {
+		pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+		if (pdata == NULL) {
+			rc = -ENOMEM;
+			pr_err(MODULE_NAME "%s: platform data is NULL\n",
+					__func__);
+		}
+	}
+
+	this_client = client;
+
+	es305_gpio_init();
+	task = kthread_run(es305_bootup_init, NULL, "es305_bootup_init");
+	if (IS_ERR(task)) {
+		rc = PTR_ERR(task);
+		task = NULL;
+	}
+
+	pr_info(MODULE_NAME "%s : finish\n", __func__);
+
+	return rc;
+}
+
+static int es305_remove(struct i2c_client *client)
+{
+	struct es305_platform_data *pes305data = i2c_get_clientdata(client);
+	kfree(pes305data);
+
+	return 0;
+}
+
+static int es305_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	return 0;
+}
+
+static int es305_resume(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id es305_id[] = {
+	{ "audience_es305", 0 },
+	{ }
+};
+
+static struct i2c_driver es305_driver = {
+	.probe = es305_probe,
+	.remove = es305_remove,
+	.suspend = es305_suspend,
+	.resume	= es305_resume,
+	.id_table = es305_id,
+	.driver = {
+		.name = "audience_es305",
+	},
+};
+
+static int __init es305_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return i2c_add_driver(&es305_driver);
+}
+
+static void __exit es305_exit(void)
+{
+	i2c_del_driver(&es305_driver);
+}
+
+module_init(es305_init);
+module_exit(es305_exit);
+
+MODULE_DESCRIPTION("audience es305 voice processor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/fm34_we395.c b/drivers/misc/fm34_we395.c
new file mode 100644
index 0000000..aa12226
--- /dev/null
+++ b/drivers/misc/fm34_we395.c
@@ -0,0 +1,1402 @@
+/* drivers/misc/fm34_we395.c - fm34_we395 voice processor driver
+ *
+ * Copyright (C) 2012 Samsung 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/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/kthread.h>
+
+#include <linux/i2c/fm34_we395.h>
+
+#define MODULE_NAME "[FM34_WE395] :"
+#define DEBUG	0
+
+static struct i2c_client *this_client;
+static struct fm34_platform_data *pdata;
+
+#if defined(CONFIG_MACH_C1_KOR_LGT) || defined(CONFIG_MACH_C1VZW)
+unsigned char loopback_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC0, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xC1, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xC2, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x22, 0xC3, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xB8,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char bypass_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xC0, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC1, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xC2, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC3, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xB8,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char HS_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x68, 0x64, 0x04,
+0xFC, 0xF3, 0x3B, 0x3F, 0xE8, 0x00, 0x50,
+0xFC, 0xF3, 0x0D, 0x10, 0x00, 0x0C, 0x2C, 0x00,
+0xFC, 0xF3, 0x88,
+0x2E, 0xEC, 0xAC, 0x19, 0x00, 0x52, 0x47, 0xFF,
+0xF4, 0x19, 0x00, 0x6F, 0x1C, 0x99, 0x8F, 0x37,
+0x83, 0x01, 0x47, 0xC2, 0x97, 0x60, 0x00, 0xA5,
+0x62, 0xE2, 0xE6, 0x19, 0x00, 0xC4, 0x42, 0x66,
+0x67, 0x20, 0xCE, 0x0F, 0x0D, 0x00, 0xBC, 0x0D,
+0x00, 0xCD, 0x21, 0x0A, 0x0F, 0x6A, 0x64, 0xB4,
+0x80, 0x7A, 0x17, 0x68, 0x8A, 0xA6, 0x0E, 0x64,
+0x0F, 0x0E, 0x44, 0x0F, 0x10, 0x5B, 0x59, 0x90,
+0x7A, 0x1F, 0x80, 0x7A, 0x2A, 0x22, 0x6A, 0x0F,
+0x90, 0x7A, 0x2A, 0x34, 0x00, 0x0E, 0x1A, 0x61,
+0x0F,
+0xFC, 0xF3, 0x68, 0x64, 0x00,
+0xFC, 0xF3, 0x3B, 0x3F, 0xA1, 0xA6, 0x06,
+0xFC, 0xF3, 0x3B, 0x3F, 0xB1, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF7, 0x00, 0x80,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x03, 0x7D, 0xB9,
+0xFC, 0xF3, 0x3B, 0x23, 0x04, 0x01, 0xDE,
+0xFC, 0xF3, 0x3B, 0x23, 0x05, 0x20, 0x6C,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x08, 0x08, 0xFA,
+0xFC, 0xF3, 0x3B, 0x23, 0x09, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xE9,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x01, 0xD0,
+0xFC, 0xF3, 0x3B, 0x23, 0x10, 0x12, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x25, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x26, 0x00, 0x38,
+0xFC, 0xF3, 0x3B, 0x23, 0x2F, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x32, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x33, 0x00, 0x0C,
+0xFC, 0xF3, 0x3B, 0x23, 0x37, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x39, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x48, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x49, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x50, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x51, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x52, 0x00, 0xC0,
+0xFC, 0xF3, 0x3B, 0x23, 0x56, 0x50, 0x28,
+0xFC, 0xF3, 0x3B, 0x23, 0x5A, 0x06, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x5B, 0x1E, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x60, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x61, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x64, 0x00, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x6E, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6F, 0x0E, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x70, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x71, 0x05, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x73, 0x21, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x74, 0x1C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x75, 0x12, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x77, 0x2C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7C, 0x58, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x80, 0x0A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x81, 0x05, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x82, 0x03, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x83, 0x03, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x84, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0x86, 0x48, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x8B, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x90, 0xB9, 0x88,
+0xFC, 0xF3, 0x3B, 0x23, 0x91, 0x87, 0x54,
+0xFC, 0xF3, 0x3B, 0x23, 0x92, 0x44, 0x43,
+0xFC, 0xF3, 0x3B, 0x23, 0x93, 0x32, 0x20,
+0xFC, 0xF3, 0x3B, 0x23, 0x9C, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xA6, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x23, 0xBD, 0x30, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xC9, 0x16, 0x18,
+0xFC, 0xF3, 0x3B, 0x23, 0xCB, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xCD, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0xCF, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD0, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD1, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD2, 0x03, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xDB, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDC, 0x39, 0xDE,
+0xFC, 0xF3, 0x3B, 0x23, 0xDF, 0x54, 0x44,
+0xFC, 0xF3, 0x3B, 0x23, 0xE0, 0x43, 0x34,
+0xFC, 0xF3, 0x3B, 0x23, 0xE1, 0x56, 0x66,
+0xFC, 0xF3, 0x3B, 0x23, 0xE2, 0x67, 0x68,
+0xFC, 0xF3, 0x3B, 0x23, 0xE5, 0x30, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE6, 0x60, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE7, 0x16, 0x3E,
+0xFC, 0xF3, 0x3B, 0x23, 0xE8, 0x16, 0xA2,
+0xFC, 0xF3, 0x3B, 0x23, 0xE9, 0x2C, 0xCC,
+0xFC, 0xF3, 0x3B, 0x23, 0xEA, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xEB, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0xEC, 0x00, 0x08,
+0xFC, 0xF3, 0x3B, 0x23, 0xED, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xEE, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xF0, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char HF_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x68, 0x64, 0x04,
+0xFC, 0xF3, 0x3B, 0x3F, 0xE8, 0x00, 0x50,
+0xFC, 0xF3, 0x0D, 0x10, 0x00, 0x0C, 0x2C, 0x00,
+0xFC, 0xF3, 0x88,
+0x2E, 0xEC, 0xAC, 0x19, 0x00, 0x52, 0x47, 0xFF,
+0xF4, 0x19, 0x00, 0x6F, 0x1C, 0x99, 0x8F, 0x37,
+0x83, 0x01, 0x47, 0xC2, 0x97, 0x60, 0x00, 0xA5,
+0x62, 0xE2, 0xE6, 0x19, 0x00, 0xC4, 0x42, 0x66,
+0x67, 0x20, 0xCE, 0x0F, 0x0D, 0x00, 0xBC, 0x0D,
+0x00, 0xCD, 0x21, 0x0A, 0x0F, 0x6A, 0x64, 0xB4,
+0x80, 0x7A, 0x17, 0x68, 0x8A, 0xA6, 0x0E, 0x64,
+0x0F, 0x0E, 0x44, 0x0F, 0x10, 0x5B, 0x59, 0x90,
+0x7A, 0x1F, 0x80, 0x7A, 0x2A, 0x22, 0x6A, 0x0F,
+0x90, 0x7A, 0x2A, 0x34, 0x00, 0x0E, 0x1A, 0x61,
+0x0F,
+0xFC, 0xF3, 0x68, 0x64, 0x00,
+0xFC, 0xF3, 0x3B, 0x3F, 0xA1, 0xA6, 0x06,
+0xFC, 0xF3, 0x3B, 0x3F, 0xB1, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xB8, 0x1F, 0x40,
+0xFC, 0xF3, 0x3B, 0x22, 0xB9, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF2, 0x00, 0x48,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x03, 0x6D, 0xD9,
+0xFC, 0xF3, 0x3B, 0x23, 0x04, 0x03, 0xCF,
+0xFC, 0xF3, 0x3B, 0x23, 0x05, 0x00, 0x05,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x08, 0x04, 0x03,
+0xFC, 0xF3, 0x3B, 0x23, 0x09, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x01, 0x55,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x0A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x10, 0x12, 0x48,
+0xFC, 0xF3, 0x3B, 0x23, 0x11, 0x4E, 0xA9,
+0xFC, 0xF3, 0x3B, 0x23, 0x12, 0xB1, 0x5C,
+0xFC, 0xF3, 0x3B, 0x23, 0x13, 0x4E, 0xA9,
+0xFC, 0xF3, 0x3B, 0x23, 0x14, 0x95, 0x7C,
+0xFC, 0xF3, 0x3B, 0x23, 0x15, 0x5C, 0xB7,
+0xFC, 0xF3, 0x3B, 0x23, 0x16, 0x54, 0x2A,
+0xFC, 0xF3, 0x3B, 0x23, 0x17, 0xAB, 0xEF,
+0xFC, 0xF3, 0x3B, 0x23, 0x18, 0x54, 0x2A,
+0xFC, 0xF3, 0x3B, 0x23, 0x19, 0x84, 0x8D,
+0xFC, 0xF3, 0x3B, 0x23, 0x1A, 0x79, 0x55,
+0xFC, 0xF3, 0x3B, 0x23, 0x25, 0x58, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x26, 0x00, 0x38,
+0xFC, 0xF3, 0x3B, 0x23, 0x27, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x2D, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x2F, 0x00, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x32, 0x00, 0x30,
+0xFC, 0xF3, 0x3B, 0x23, 0x33, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x37, 0xFF, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0x39, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x3C, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x3D, 0x12, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x3F, 0x00, 0x06,
+0xFC, 0xF3, 0x3B, 0x23, 0x41, 0xFF, 0xF3,
+0xFC, 0xF3, 0x3B, 0x23, 0x42, 0xFF, 0xF2,
+0xFC, 0xF3, 0x3B, 0x23, 0x43, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x44, 0x00, 0x05,
+0xFC, 0xF3, 0x3B, 0x23, 0x48, 0x0B, 0x4D,
+0xFC, 0xF3, 0x3B, 0x23, 0x49, 0x0F, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x50, 0x34, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x51, 0x34, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x52, 0x00, 0x60,
+0xFC, 0xF3, 0x3B, 0x23, 0x53, 0x20, 0x10,
+0xFC, 0xF3, 0x3B, 0x23, 0x54, 0x1F, 0xF0,
+0xFC, 0xF3, 0x3B, 0x23, 0x56, 0x6A, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x5A, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x5B, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x5C, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x5F, 0x00, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x64, 0x00, 0x3C,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6E, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6F, 0x10, 0x06,
+0xFC, 0xF3, 0x3B, 0x23, 0x70, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x71, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x72, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x73, 0x12, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x74, 0x15, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x75, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7A, 0x05, 0xAA,
+0xFC, 0xF3, 0x3B, 0x23, 0x7C, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7D, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7F, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x80, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x81, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x82, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x83, 0x05, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x84, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0x8C, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x8D, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x8E, 0x74, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x90, 0xA9, 0x98,
+0xFC, 0xF3, 0x3B, 0x23, 0x91, 0x75, 0x56,
+0xFC, 0xF3, 0x3B, 0x23, 0x92, 0x54, 0x43,
+0xFC, 0xF3, 0x3B, 0x23, 0x93, 0x33, 0x36,
+0xFC, 0xF3, 0x3B, 0x23, 0x96, 0x38, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x97, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x9C, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x9D, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x9E, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xA5, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0xB3, 0x00, 0x0C,
+0xFC, 0xF3, 0x3B, 0x23, 0xB4, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0xB5, 0x60, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xB7, 0x00, 0x20,
+0xFC, 0xF3, 0x3B, 0x23, 0xB8, 0x78, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xB9, 0x24, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xBA, 0x06, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xBB, 0x0C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xBC, 0x00, 0xC0,
+0xFC, 0xF3, 0x3B, 0x23, 0xBD, 0x01, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xBE, 0x26, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xCF, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xD0, 0x06, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD1, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xD2, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xD5, 0x78, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDB, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDC, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDF, 0x77, 0x89,
+0xFC, 0xF3, 0x3B, 0x23, 0xE0, 0xAA, 0xAA,
+0xFC, 0xF3, 0x3B, 0x23, 0xE1, 0x98, 0x77,
+0xFC, 0xF3, 0x3B, 0x23, 0xE2, 0x54, 0x33,
+0xFC, 0xF3, 0x3B, 0x23, 0xE5, 0x30, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE6, 0x7F, 0xA0,
+0xFC, 0xF3, 0x3B, 0x23, 0xE7, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xE8, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xE9, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xEA, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xEB, 0x00, 0x60,
+0xFC, 0xF3, 0x3B, 0x23, 0xEC, 0x00, 0x90,
+0xFC, 0xF3, 0x3B, 0x23, 0xED, 0x00, 0x60,
+0xFC, 0xF3, 0x3B, 0x23, 0xEE, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xEF, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xF0, 0x6A, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char HS_NS_cmd[] = {
+/*0xC0,*/
+0xC0,
+0xFC, 0xF3, 0x68, 0x64, 0x04,
+0xFC, 0xF3, 0x3B, 0x3F, 0xE8, 0x00, 0x50,
+0xFC, 0xF3, 0x0D, 0x10, 0x00, 0x0C, 0x2C, 0x00,
+0xFC, 0xF3, 0x88,
+0x2E, 0xEC, 0xAC, 0x19, 0x00, 0x52, 0x47, 0xFF,
+0xF4, 0x19, 0x00, 0x6F, 0x1C, 0x99, 0x8F, 0x37,
+0x83, 0x01, 0x47, 0xC2, 0x97, 0x60, 0x00, 0xA5,
+0x62, 0xE2, 0xE6, 0x19, 0x00, 0xC4, 0x42, 0x66,
+0x67, 0x20, 0xCE, 0x0F, 0x0D, 0x00, 0xBC, 0x0D,
+0x00, 0xCD, 0x21, 0x0A, 0x0F, 0x6A, 0x64, 0xB4,
+0x80, 0x7A, 0x17, 0x68, 0x8A, 0xA6, 0x0E, 0x64,
+0x0F, 0x0E, 0x44, 0x0F, 0x10, 0x5B, 0x59, 0x90,
+0x7A, 0x1F, 0x80, 0x7A, 0x2A, 0x22, 0x6A, 0x0F,
+0x90, 0x7A, 0x2A, 0x34, 0x00, 0x0E, 0x1A, 0x61,
+0x0F,
+0xFC, 0xF3, 0x68, 0x64, 0x00,
+0xFC, 0xF3, 0x3B, 0x3F, 0xA1, 0xA6, 0x06,
+0xFC, 0xF3, 0x3B, 0x3F, 0xB1, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF7, 0x00, 0x80,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x03, 0x7D, 0xB9,
+0xFC, 0xF3, 0x3B, 0x23, 0x04, 0x01, 0xDE,
+0xFC, 0xF3, 0x3B, 0x23, 0x05, 0x20, 0x6C,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x08, 0x08, 0xFA,
+0xFC, 0xF3, 0x3B, 0x23, 0x09, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xE9,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x01, 0xD8,
+0xFC, 0xF3, 0x3B, 0x23, 0x10, 0x12, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x25, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x26, 0x00, 0x38,
+0xFC, 0xF3, 0x3B, 0x23, 0x2F, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x32, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x33, 0x00, 0x0C,
+0xFC, 0xF3, 0x3B, 0x23, 0x37, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x39, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x48, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x49, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x50, 0x38, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x51, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x52, 0x00, 0xC0,
+0xFC, 0xF3, 0x3B, 0x23, 0x56, 0x50, 0x32,
+0xFC, 0xF3, 0x3B, 0x23, 0x5A, 0x06, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x5B, 0x1E, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x60, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x61, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x64, 0x00, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x6E, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6F, 0x12, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x70, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x71, 0x05, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x73, 0x21, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x74, 0x1C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x75, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x77, 0x2C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7B, 0x00, 0x08,
+0xFC, 0xF3, 0x3B, 0x23, 0x7C, 0x58, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x80, 0x0A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x81, 0x05, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x82, 0x03, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x83, 0x03, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x84, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0x86, 0x48, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x8B, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x90, 0xA9, 0x88,
+0xFC, 0xF3, 0x3B, 0x23, 0x91, 0x87, 0x54,
+0xFC, 0xF3, 0x3B, 0x23, 0x92, 0x44, 0x43,
+0xFC, 0xF3, 0x3B, 0x23, 0x93, 0x32, 0x21,
+0xFC, 0xF3, 0x3B, 0x23, 0x9C, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xA6, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x23, 0xBD, 0x30, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xC9, 0x16, 0x18,
+0xFC, 0xF3, 0x3B, 0x23, 0xCB, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xCD, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0xCF, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD0, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD1, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD2, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xDB, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDC, 0x39, 0xDE,
+0xFC, 0xF3, 0x3B, 0x23, 0xDF, 0x54, 0x44,
+0xFC, 0xF3, 0x3B, 0x23, 0xE0, 0x43, 0x34,
+0xFC, 0xF3, 0x3B, 0x23, 0xE1, 0x56, 0x66,
+0xFC, 0xF3, 0x3B, 0x23, 0xE2, 0x66, 0x66,
+0xFC, 0xF3, 0x3B, 0x23, 0xE5, 0x30, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE6, 0x60, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE7, 0x16, 0x3E,
+0xFC, 0xF3, 0x3B, 0x23, 0xE8, 0x16, 0xA2,
+0xFC, 0xF3, 0x3B, 0x23, 0xE9, 0x2C, 0xCC,
+0xFC, 0xF3, 0x3B, 0x23, 0xEA, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xEB, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0xEC, 0x00, 0x08,
+0xFC, 0xF3, 0x3B, 0x23, 0xED, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xEE, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xF0, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char HS_ExtraVol_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x68, 0x64, 0x04,
+0xFC, 0xF3, 0x3B, 0x3F, 0xE8, 0x00, 0x50,
+0xFC, 0xF3, 0x0D, 0x10, 0x00, 0x0C, 0x2C, 0x00,
+0xFC, 0xF3, 0x88,
+0x2E, 0xEC, 0xAC, 0x19, 0x00, 0x52, 0x47, 0xFF,
+0xF4, 0x19, 0x00, 0x6F, 0x1C, 0x99, 0x8F, 0x37,
+0x83, 0x01, 0x47, 0xC2, 0x97, 0x60, 0x00, 0xA5,
+0x62, 0xE2, 0xE6, 0x19, 0x00, 0xC4, 0x42, 0x66,
+0x67, 0x20, 0xCE, 0x0F, 0x0D, 0x00, 0xBC, 0x0D,
+0x00, 0xCD, 0x21, 0x0A, 0x0F, 0x6A, 0x64, 0xB4,
+0x80, 0x7A, 0x17, 0x68, 0x8A, 0xA6, 0x0E, 0x64,
+0x0F, 0x0E, 0x44, 0x0F, 0x10, 0x5B, 0x59, 0x90,
+0x7A, 0x1F, 0x80, 0x7A, 0x2A, 0x22, 0x6A, 0x0F,
+0x90, 0x7A, 0x2A, 0x34, 0x00, 0x0E, 0x1A, 0x61,
+0x0F,
+0xFC, 0xF3, 0x68, 0x64, 0x00,
+0xFC, 0xF3, 0x3B, 0x3F, 0xA1, 0xA6, 0x06,
+0xFC, 0xF3, 0x3B, 0x3F, 0xB1, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF7, 0x00, 0x80,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x03, 0x7D, 0xB9,
+0xFC, 0xF3, 0x3B, 0x23, 0x04, 0x01, 0xDE,
+0xFC, 0xF3, 0x3B, 0x23, 0x05, 0x20, 0x6C,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x08, 0x08, 0xFA,
+0xFC, 0xF3, 0x3B, 0x23, 0x09, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xE9,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x05, 0xA0,
+0xFC, 0xF3, 0x3B, 0x23, 0x10, 0x12, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x25, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x26, 0x00, 0x38,
+0xFC, 0xF3, 0x3B, 0x23, 0x2F, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x32, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x33, 0x00, 0x0C,
+0xFC, 0xF3, 0x3B, 0x23, 0x37, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x39, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x48, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x49, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x50, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x51, 0x28, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x52, 0x00, 0xC0,
+0xFC, 0xF3, 0x3B, 0x23, 0x56, 0x50, 0x28,
+0xFC, 0xF3, 0x3B, 0x23, 0x5A, 0x06, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x5B, 0x1E, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x60, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x61, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x64, 0x00, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x6E, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6F, 0x0E, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x70, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x71, 0x05, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x73, 0x21, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x74, 0x1C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x75, 0x12, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x77, 0x2C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7C, 0x58, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x80, 0x0A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x81, 0x05, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x82, 0x03, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x83, 0x03, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x84, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0x86, 0x48, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x8B, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x90, 0xB9, 0x88,
+0xFC, 0xF3, 0x3B, 0x23, 0x91, 0x87, 0x54,
+0xFC, 0xF3, 0x3B, 0x23, 0x92, 0x44, 0x43,
+0xFC, 0xF3, 0x3B, 0x23, 0x93, 0x32, 0x20,
+0xFC, 0xF3, 0x3B, 0x23, 0x9C, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xA6, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x23, 0xBD, 0x30, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xC9, 0x16, 0x18,
+0xFC, 0xF3, 0x3B, 0x23, 0xCB, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xCD, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0xCF, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD0, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD1, 0x04, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD2, 0x03, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xDB, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDC, 0x39, 0xDE,
+0xFC, 0xF3, 0x3B, 0x23, 0xDF, 0x65, 0x55,
+0xFC, 0xF3, 0x3B, 0x23, 0xE0, 0x54, 0x45,
+0xFC, 0xF3, 0x3B, 0x23, 0xE1, 0x54, 0x43,
+0xFC, 0xF3, 0x3B, 0x23, 0xE2, 0x33, 0x22,
+0xFC, 0xF3, 0x3B, 0x23, 0xE5, 0x30, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE6, 0x60, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE7, 0x16, 0x3E,
+0xFC, 0xF3, 0x3B, 0x23, 0xE8, 0x16, 0xA2,
+0xFC, 0xF3, 0x3B, 0x23, 0xE9, 0x2C, 0xCC,
+0xFC, 0xF3, 0x3B, 0x23, 0xEA, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xEB, 0x00, 0x10,
+0xFC, 0xF3, 0x3B, 0x23, 0xEC, 0x00, 0x20,
+0xFC, 0xF3, 0x3B, 0x23, 0xED, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xEE, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xF0, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char HF_ExtraVol_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x68, 0x64, 0x04,
+0xFC, 0xF3, 0x3B, 0x3F, 0xE8, 0x00, 0x50,
+0xFC, 0xF3, 0x0D, 0x10, 0x00, 0x0C, 0x2C, 0x00,
+0xFC, 0xF3, 0x88,
+0x2E, 0xEC, 0xAC, 0x19, 0x00, 0x52, 0x47, 0xFF,
+0xF4, 0x19, 0x00, 0x6F, 0x1C, 0x99, 0x8F, 0x37,
+0x83, 0x01, 0x47, 0xC2, 0x97, 0x60, 0x00, 0xA5,
+0x62, 0xE2, 0xE6, 0x19, 0x00, 0xC4, 0x42, 0x66,
+0x67, 0x20, 0xCE, 0x0F, 0x0D, 0x00, 0xBC, 0x0D,
+0x00, 0xCD, 0x21, 0x0A, 0x0F, 0x6A, 0x64, 0xB4,
+0x80, 0x7A, 0x17, 0x68, 0x8A, 0xA6, 0x0E, 0x64,
+0x0F, 0x0E, 0x44, 0x0F, 0x10, 0x5B, 0x59, 0x90,
+0x7A, 0x1F, 0x80, 0x7A, 0x2A, 0x22, 0x6A, 0x0F,
+0x90, 0x7A, 0x2A, 0x34, 0x00, 0x0E, 0x1A, 0x61,
+0x0F,
+0xFC, 0xF3, 0x68, 0x64, 0x00,
+0xFC, 0xF3, 0x3B, 0x3F, 0xA1, 0xA6, 0x06,
+0xFC, 0xF3, 0x3B, 0x3F, 0xB1, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xB8, 0x1F, 0x40,
+0xFC, 0xF3, 0x3B, 0x22, 0xB9, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF2, 0x00, 0x48,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x03, 0x6D, 0xD9,
+0xFC, 0xF3, 0x3B, 0x23, 0x04, 0x03, 0xCF,
+0xFC, 0xF3, 0x3B, 0x23, 0x05, 0x00, 0x05,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x08, 0x04, 0x03,
+0xFC, 0xF3, 0x3B, 0x23, 0x09, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x01, 0x30,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x10, 0x12, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x11, 0x4E, 0xA9,
+0xFC, 0xF3, 0x3B, 0x23, 0x12, 0xB1, 0x5C,
+0xFC, 0xF3, 0x3B, 0x23, 0x13, 0x4E, 0xA9,
+0xFC, 0xF3, 0x3B, 0x23, 0x14, 0x95, 0x7C,
+0xFC, 0xF3, 0x3B, 0x23, 0x15, 0x5C, 0xB7,
+0xFC, 0xF3, 0x3B, 0x23, 0x16, 0x54, 0x2A,
+0xFC, 0xF3, 0x3B, 0x23, 0x17, 0xAB, 0xEF,
+0xFC, 0xF3, 0x3B, 0x23, 0x18, 0x54, 0x2A,
+0xFC, 0xF3, 0x3B, 0x23, 0x19, 0x84, 0x8D,
+0xFC, 0xF3, 0x3B, 0x23, 0x1A, 0x79, 0x55,
+0xFC, 0xF3, 0x3B, 0x23, 0x25, 0x58, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x26, 0x00, 0x38,
+0xFC, 0xF3, 0x3B, 0x23, 0x27, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x2D, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x2F, 0x00, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0x32, 0x00, 0x30,
+0xFC, 0xF3, 0x3B, 0x23, 0x33, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x37, 0xFF, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0x39, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x3C, 0x01, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x3D, 0x12, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x3F, 0x00, 0x06,
+0xFC, 0xF3, 0x3B, 0x23, 0x41, 0xFF, 0xF3,
+0xFC, 0xF3, 0x3B, 0x23, 0x42, 0xFF, 0xF2,
+0xFC, 0xF3, 0x3B, 0x23, 0x43, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x44, 0x00, 0x05,
+0xFC, 0xF3, 0x3B, 0x23, 0x48, 0x0B, 0x4D,
+0xFC, 0xF3, 0x3B, 0x23, 0x49, 0x0F, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x50, 0x34, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x51, 0x34, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x52, 0x00, 0x60,
+0xFC, 0xF3, 0x3B, 0x23, 0x53, 0x20, 0x10,
+0xFC, 0xF3, 0x3B, 0x23, 0x54, 0x1F, 0xF0,
+0xFC, 0xF3, 0x3B, 0x23, 0x56, 0x6A, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x5A, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x5B, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x5C, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x5F, 0x00, 0x40,
+0xFC, 0xF3, 0x3B, 0x23, 0x64, 0x00, 0x3C,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6E, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x6F, 0x10, 0x06,
+0xFC, 0xF3, 0x3B, 0x23, 0x70, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x71, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x72, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x73, 0x12, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x74, 0x15, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x75, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7A, 0x05, 0xAA,
+0xFC, 0xF3, 0x3B, 0x23, 0x7C, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7D, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x7F, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x80, 0x70, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x81, 0x50, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x82, 0x04, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x83, 0x05, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x84, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0x8C, 0x10, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x8D, 0x00, 0x04,
+0xFC, 0xF3, 0x3B, 0x23, 0x8E, 0x74, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x90, 0xA9, 0x98,
+0xFC, 0xF3, 0x3B, 0x23, 0x91, 0x75, 0x56,
+0xFC, 0xF3, 0x3B, 0x23, 0x92, 0x54, 0x43,
+0xFC, 0xF3, 0x3B, 0x23, 0x93, 0x33, 0x36,
+0xFC, 0xF3, 0x3B, 0x23, 0x96, 0x38, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x97, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x9C, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x9D, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x9E, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xA5, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0xB3, 0x00, 0x0C,
+0xFC, 0xF3, 0x3B, 0x23, 0xB4, 0x00, 0x07,
+0xFC, 0xF3, 0x3B, 0x23, 0xB5, 0x60, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xB7, 0x00, 0x20,
+0xFC, 0xF3, 0x3B, 0x23, 0xB8, 0x78, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xB9, 0x24, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xBA, 0x06, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xBB, 0x0C, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xBC, 0x00, 0xC0,
+0xFC, 0xF3, 0x3B, 0x23, 0xBD, 0x01, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xBE, 0x26, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xCF, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xD0, 0x06, 0x80,
+0xFC, 0xF3, 0x3B, 0x23, 0xD1, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xD2, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xD5, 0x78, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDB, 0x18, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDC, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xDF, 0xFD, 0xA8,
+0xFC, 0xF3, 0x3B, 0x23, 0xE0, 0x76, 0x54,
+0xFC, 0xF3, 0x3B, 0x23, 0xE1, 0x32, 0x10,
+0xFC, 0xF3, 0x3B, 0x23, 0xE2, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE5, 0x30, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xE6, 0x7F, 0xA0,
+0xFC, 0xF3, 0x3B, 0x23, 0xE7, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xE8, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xE9, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xEA, 0x7F, 0xFF,
+0xFC, 0xF3, 0x3B, 0x23, 0xEB, 0x00, 0x60,
+0xFC, 0xF3, 0x3B, 0x23, 0xEC, 0x00, 0x90,
+0xFC, 0xF3, 0x3B, 0x23, 0xED, 0x00, 0x60,
+0xFC, 0xF3, 0x3B, 0x23, 0xEE, 0x20, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xEF, 0x40, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0xF0, 0x6A, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char EP_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xC0, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC1, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xC2, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC3, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xB8,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char BT_SCO_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xC0, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC1, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xC2, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC3, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xB8,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char HS_FACTORY_RCV_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xC0, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC1, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC2, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC3, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xB8,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+
+unsigned char HS_FACTORY_SPK_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xC0, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xC1, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xC2, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC3, 0x00, 0x02,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x08, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x23, 0x07, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x0C, 0x00, 0xB8,
+0xFC, 0xF3, 0x3B, 0x23, 0x0D, 0x02, 0x00,
+0xFC, 0xF3, 0x3B, 0x23, 0x65, 0x08, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00
+};
+#else
+unsigned char bypass_cmd[] = {
+/*0xC0,*/
+0xFC, 0xF3, 0x3B, 0x22, 0xF5, 0x00, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xF8, 0x80, 0x03,
+0xFC, 0xF3, 0x3B, 0x22, 0xC6, 0x00, 0x7D,
+0xFC, 0xF3, 0x3B, 0x22, 0xC7, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xC8, 0x00, 0x18,
+0xFC, 0xF3, 0x3B, 0x23, 0x0A, 0x1A, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xFA, 0x24, 0x8B,
+0xFC, 0xF3, 0x3B, 0x22, 0xF9, 0x00, 0x7F,
+0xFC, 0xF3, 0x3B, 0x22, 0xF6, 0x00, 0x00,
+0xFC, 0xF3, 0x3B, 0x22, 0xD2, 0x82, 0x94,
+0xFC, 0xF3, 0x3B, 0x22, 0xEE, 0x00, 0x01,
+0xFC, 0xF3, 0x3B, 0x22, 0xFB, 0x00, 0x00,
+};
+#endif
+
+static int fm34_i2c_read(char *rxData, int length)
+{
+	int rc;
+	struct i2c_msg msgs[] = {
+		{
+			.addr = this_client->addr,
+			.flags = I2C_M_RD,
+			.len = length,
+			.buf = rxData,
+		},
+	};
+
+	rc = i2c_transfer(this_client->adapter, msgs, 1);
+	if (rc < 0) {
+		pr_err("%s: transfer error %d\n", __func__, rc);
+		return rc;
+	}
+	return 0;
+}
+
+static int fm34_i2c_write(char *txData, int length)
+{
+	int rc;
+	struct i2c_msg msg[] = {
+		{
+			.addr = this_client->addr,
+			.flags = 0,
+			.len = length,
+			.buf = txData,
+		},
+	};
+
+	rc = i2c_transfer(this_client->adapter, msg, 1);
+	if (rc < 0) {
+		pr_err("%s: transfer error %d\n", __func__, rc);
+		return rc;
+	}
+
+	return 0;
+}
+
+#if defined(CONFIG_MACH_C1_KOR_LGT) || defined(CONFIG_MACH_C1VZW)
+void fm34_parameter_reset(void)
+{
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (pdata->gpio_rst) {
+		gpio_set_value(pdata->gpio_rst, 0);
+		usleep_range(5000, 5000);
+		gpio_set_value(pdata->gpio_rst, 1);
+	}
+	usleep_range(5000, 5000);
+
+}
+
+int fm34_set_bypass_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = bypass_cmd;
+	size = sizeof(bypass_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_hw_bypass_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	usleep_range(20000, 20000);
+	gpio_set_value(pdata->gpio_pwdn, 0);
+
+	return rc;
+}
+
+
+int fm34_set_loopback_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = loopback_cmd;
+	size = sizeof(loopback_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_HS_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = HS_cmd;
+	size = sizeof(HS_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_SPK_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = HF_cmd;
+	size = sizeof(HF_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_HS_NS_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = HS_NS_cmd;
+	size = sizeof(HS_NS_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_HS_ExtraVol_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = HS_ExtraVol_cmd;
+	size = sizeof(HS_ExtraVol_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_SPK_ExtraVol_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = HF_ExtraVol_cmd;
+	size = sizeof(HF_ExtraVol_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_EP_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = EP_cmd;
+	size = sizeof(EP_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_BTSCO_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = BT_SCO_cmd;
+	size = sizeof(BT_SCO_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_factory_rcv_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = HS_FACTORY_RCV_cmd;
+	size = sizeof(HS_FACTORY_RCV_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_factory_spk_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+	int val = gpio_get_value(pdata->gpio_pwdn);
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	if (val == 0)
+		gpio_set_value(pdata->gpio_pwdn, 1);
+
+	fm34_parameter_reset();
+
+	i2c_cmds = HS_FACTORY_SPK_cmd;
+	size = sizeof(HS_FACTORY_SPK_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+	if (rc < 0)
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+
+	return rc;
+}
+
+int fm34_set_mode(int mode)
+{
+	int ret = 0;
+
+	pr_info(MODULE_NAME "%s : fm34_set_mode mode[%d]\n", __func__, mode);
+
+	if (mode == 0)
+		ret = fm34_set_bypass_mode();  /* OFF,Bypass */
+	else if (mode == 1)
+		ret = fm34_set_HS_mode(); /* Receiver */
+	else if (mode == 2)
+		ret = fm34_set_bypass_mode(); /* S/W Bypass */
+	else if (mode == 3)
+		ret = fm34_set_SPK_mode(); /* Speaker */
+	else if (mode == 4)
+		ret = fm34_set_hw_bypass_mode(); /* PwrDn H/W Bypass */
+	else if (mode == 5)
+		ret = fm34_set_HS_NS_mode(); /* Receiver+NS */
+	else if (mode == 6)
+		ret = fm34_set_HS_ExtraVol_mode(); /* Receiver+ExtraVol */
+	else if (mode == 7)
+		ret = fm34_set_SPK_ExtraVol_mode(); /* Speaker+ExtraVol */
+	else if (mode == 8)
+		ret = fm34_set_EP_mode(); /* Headset */
+	else if (mode == 9)
+		ret = fm34_set_BTSCO_mode(); /* BT SCO */
+	else if (mode == 11)
+		ret = fm34_set_factory_rcv_mode(); /* Factory Mode RCV */
+	else if (mode == 12)
+		ret = fm34_set_factory_spk_mode(); /* Factory Mode SPK */
+	else
+		pr_err(MODULE_NAME"fm34_set_mode : INVALID mode[%d]\n", mode);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(fm34_set_mode);
+#else
+int fm34_set_bypass_mode(void)
+{
+	int i = 0, rc = 0, size = 0;
+	unsigned char *i2c_cmds;
+
+	pr_info(MODULE_NAME "%s\n", __func__);
+
+	i2c_cmds = bypass_cmd;
+	size = sizeof(bypass_cmd);
+
+#if DEBUG
+	for (i = 0; i < size; i += 1)
+		pr_info(MODULE_NAME "%s : i2c_cmds[%d/%d] = 0x%x\n",
+			__func__, i, size, i2c_cmds[i]);
+#endif
+	rc = fm34_i2c_write(i2c_cmds, size);
+
+
+	if (rc < 0) {
+		pr_err(MODULE_NAME "%s failed return %d\n", __func__, rc);
+	} else if (pdata->gpio_pwdn) {
+		msleep(20);
+		gpio_set_value(pdata->gpio_pwdn, 0);
+	}
+
+	return rc;
+}
+#endif
+
+static struct miscdevice fm34_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "fm34_we395",
+};
+
+static void fm34_gpio_init(void)
+{
+	if (pdata->gpio_rst) {
+		gpio_request(pdata->gpio_rst, "FM34_RESET");
+		gpio_direction_output(pdata->gpio_rst, 1);
+		gpio_free(pdata->gpio_rst);
+		gpio_set_value(pdata->gpio_rst, 0);
+	}
+
+	usleep_range(10000, 10000);
+
+	if (pdata->gpio_pwdn) {
+		gpio_request(pdata->gpio_pwdn, "FM34_PWDN");
+		gpio_direction_output(pdata->gpio_pwdn, 1);
+		gpio_free(pdata->gpio_pwdn);
+		gpio_set_value(pdata->gpio_pwdn, 1);
+	}
+
+	if (pdata->gpio_bp) {
+		gpio_request(pdata->gpio_bp, "FM34_BYPASS");
+		gpio_direction_output(pdata->gpio_bp, 1);
+		gpio_free(pdata->gpio_bp);
+		gpio_set_value(pdata->gpio_bp, 1);
+	}
+}
+
+static void fm34_bootup_init(void)
+{
+	if (pdata->set_mclk != NULL)
+		pdata->set_mclk(true, false);
+
+	msleep(20);
+	if (pdata->gpio_rst) {
+		gpio_set_value(pdata->gpio_rst, 0);
+		msleep(20);
+		gpio_set_value(pdata->gpio_rst, 1);
+	}
+	msleep(50);
+}
+
+
+static int fm34_probe(
+		struct i2c_client *client, const struct i2c_device_id *id)
+{
+	int rc = 0;
+	pdata = client->dev.platform_data;
+
+	pr_info(MODULE_NAME "%s : start\n", __func__);
+
+	if (pdata == NULL) {
+		pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+		if (pdata == NULL) {
+			rc = -ENOMEM;
+			pr_err(MODULE_NAME "%s: platform data is NULL\n",
+					__func__);
+		}
+	}
+
+	this_client = client;
+
+	fm34_gpio_init();
+	fm34_bootup_init();
+
+	if (fm34_set_bypass_mode() < 0)
+		pr_err(MODULE_NAME "bypass setting failed %d\n", rc);
+
+	pr_info(MODULE_NAME "%s : finish\n", __func__);
+
+	return rc;
+}
+
+static int fm34_remove(struct i2c_client *client)
+{
+	struct fm34_platform_data *pfm34data = i2c_get_clientdata(client);
+	kfree(pfm34data);
+
+	return 0;
+}
+
+static int fm34_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	return 0;
+}
+
+static int fm34_resume(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id fm34_id[] = {
+	{ "fm34_we395", 0 },
+	{ }
+};
+
+static struct i2c_driver fm34_driver = {
+	.probe = fm34_probe,
+	.remove = fm34_remove,
+	.suspend = fm34_suspend,
+	.resume	= fm34_resume,
+	.id_table = fm34_id,
+	.driver = {
+		.name = "fm34_we395",
+	},
+};
+
+static int __init fm34_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return i2c_add_driver(&fm34_driver);
+}
+
+static void __exit fm34_exit(void)
+{
+	i2c_del_driver(&fm34_driver);
+}
+
+module_init(fm34_init);
+module_exit(fm34_exit);
+
+MODULE_DESCRIPTION("fm34 voice processor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/inv_mpu/Kconfig b/drivers/misc/inv_mpu/Kconfig
new file mode 100644
index 0000000..060b79f
--- /dev/null
+++ b/drivers/misc/inv_mpu/Kconfig
@@ -0,0 +1,24 @@
+config MPU_SENSORS_GYRO
+	tristate "MPU6050 built in gyroscope"
+	depends on MPU_SENSORS_MPU6050
+
+config MPU_SENSORS_TIMERIRQ
+    tristate "Timer IRQ"
+    depends on MPU_SENSORS_MPU6050
+    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 "MPU6050 MPU debug"
+    depends on MPU_SENSORS_TIMERIRQ
+    help
+      If you say yes here you get extra debug messages from the MPU6050
+      and other slave sensors.
+      
+config MPU_SENSORS_CORE
+	tristate "Sensors core"
+	
+
diff --git a/drivers/misc/inv_mpu/Makefile b/drivers/misc/inv_mpu/Makefile
new file mode 100644
index 0000000..c129cf2
--- /dev/null
+++ b/drivers/misc/inv_mpu/Makefile
@@ -0,0 +1,22 @@
+
+# Kernel makefile for motions sensors
+#
+
+# MPU
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050)	+= mpu6050.o
+mpu6050-objs += mpuirq.o \
+	slaveirq.o \
+	mpu-dev.o \
+	mlsl-kernel.o \
+	mldl_cfg.o \
+	mldl_print_cfg.o \
+	sensors_core.o \
+	accel/mpu6050.o \
+	compass/ak8975.o
+
+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
diff --git a/drivers/misc/inv_mpu/README b/drivers/misc/inv_mpu/README
new file mode 100644
index 0000000..ce592c8
--- /dev/null
+++ b/drivers/misc/inv_mpu/README
@@ -0,0 +1,104 @@
+Kernel driver mpu
+=====================
+
+Supported chips:
+  * InvenSense IMU3050
+    Prefix: 'mpu3050'
+    Datasheet:
+        PS-MPU-3000A-00.2.4b.pdf
+
+Author: InvenSense <http://invensense.com>
+
+Description
+-----------
+The mpu is a motion processor unit that controls the mpu3050 gyroscope, a slave
+accelerometer, a compass and a pressure sensor.  This document describes how to
+install the driver into a Linux kernel.
+
+Sysfs entries
+-------------
+/dev/mpu
+/dev/mpuirq
+/dev/accelirq
+/dev/compassirq
+/dev/pressureirq
+
+General Remarks MPU3050
+-----------------------
+* Valid addresses for the MPU3050 is 0x68.
+* Accelerometer must be on the secondary I2C bus for MPU3050, the
+  magnetometer must be on the primary bus and pressure sensor must
+  be on the primary bus.
+
+Programming the chip using /dev/mpu
+----------------------------------
+Programming of MPU3050 is done by first opening the /dev/mpu file and
+then performing a series of IOCTLS on the handle returned.  The IOCTL codes can
+be found in mpu.h.  Typically this is done by the mllite library in user
+space.
+
+Board and Platform Data
+-----------------------
+
+In order for the driver to work, board and platform data specific to the device
+needs to be added to the board file.  A mpu_platform_data structure must
+be created and populated and set in the i2c_board_info_structure.  For details
+of each structure member see mpu.h.  All values below are simply an example and
+should be modified for your platform.
+
+#include <linux/mpu.h>
+
+static struct mpu_platform_data mpu3050_data = {
+	.int_config  = 0x10,
+	.orientation = {  -1,  0,  0,
+			   0,  1,  0,
+			   0,  0, -1 },
+};
+
+/* accel */
+static struct ext_slave_platform_data inv_mpu_kxtf9_data = {
+	.bus         = EXT_SLAVE_BUS_SECONDARY,
+	.orientation = {  -1,  0,  0,
+			  0,  1,  0,
+			  0,  0, -1 },
+};
+/* compass */
+static struct ext_slave_platform_data inv_mpu_ak8975_data = {
+	.bus         = EXT_SLAVE_BUS_PRIMARY,
+	.orientation = { 1, 0, 0,
+			 0, 1, 0,
+			 0, 0, 1 },
+};
+
+static struct i2c_board_info __initdata panda_inv_mpu_i2c4_boardinfo[] = {
+	{
+		I2C_BOARD_INFO("mpu3050", 0x68),
+		.irq = (IH_GPIO_BASE + MPUIRQ_GPIO),
+		.platform_data = &mpu3050_data,
+	},
+	{
+		I2C_BOARD_INFO("kxtf9", 0x0F),
+		.irq = (IH_GPIO_BASE + ACCEL_IRQ_GPIO),
+		.platform_data = &inv_mpu_kxtf9_data
+	},
+	{
+		I2C_BOARD_INFO("ak8975", 0x0E),
+		.irq = (IH_GPIO_BASE + COMPASS_IRQ_GPIO),
+		.platform_data = &inv_mpu_ak8975_data,
+	},
+};
+
+Typically the IRQ is a GPIO input pin and needs to be configured properly.  If
+in the above example GPIO 168 corresponds to IRQ 299, the following should be
+done as well:
+
+#define MPU_GPIO_IRQ 168
+
+    gpio_request(MPU_GPIO_IRQ,"MPUIRQ");
+    gpio_direction_input(MPU_GPIO_IRQ)
+
+Dynamic Debug
+=============
+
+The mpu3050 makes use of dynamic debug.  For details on how to use this
+refer to Documentation/dynamic-debug-howto.txt
diff --git a/drivers/misc/inv_mpu/accel/Kconfig b/drivers/misc/inv_mpu/accel/Kconfig
new file mode 100644
index 0000000..8e24177
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/Kconfig
@@ -0,0 +1,133 @@
+menuconfig MPU_SENSORS_ACCEL
+	bool "Accelerometer Slave Sensors"
+	default n
+	---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 MPU_SENSORS_ACCEL
+
+config MPU_SENSORS_ADXL34X
+	bool "ADI adxl34x"
+	depends on 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
+	bool "Bosch BMA222"
+	depends on 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
+	bool "Bosch BMA150"
+	depends on 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
+	bool "Bosch BMA250"
+	depends on 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
+	bool "Kionix KXSD9"
+	depends on 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
+	bool "Kionix KXTF9"
+	depends on 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
+	bool "ST lis331dlh"
+	depends on 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
+	bool "ST lis3dh"
+	depends on 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
+	bool "ST lsm303dlx"
+	depends on 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
+	bool "Freescale mma8450"
+	depends on 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
+	bool "Freescale mma8451/8452/8453"
+	depends on 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_BUILTIN_ACCEL
+	bool "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 0000000..1f0f5be
--- /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 0000000..f2bff8a
--- /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 0000000..c35f43a
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/bma150.c
@@ -0,0 +1,776 @@
+/*
+	$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 0000000..e9fc99b
--- /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 0000000..6a245f4
--- /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 0000000..496d1f2
--- /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 0000000..5cb4eaf
--- /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 0000000..80776f2
--- /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 0000000..bcbec25
--- /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 0000000..27206e4
--- /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 0000000..576282a
--- /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 0000000..f698ee9
--- /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 0000000..5f62b22
--- /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 0000000..114164e
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/mpu6050.c
@@ -0,0 +1,732 @@
+/*
+	$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_411.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;
+
+	return 0;
+
+	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(20);
+
+	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;
+	int x, y, z;
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 slave->read_reg, slave->read_len, data);
+#if 1
+	if (slave->read_len == 6) {
+		/*
+		pr_info("\n mantis_read cal x: %d y: %d z: %d\",
+				cal_data.x, cal_data.y, cal_data.z);
+
+		x = (s16)((data[0] <<8) |data[1]);
+		y = (s16)((data[2] <<8) |data[3]);
+		z = (s16)((data[4] <<8) |data[5]);
+
+		pr_info("mantis_read RAW x: %d y: %d z: %d", x, y, z);
+		*/
+		x = (s16)((data[0] << 8) | data[1]) - cal_data.x;
+		y = (s16)((data[2] << 8) | data[3]) - cal_data.y;
+		z = (s16)((data[4] << 8) | data[5]) - cal_data.z;
+
+		/*
+		pr_info("mantis_read CAL x: %d y: %d z: %d", x, y, z);
+		*/
+
+		data[0] = (x & 0xff00) >> 8;
+		data[1] = ((x << 4) >> 4) & 0xff;
+		data[2] = (y & 0xff00) >> 8;
+		data[3] = ((y << 4) >> 4) & 0xff;
+		data[4] = (z & 0xff00) >> 8;
+		data[5] = ((z << 4) >> 4) & 0xff;
+/*
+		x = (s16)((data[0] <<8) |data[1]);
+		y = (s16)((data[2] <<8) |data[3]);
+		z = (s16)((data[4] <<8) |data[5]);
+
+		pr_info("mantis_read CHK x: %d y: %d z: %d", x, y, z);
+*/
+	}
+#endif
+	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 0000000..a779255
--- /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_411.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 0000000..0881d8d
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/Kconfig
@@ -0,0 +1,94 @@
+menuconfig INV_SENSORS_MPU6050_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_MPU6050_COMPASS
+
+config MPU_SENSORS_MPU6050_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_MPU6050_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_MPU6050_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_MPU6050_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_MPU6050_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_MPU6050_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_MPU6050_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_MPU6050_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_MPU6050_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 0000000..5533d84
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/Makefile
@@ -0,0 +1,30 @@
+#
+# Compass Slaves MPUxxxx
+#
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050_LSM303DLX_M) += inv_mpu_lsm303dlx_m.o
+inv_mpu_lsm303dlx_m-objs +=	lsm303dlx_m.o
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050_MMC314X) += inv_mpu_mmc314x.o
+inv_mpu_mmc314x-objs +=	mmc314x.o
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050_YAS529) += inv_mpu_yas529.o
+inv_mpu_yas529-objs +=	yas529-kernel.o
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050_YAS530_411) += inv_mpu_yas530.o
+inv_mpu_yas530-objs +=	yas530.o
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050_HSCDTD002B) += inv_mpu_hscdtd002b.o
+inv_mpu_hscdtd002b-objs +=	hscdtd002b.o
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050_HSCDTD004A) += inv_mpu_hscdtd004a.o
+inv_mpu_hscdtd004a-objs +=	hscdtd004a.o
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050_AK8975) += inv_mpu_ak8975.o
+inv_mpu_ak8975-objs +=	ak8975.o
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050_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 0000000..7eb15b4
--- /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 0000000..b8dea1b
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ak8975.c
@@ -0,0 +1,504 @@
+/*
+	$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         (0x10)
+#define AK8975_CNTL_MODE_SINGLE_MEASUREMENT (0x11)
+#define AK8975_CNTL_MODE_FUSE_ROM_ACCESS    (0x1f)
+
+/* -------------------------------------------------------------------------- */
+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);
+		kfree(private_data);
+		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);
+		kfree(private_data);
+		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);
+		kfree(private_data);
+		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);
+		kfree(private_data);
+		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(20);		/* 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_mod", 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/hscdtd002b.c b/drivers/misc/inv_mpu/compass/hscdtd002b.c
new file mode 100644
index 0000000..4f6013c
--- /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 0000000..f091559
--- /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 0000000..32f8cdd
--- /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 0000000..786fadc
--- /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 0000000..f53223f
--- /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 0000000..263990f
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/yas530.c
@@ -0,0 +1,596 @@
+/*
+	$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_411.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 int yas530_get_config(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata,
+			     struct ext_slave_config *data)
+{
+	int result = INV_SUCCESS;
+	//struct yas530_private_data *private_data = pdata->private_data;
+
+	switch (data->key)
+	{
+		default:
+			return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	}
+	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       = yas530_get_config,
+	.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/compass/yas530_ext.c b/drivers/misc/inv_mpu/compass/yas530_ext.c
new file mode 100644
index 0000000..7a64258
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/yas530_ext.c
@@ -0,0 +1,288 @@
+#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"
+
+struct yas530_ext_private_data {
+	int flags;
+	char offsets[3];
+	const int *correction_matrix;
+};
+
+
+extern int geomagnetic_api_read(int *xyz, int *raw, int *xy1y2, int *accuracy);
+extern int geomagnetic_api_resume(void);
+extern int geomagnetic_api_suspend(void);
+
+
+static int yas530_ext_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	geomagnetic_api_suspend();
+
+	return result;
+}
+
+
+static int yas530_ext_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	struct yas530_ext_private_data *private_data = pdata->private_data;
+
+	geomagnetic_api_resume();
+
+	return result;
+}
+
+static int yas530_ext_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result = INV_SUCCESS;
+	int raw[3] = {0,};
+	int xyz[3] = {0,};
+	int accuracy = 0;
+	int i = 0;
+	short xyz_scaled[3] = {0,};
+
+	geomagnetic_api_read(xyz, raw, NULL, &accuracy);
+
+	xyz_scaled[0] = (short)(xyz[0]/100);
+	xyz_scaled[1] = (short)(xyz[1]/100);
+	xyz_scaled[2] = (short)(xyz[2]/100);
+
+	data[0] = xyz_scaled[0] >> 8;
+	data[1] = xyz_scaled[0] & 0xFF;
+	data[2] = xyz_scaled[1] >> 8;
+	data[3] = xyz_scaled[1] & 0xFF;
+	data[4] = xyz_scaled[2] >> 8;
+	data[5] = xyz_scaled[2] & 0xFF;
+	data[6] = (unsigned char)accuracy;
+
+
+	return result;
+
+}
+
+static int yas530_ext_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	int result = INV_SUCCESS;
+	struct yas530_private_data *private_data = pdata->private_data;
+
+
+	return result;
+
+}
+
+
+static int yas530_ext_get_config(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata,
+			     struct ext_slave_config *data)
+{
+	int result = INV_SUCCESS;
+	struct yas530_ext_private_data *private_data = pdata->private_data;
+
+	switch (data->key)
+	{
+		default:
+			return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	}
+	return result;
+}
+
+static int yas530_ext_init(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+
+	struct yas530_ext_private_data *private_data;
+	int result = INV_SUCCESS;
+	char offset[3] = {0, 0, 0};
+
+	private_data = (struct yas530_ext_private_data *)
+			kzalloc(sizeof(struct yas530_ext_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	private_data->correction_matrix = pdata->private_data;
+
+	pdata->private_data = private_data;
+
+	return result;
+}
+
+static int yas530_ext_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_descr yas530_ext_descr = {
+	.init             = yas530_ext_init,
+	.exit             = yas530_ext_exit,
+	.suspend          = yas530_ext_suspend,
+	.resume           = yas530_ext_resume,
+	.read             = yas530_ext_read,
+	.config           = yas530_ext_config,
+	.get_config       = yas530_ext_get_config,
+	.name             = "yas530ext",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_YAS530_EXT,
+	.read_reg         = 0x06,
+	.read_len         = 7,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {3276, 8001},
+	.trigger          = NULL,
+};
+
+
+struct ext_slave_descr *yas530_ext_get_slave_descr(void)
+{
+	return &yas530_ext_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct yas530_ext_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int yas530_ext_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct yas530_ext_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_ext_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_ext_mod_remove(struct i2c_client *client)
+{
+	struct yas530_ext_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_ext_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id yas530_ext_mod_id[] = {
+	{ "yas530ext", COMPASS_ID_YAS530_EXT},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, yas530_ext_mod_id);
+
+static struct i2c_driver yas530_ext_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = yas530_ext_mod_probe,
+	.remove = yas530_ext_mod_remove,
+	.id_table = yas530_ext_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "yas530_ext_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init yas530_ext_mod_init(void)
+{
+	int res = i2c_add_driver(&yas530_ext_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "yas530_ext_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit yas530_ext_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&yas530_ext_mod_driver);
+}
+
+module_init(yas530_ext_mod_init);
+module_exit(yas530_ext_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate YAS530 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("yas530_ext_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/yas530_ext.h b/drivers/misc/inv_mpu/compass/yas530_ext.h
new file mode 100644
index 0000000..0c343ec
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/yas530_ext.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 __YAS530_EXT_H__
+#define __YAS530_EXT_H__
+
+#include <linux/mpu.h>
+
+struct ext_slave_descr *yas530_ext_get_slave_descr(void);
+
+#endif
diff --git a/drivers/misc/inv_mpu/log.h b/drivers/misc/inv_mpu/log.h
new file mode 100644
index 0000000..5630602e
--- /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.c b/drivers/misc/inv_mpu/mldl_cfg.c
new file mode 100644
index 0000000..e2d9900
--- /dev/null
+++ b/drivers/misc/inv_mpu/mldl_cfg.c
@@ -0,0 +1,1913 @@
+/*
+	$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_411.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(20);
+		}
+		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)   */
+	{MPL_PROD_KEY(40, 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;
+	pr_info("%s", __func__);
+	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;
+	}
+
+	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;
+	}
+	pr_info("%s key=%d", __func__, key);
+	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(20);
+	}
+
+	/* 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;
+		}
+#if 0
+		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;
+			}
+		}
+#endif
+		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]) {
+		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/mldl_cfg.h b/drivers/misc/inv_mpu/mldl_cfg.h
new file mode 100644
index 0000000..1d676a9
--- /dev/null
+++ b/drivers/misc/inv_mpu/mldl_cfg.h
@@ -0,0 +1,381 @@
+/*
+	$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_411.h>
+#include "mpu6050b1.h"
+
+#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;
+};
+
+#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)
+
+/* Driver related state information */
+struct inv_mpu_state {
+	__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.c b/drivers/misc/inv_mpu/mldl_print_cfg.c
new file mode 100644
index 0000000..78d4090
--- /dev/null
+++ b/drivers/misc/inv_mpu/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_411.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/mldl_print_cfg.h b/drivers/misc/inv_mpu/mldl_print_cfg.h
new file mode 100644
index 0000000..2e19114
--- /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-kernel.c b/drivers/misc/inv_mpu/mlsl-kernel.c
new file mode 100644
index 0000000..f1c228f
--- /dev/null
+++ b/drivers/misc/inv_mpu/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/mlsl.h b/drivers/misc/inv_mpu/mlsl.h
new file mode 100644
index 0000000..3fc6be9
--- /dev/null
+++ b/drivers/misc/inv_mpu/mlsl.h
@@ -0,0 +1,193 @@
+/*
+	$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_411.h>
+
+
+/* acceleration data */
+struct acc_data {
+	s16 x;
+	s16 y;
+	s16 z;
+};
+
+/*
+ * 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 0000000..a249f93
--- /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.c b/drivers/misc/inv_mpu/mpu-dev.c
new file mode 100644
index 0000000..c025f50
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu-dev.c
@@ -0,0 +1,2348 @@
+/*
+	$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/delay.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_411.h>
+
+#include "accel/mpu6050.h"
+#include "mpu-dev.h"
+
+
+#ifdef CONFIG_INPUT_YAS_MAGNETOMETER
+#include "compass/yas530_ext.h"
+#endif
+
+#include <linux/akm8975.h>
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+#define MAG_VENDOR	"AKM"
+#define MAG_PART_ID	"AK8963C"
+#define MPU_VENDOR	"INVENSENSE"
+#define MPU_PART_ID	"MPU-6050"
+
+#define MPU_EARLY_SUSPEND_IN_DRIVER 1
+
+
+#define CALIBRATION_FILE_PATH	"/efs/calibration_data"
+#define CALIBRATION_DATA_AMOUNT	100
+
+struct acc_data cal_data = {0, 0, 0};
+
+
+
+/* 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;
+	unsigned long		event;
+	int			pid;
+	struct module *slave_modules[EXT_SLAVE_NUM_TYPES];
+	struct {
+		atomic_t enable;
+		unsigned char is_activated;
+		unsigned char turned_by_mpu_accel;
+	} mpu_accel;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+};
+
+static struct i2c_client *this_client;
+
+#define IDEAL_X 0
+#define IDEAL_Y 0
+#define IDEAL_Z -1024
+/*#define CAL_DIV 8*/
+static int CAL_DIV = 8;
+
+struct mpu_private_data *mpu_data;
+
+void mpu_accel_enable_set(int enable)
+{
+
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(this_client);
+	struct i2c_client *client = mpu->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;
+
+	if (enable) {
+		mpu->mpu_accel.is_activated =
+			!(mldl_cfg->inv_mpu_state->status
+			& MPU_ACCEL_IS_SUSPENDED);
+
+			(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],
+				INV_THREE_AXIS_ACCEL);
+
+			mpu->mpu_accel.turned_by_mpu_accel = 1;
+	} else {
+		if (!mpu->mpu_accel.is_activated
+			&& mpu->mpu_accel.turned_by_mpu_accel) {
+
+			(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_THREE_AXIS_ACCEL);
+
+			mpu->mpu_accel.turned_by_mpu_accel = 0;
+		}
+	}
+
+	atomic_set(&mpu->mpu_accel.enable, enable);
+}
+
+int read_accel_raw_xyz(struct acc_data *acc)
+{
+	s16 x, y, z;
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(this_client);
+	struct i2c_client *client = mpu->client;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	int retval = 0;
+	unsigned char data[6];
+
+	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;
+
+
+	retval = inv_serial_read(slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			0x68, 0x3B, 6, data);
+
+	if (mldl_cfg->mpu_chip_info->accel_sens_trim == 16384)
+		CAL_DIV = 16;
+	x = (s16)((data[0] << 8) | data[1])/CAL_DIV;
+	y = (s16)((data[2] << 8) | data[3])/CAL_DIV;
+	z = (s16)((data[4] << 8) | data[5])/CAL_DIV;
+
+	acc->x = x;
+
+	acc->y = y;
+
+	acc->z = z;
+
+	/*
+	pr_info("read_accel_raw_xyz acc x: %d y: %d z: %d",
+			acc->x,acc->y,acc->z);
+	*/
+	return 0;
+}
+
+static int accel_open_calibration(void)
+{
+	struct file *cal_filp = NULL;
+	int err = 0;
+	mm_segment_t old_fs;
+
+	old_fs = get_fs();
+	set_fs(KERNEL_DS);
+
+	cal_filp = filp_open(CALIBRATION_FILE_PATH, O_RDONLY, 0666);
+	if (IS_ERR(cal_filp)) {
+		pr_err("%s: Can't open calibration file", __func__);
+		set_fs(old_fs);
+		err = PTR_ERR(cal_filp);
+		return err;
+	}
+
+	err = cal_filp->f_op->read(cal_filp,
+		(char *)&cal_data, 3 * sizeof(s16), &cal_filp->f_pos);
+	if (err != 3 * sizeof(s16)) {
+		pr_err("%s: Can't read the cal data from file", __func__);
+		err = -EIO;
+	}
+
+	pr_info("%s: (%u,%u,%u)", __func__,
+		cal_data.x, cal_data.y, cal_data.z);
+
+	filp_close(cal_filp, current->files);
+	set_fs(old_fs);
+
+	return err;
+}
+
+static int accel_do_calibrate(int enable)
+{
+	struct acc_data data = { 0, };
+	struct file *cal_filp = NULL;
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(this_client);
+	/* struct i2c_client *client = mpu->client; */
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+
+	int sum[3] = { 0, };
+	int err = 0;
+	int i;
+	mm_segment_t old_fs;
+
+	/* mutex_lock(&mpu->mutex); */
+	mpu_accel_enable_set(1);
+	msleep(20);
+
+	for (i = 0; i < CALIBRATION_DATA_AMOUNT; i++) {
+		err = read_accel_raw_xyz(&data);
+		if (err < 0) {
+			pr_err("%s: accel_read_accel_raw_xyz() "
+				"failed in the %dth loop", __func__, i);
+			return err;
+		}
+
+		sum[0] += data.x;
+		sum[1] += data.y;
+		sum[2] += data.z;
+	}
+
+	mpu_accel_enable_set(0);
+	msleep(20);
+	/* mutex_unlock(&mpu->mutex); */
+
+	if (mldl_cfg->mpu_chip_info->accel_sens_trim == 16384)
+		CAL_DIV = 16;
+
+	if (enable) {
+		cal_data.x = ((sum[0] / CALIBRATION_DATA_AMOUNT)
+				- IDEAL_X)*CAL_DIV;
+		cal_data.y = ((sum[1] / CALIBRATION_DATA_AMOUNT)
+				- IDEAL_Y)*CAL_DIV;
+		cal_data.z = ((sum[2] / CALIBRATION_DATA_AMOUNT)
+				- IDEAL_Z)*CAL_DIV;
+	} else {
+		cal_data.x = 0;
+		cal_data.y = 0;
+		cal_data.z = 0;
+	}
+
+	pr_info("%s: cal data (%d,%d,%d)", __func__,
+			cal_data.x, cal_data.y, cal_data.z);
+
+	old_fs = get_fs();
+	set_fs(KERNEL_DS);
+
+	cal_filp = filp_open(CALIBRATION_FILE_PATH,
+			O_CREAT | O_TRUNC | O_WRONLY, 0666);
+	if (IS_ERR(cal_filp)) {
+		pr_err("%s: Can't open calibration file", __func__);
+		set_fs(old_fs);
+		err = PTR_ERR(cal_filp);
+		return err;
+	}
+
+	err = cal_filp->f_op->write(cal_filp,
+		(char *)&cal_data, 3 * sizeof(s16), &cal_filp->f_pos);
+	if (err != 3 * sizeof(s16)) {
+		pr_err("%s: Can't write the cal data to file", __func__);
+		err = -EIO;
+	}
+
+	filp_close(cal_filp, current->files);
+	set_fs(old_fs);
+
+	return err;
+}
+
+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", __func__);
+	complete(&mpu->completion);
+}
+#if 0
+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", __func__, event);
+
+	/* Prevent the file handle from being closed before we initialize
+	   the completion event */
+	pr_info("[%s] event = %ld", __func__, 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", __func__, event);
+	return NOTIFY_OK;
+}
+#endif
+static int mpu_early_notifier_callback(struct mpu_private_data *mpu,
+				    unsigned long event, void *unused)
+{
+	struct i2c_client *client = mpu->client;
+	struct timeval event_time;
+	dev_dbg(&client->adapter->dev, "%s: %s", __func__,
+		(event == MPU_PM_EVENT_SUSPEND_PREPARE) ?
+		"MPU_PM_EVENT_SUSPEND_PREPARE" : "MPU_PM_EVENT_POST_SUSPEND");
+
+	/* Prevent the file handle from being closed before we initialize
+	   the completion event */
+	pr_info("[%s] event = %ld", __func__, 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: %s DONE", __func__,
+		(event == MPU_PM_EVENT_SUSPEND_PREPARE) ?
+		"MPU_PM_EVENT_SUSPEND_PREPARE" : "MPU_PM_EVENT_POST_SUSPEND");
+	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", __func__);
+	dev_dbg(&client->adapter->dev, "current->pid %d", current->pid);
+
+	accel_open_calibration();
+
+	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",
+			__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");
+
+	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", 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,
+			(unsigned char)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",
+			__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",
+			__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", __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;
+	case MPU_READ_ACCEL_OFFSET:
+		{
+
+			retval = copy_to_user((signed short __user *)arg,
+			  &cal_data, sizeof(cal_data));
+		      if (INV_SUCCESS != retval) {
+				dev_err(&client->adapter->dev,
+					"%s: cmd %x, arg %lu",
+					__func__, cmd, arg);
+			}
+		}
+		break;
+	default:
+		dev_err(&client->adapter->dev,
+			"%s: Unknown cmd %x, arg %lu",
+			__func__, cmd, arg);
+		retval = -EINVAL;
+	};
+
+	mutex_unlock(&mpu->mutex);
+	dev_dbg(&client->adapter->dev, "%s: %08x, %08lx, %d",
+		__func__, cmd, arg, retval);
+
+	if (retval > 0)
+		retval = -retval;
+
+	return retval;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+void mpu_dev_early_suspend(struct early_suspend *h)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(this_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;
+	pr_info("----------\n%s\n----------", __func__);
+
+	mpu_early_notifier_callback(mpu, PM_SUSPEND_PREPARE, NULL);
+
+	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] = this_client->adapter;
+	mutex_lock(&mpu->mutex);
+	if (!mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
+		(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);
+}
+
+void mpu_dev_early_resume(struct early_suspend *h)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(this_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;
+	pr_info("----------\n%s\n----------", __func__);
+
+	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] = this_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);
+	}
+	mutex_unlock(&mpu->mutex);
+	mpu_early_notifier_callback(mpu, PM_POST_SUSPEND, NULL);
+}
+#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 *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", __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;
+	pr_info("----------\n%s\n----------", __func__);
+	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", __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", __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;
+	pr_info("----------\n%s\n----------", __func__);
+	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", __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_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", __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", __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",
+		__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",
+				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",
+			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",
+				slave_descr->name, warn_result);
+	} else {
+		dev_warn(&slave_client->adapter->dev,
+			"%s irq not assigned: %d %d %d",
+			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_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);
+
+static int mpu6050_factory_on(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(this_client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	int prev_gyro_suspended = 0;
+
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	pr_info("----------\n%s\n----------", __func__);
+
+	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 (1) {
+		(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);
+	}
+	mutex_unlock(&mpu->mutex);
+	return prev_gyro_suspended;
+}
+
+static ssize_t mpu6050_power_on(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+
+	dev_dbg(dev, "this_client = %d\n", (int)this_client);
+	count = sprintf(buf, "%d\n", (this_client != NULL ? 1 : 0));
+
+	return count;
+}
+
+static ssize_t mpu6050_get_temp(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	short int temperature = 0;
+	unsigned char data[2];
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(this_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] = this_client->adapter;
+
+	mpu6050_factory_on(this_client);
+
+	/* MPUREG_TEMP_OUT_H, */	/* 27 0x1b */
+	/* MPUREG_TEMP_OUT_L, */	/* 28 0x1c */
+	/* TEMP_OUT_H/L: 16-bit temperature data (2's complement data format) */
+	inv_serial_read(slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			DEFAULT_MPU_SLAVEADDR,
+			MPUREG_TEMP_OUT_H,
+			2,
+			data);
+	temperature = (short) (((data[0]) << 8) | data[1]);
+	temperature = (((temperature + 521) / 340) + 35);
+	pr_info("read temperature = %d\n", temperature);
+
+	count = sprintf(buf, "%d\n", temperature);
+
+	return count;
+}
+
+static ssize_t mpu6050_acc_read(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+
+	s16 x, y, z;
+	int count = 0;
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(this_client);
+	struct i2c_client *client = mpu->client;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	int retval = 0;
+	unsigned char data[6];
+
+	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); */
+	mpu_accel_enable_set(1);
+	msleep(20);
+	retval = inv_serial_read(slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+				0x68, 0x3B, 6, data);
+
+	x = (s16)(((data[0] << 8) | data[1]) - cal_data.x);/*CAL_DIV;*/
+	y = (s16)(((data[2] << 8) | data[3]) - cal_data.y);/*CAL_DIV;*/
+	z = (s16)(((data[4] << 8) | data[5]) - cal_data.z);/*CAL_DIV;*/
+
+	z *= -1;
+
+	pr_info("mpu6050_acc_read x: %d y: %d z: %d", y, x, z);
+	mpu_accel_enable_set(0);
+	msleep(20);
+	/* mutex_unlock(&mpu->mutex); */
+
+	count = sprintf(buf, "%d, %d, %d\n", y, x, z);
+
+	return count;
+}
+
+static ssize_t accel_calibration_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+
+	int count = 0;
+
+	pr_info(" accel_calibration_show %d %d %d",
+		cal_data.x, cal_data.y, cal_data.z);
+
+	count = sprintf(buf, "%d %d %d\n", cal_data.x, cal_data.y, cal_data.z);
+	return count;
+}
+
+static ssize_t accel_calibration_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t size)
+{
+	bool do_calib;
+	int err;
+	int count = 0;
+	char str[11];
+
+	if (sysfs_streq(buf, "1"))
+		do_calib = true;
+	else if (sysfs_streq(buf, "0"))
+		do_calib = false;
+	else {
+		pr_debug("%s: invalid value %d", __func__, *buf);
+		return -EINVAL;
+	}
+
+	err = accel_do_calibrate(do_calib);
+	if (err < 0)
+		pr_err("%s: accel_do_calibrate() failed", __func__);
+
+	pr_info("accel_calibration_show :%d %d %d",
+		cal_data.x, cal_data.y, cal_data.z);
+	if (err > 0)
+		err = 0;
+	count = sprintf(str, "%d\n", err);
+
+	strcpy(str, buf);
+	return count;
+}
+
+static ssize_t mpu_vendor_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s\n", MPU_VENDOR);
+}
+
+static ssize_t mpu_name_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s\n", MPU_PART_ID);
+}
+
+static int akm8975_wait_for_data_ready(struct i2c_adapter *sl_adapter)
+{
+	int err;
+	u8 buf;
+	int count = 10;
+
+	while (1) {
+		msleep(20);
+		err = inv_serial_read(sl_adapter, 0x0C,
+				AK8975_REG_ST1, sizeof(buf), &buf);
+		if (err) {
+			pr_err("%s: read data over i2c failed\n", __func__);
+			return -EIO;
+		}
+
+		if (buf&0x1)
+			break;
+
+		count--;
+		if (!count)
+			break;
+	}
+	return 0;
+
+}
+
+static ssize_t ak8975_adc(struct device *dev,
+		struct device_attribute *attr, char *strbuf)
+{
+	struct mpu_private_data *mpu =
+		(struct mpu_private_data *) i2c_get_clientdata(this_client);
+	struct i2c_client *client = mpu->client;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+
+
+	u8 buf[8];
+	s16 x, y, z;
+	int err, success;
+	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);
+	}
+
+	pr_info("%s %s", __func__, client->name);
+
+	mutex_lock(&mpu->mutex);
+
+	/* start ADC conversion */
+	err = inv_serial_single_write(slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+	0x0C, AK8975_REG_CNTL, AK8975_MODE_SNG_MEASURE);
+
+	if (err)
+		pr_err("ak8975_adc write err:%d\n", err);
+
+	/* wait for ADC conversion to complete */
+
+	err = akm8975_wait_for_data_ready
+		(slave_adapter[EXT_SLAVE_TYPE_COMPASS]);
+	if (err) {
+		pr_err("%s: wait for data ready failed\n", __func__);
+		return err;
+	}
+
+	msleep(20);/*msleep(10);*/
+	/* get the value and report it */
+	err = inv_serial_read(slave_adapter[EXT_SLAVE_TYPE_COMPASS], 0x0C,
+					AK8975_REG_ST1, sizeof(buf), buf);
+
+	if (err) {
+		pr_err("%s: read data over i2c failed %d\n", __func__, err);
+		mutex_unlock(&mpu->mutex);
+		return -EIO;
+	}
+	mutex_unlock(&mpu->mutex);
+
+	/* buf[0] is status1, buf[7] is status2 */
+	if ((buf[0] == 0) | (buf[7] == 1))
+		success = 0;
+	else
+		success = 1;
+
+	x = buf[1] | (buf[2] << 8);
+	y = buf[3] | (buf[4] << 8);
+	z = buf[5] | (buf[6] << 8);
+
+	pr_err("%s: raw x = %d, y = %d, z = %d\n", __func__, x, y, z);
+
+	return snprintf(strbuf, PAGE_SIZE, "%s, %d, %d, %d\n",
+		(success ? "OK" : "NG"), x, y, z);
+}
+
+static ssize_t ak8975_check_cntl(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(this_client);
+	struct i2c_client *client = mpu->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, err;
+	u8 data;
+	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);
+	}
+	mutex_lock(&mpu->mutex);
+	err = inv_serial_single_write(slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+		0x0C, AK8975_REG_CNTL,
+		AK8975_MODE_POWER_DOWN);
+
+	if (err) {
+		pr_err("ak8975_adc write err:%d\n", err);
+		mutex_unlock(&mpu->mutex);
+		return -EIO;
+	}
+	err = inv_serial_read(slave_adapter[EXT_SLAVE_TYPE_COMPASS], 0x0C,
+				AK8975_REG_CNTL, sizeof(data), &data);
+	if (err) {
+		pr_err("%s: read data over i2c failed %d\n", __func__, err);
+		mutex_unlock(&mpu->mutex);
+		return -EIO;
+	}
+	mutex_unlock(&mpu->mutex);
+
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+		data == AK8975_MODE_POWER_DOWN ? "OK" : "NG");
+
+}
+
+static ssize_t akm8975_rawdata_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(this_client);
+	struct i2c_client *client = mpu->client;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+
+	short x = 0, y = 0, z = 0;
+	int err;
+	u8 data[8];
+
+	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);
+	}
+
+	mutex_lock(&mpu->mutex);
+	err = inv_serial_single_write(slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+		0x0C, AK8975_REG_CNTL,
+		AK8975_MODE_SNG_MEASURE);
+
+	if (err) {
+		pr_err("ak8975_adc write err:%d\n", err);
+		mutex_unlock(&mpu->mutex);
+		goto done;
+
+	}
+
+	err = akm8975_wait_for_data_ready
+		(slave_adapter[EXT_SLAVE_TYPE_COMPASS]);
+	if (err) {
+		mutex_unlock(&mpu->mutex);
+		goto done;
+	}
+
+	/* get the value and report it */
+	err = inv_serial_read(slave_adapter[EXT_SLAVE_TYPE_COMPASS], 0x0C,
+					AK8975_REG_ST1, sizeof(data), data);
+
+	if (err) {
+		pr_err("%s: read data over i2c failed %d\n", __func__, err);
+		mutex_unlock(&mpu->mutex);
+		return -EIO;
+	}
+
+	mutex_unlock(&mpu->mutex);
+
+	if (err) {
+		pr_err("%s: failed to read %d bytes of mag data\n",
+		       __func__, sizeof(data));
+		goto done;
+	}
+
+	if (data[0] & 0x01) {
+		x = (data[2] << 8) + data[1];
+		y = (data[4] << 8) + data[3];
+		z = (data[6] << 8) + data[5];
+	} else
+		pr_err("%s: invalid raw data(st1 = %d)\n",
+					__func__, data[0] & 0x01);
+
+done:
+	return snprintf(buf, PAGE_SIZE, "%d,%d,%d\n", x, y, z);
+}
+
+struct ak8975_config {
+	char asa[COMPASS_NUM_AXES];	/* axis sensitivity adjustment */
+};
+
+struct ak8975_private_data {
+	struct ak8975_config init;
+};
+static ssize_t ak8975c_get_status(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(this_client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int success;
+
+	struct ak8975_private_data *private_data =
+		(struct ak8975_private_data *)
+		pdata_slave[EXT_SLAVE_TYPE_COMPASS]->private_data;
+	if ((private_data->init.asa[0] == 0) |
+		(private_data->init.asa[0] == 0xff) |
+		(private_data->init.asa[1] == 0) |
+		(private_data->init.asa[1] == 0xff) |
+		(private_data->init.asa[2] == 0) |
+		(private_data->init.asa[2] == 0xff))
+		success = 0;
+	else
+		success = 1;
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", (success ? "OK" : "NG"));
+
+}
+
+int ak8975c_selftest(struct i2c_adapter *slave_adapter,
+	struct ak8975_private_data *private_data, int *sf)
+{
+	int err;
+	u8 data;
+	u8 buf[6];
+	int count = 20;
+	s16 x, y, z;
+
+	/* set ATSC self test bit to 1 */
+	err = inv_serial_single_write(slave_adapter, 0x0C,
+	AK8975_REG_ASTC, 0x40);
+
+	/* start self test */
+	err = inv_serial_single_write(slave_adapter, 0x0C,
+	AK8975_REG_CNTL, AK8975_MODE_SELF_TEST);
+
+	/* wait for data ready */
+	while (1) {
+		msleep(20);
+		err = inv_serial_read(slave_adapter, 0x0C,
+			AK8975_REG_ST1, sizeof(data), &data);
+
+		if (data == 1)
+			break;
+		count--;
+		if (!count)
+			break;
+	}
+	err = inv_serial_read(slave_adapter, 0x0C,
+					AK8975_REG_HXL, sizeof(buf), buf);
+
+		/* set ATSC self test bit to 0 */
+	err = inv_serial_single_write(slave_adapter, 0x0C,
+					AK8975_REG_ASTC, 0x00);
+
+	x = buf[0] | (buf[1] << 8);
+	y = buf[2] | (buf[3] << 8);
+	z = buf[4] | (buf[5] << 8);
+
+	/* Hadj = (H*(Asa+128))/256 */
+	x = (x*(private_data->init.asa[0] + 128)) >> 8;
+	y = (y*(private_data->init.asa[1] + 128)) >> 8;
+	z = (z*(private_data->init.asa[2] + 128)) >> 8;
+
+	pr_info("%s: self test x = %d, y = %d, z = %d\n",
+		__func__, x, y, z);
+	if ((x >= -200) && (x <= 200))
+		pr_info("%s: x passed self test, expect -200<=x<=200\n",
+			__func__);
+	else
+		pr_info("%s: x failed self test, expect -200<=x<=200\n",
+			__func__);
+	if ((y >= -200) && (y <= 200))
+		pr_info("%s: y passed self test, expect -200<=y<=200\n",
+			__func__);
+	else
+		pr_info("%s: y failed self test, expect -200<=y<=200\n",
+			__func__);
+	if ((z >= -3200) && (z <= -800))
+		pr_info("%s: z passed self test, expect -3200<=z<=-800\n",
+			__func__);
+	else
+		pr_info("%s: z failed self test, expect -3200<=z<=-800\n",
+			__func__);
+
+	sf[0] = x;
+	sf[1] = y;
+	sf[2] = z;
+
+	if (((x >= -200) && (x <= 200)) &&
+		((y >= -200) && (y <= 200)) &&
+		((z >= -3200) && (z <= -800)))
+		return 1;
+	else
+		return 0;
+}
+
+static ssize_t ak8975c_get_selftest(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(this_client);
+	struct i2c_client *client = mpu->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;
+	struct ak8975_private_data *private_data =
+		(struct ak8975_private_data *)
+		pdata_slave[EXT_SLAVE_TYPE_COMPASS]->private_data;
+	int ii, success;
+	int sf[3] = {0,};
+	int retry = 3;
+
+	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);
+	}
+	do {
+		retry--;
+		success = ak8975c_selftest(
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			private_data, sf);
+		if (success)
+			break;
+	} while (retry > 0);
+
+	return snprintf(buf, PAGE_SIZE, "%d, %d, %d, %d\n",
+		success, sf[0], sf[1], sf[2]);
+}
+
+static ssize_t akm_vendor_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s\n", MAG_VENDOR);
+}
+
+static ssize_t akm_name_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s\n", MAG_PART_ID);
+}
+
+static DEVICE_ATTR(power_on, S_IRUGO, mpu6050_power_on, NULL);
+static DEVICE_ATTR(temperature, S_IRUGO, mpu6050_get_temp, NULL);
+
+static DEVICE_ATTR(calibration, S_IRUGO | S_IWUSR,
+		accel_calibration_show, accel_calibration_store);
+
+static DEVICE_ATTR(raw_data, S_IRUGO, mpu6050_acc_read, NULL);
+
+static DEVICE_ATTR(vendor, S_IRUGO, mpu_vendor_show, NULL);
+static DEVICE_ATTR(name, S_IRUGO, mpu_name_show, NULL);
+
+
+static DEVICE_ATTR(adc, S_IRUGO, ak8975_adc, NULL);
+
+static DEVICE_ATTR(dac, S_IRUGO, ak8975_check_cntl, NULL);
+static DEVICE_ATTR(status, S_IRUGO, ak8975c_get_status, NULL);
+static DEVICE_ATTR(selftest, S_IRUGO, ak8975c_get_selftest, NULL);
+
+static struct device_attribute dev_attr_mag_rawdata =
+	__ATTR(raw_data, S_IRUGO, akm8975_rawdata_show, NULL);
+
+static struct device_attribute dev_attr_mag_vendor =
+	__ATTR(vendor, S_IRUGO, akm_vendor_show, NULL);
+
+static struct device_attribute dev_attr_mag_name =
+	__ATTR(name, S_IRUGO, akm_name_show, NULL);
+
+static struct device_attribute *gyro_sensor_attrs[] = {
+	&dev_attr_power_on,
+	&dev_attr_temperature,
+	&dev_attr_vendor,
+	&dev_attr_name,
+	NULL,
+};
+
+static struct device_attribute *accel_sensor_attrs[] = {
+	&dev_attr_raw_data,
+	&dev_attr_calibration,
+	&dev_attr_vendor,
+	&dev_attr_name,
+	NULL,
+};
+
+static struct device_attribute *magnetic_sensor_attrs[] = {
+	&dev_attr_adc,
+	&dev_attr_mag_rawdata,
+	&dev_attr_dac,
+	&dev_attr_status,
+	&dev_attr_selftest,
+	&dev_attr_mag_vendor,
+	&dev_attr_mag_name,
+	NULL,
+};
+
+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;
+	struct device *gyro_sensor_device = NULL;
+	struct device *accel_sensor_device = NULL;
+	struct device *magnetic_sensor_device = NULL;
+	int res = 0;
+	int ii;
+
+	pr_info("===========\n%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;
+	}
+	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_data = mpu;
+	i2c_set_clientdata(client, mpu);
+	this_client = client;
+	mpu->client = client;
+
+	init_waitqueue_head(&mpu->mpu_event_wait);
+	mutex_init(&mpu->mutex);
+	init_completion(&mpu->completion);
+
+
+	mpu->response_timeout = 1;	/* Seconds */
+	mpu->timeout.function = mpu_pm_timeout;
+	mpu->timeout.data = (u_long) mpu;
+	init_timer(&mpu->timeout);
+#if 0
+	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", res);
+		goto out_register_pm_notifier_failed;
+	}
+#endif
+	pdata = (struct mpu_platform_data *)client->dev.platform_data;
+	if (!pdata) {
+		dev_warn(&client->adapter->dev,
+			 "Missing platform data for mpu");
+	}
+	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", 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", res);
+		goto out_misc_register_failed;
+	}
+
+	if (client->irq) {
+		dev_info(&client->adapter->dev,
+			 "Installing irq using %d", client->irq);
+		res = mpuirq_init(client, mldl_cfg);
+		if (res)
+			goto out_mpuirq_failed;
+	} else {
+		dev_warn(&client->adapter->dev,
+			 "Missing %s IRQ", 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;
+		}
+	}
+
+	res = sensors_register(gyro_sensor_device, NULL, gyro_sensor_attrs,
+				"gyro_sensor");
+	if (res) {
+		pr_err("%s: cound not register gyro sensor device(%d).",
+				__func__, res);
+		goto out_sensor_register_failed;
+	}
+
+	res = sensors_register(accel_sensor_device, NULL, accel_sensor_attrs,
+				"accelerometer_sensor");
+	if (res) {
+		pr_err("%s: cound not register accelerometer " \
+				"sensor device(%d).",
+				__func__, res);
+		goto out_sensor_register_failed;
+	}
+
+	res = sensors_register(magnetic_sensor_device, NULL,
+			magnetic_sensor_attrs, "magnetic_sensor");
+	if (res) {
+		pr_err("%s: cound not register magnetic sensor device(%d).",
+				__func__, res);
+		goto out_sensor_register_failed;
+	}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+		mpu->early_suspend.level = EARLY_SUSPEND_LEVEL_DISABLE_FB + 1;
+		mpu->early_suspend.suspend = mpu_dev_early_suspend;
+		mpu->early_suspend.resume = mpu_dev_early_resume;
+		register_early_suspend(&mpu->early_suspend);
+#endif
+
+	return res;
+
+out_sensor_register_failed:
+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);
+#if 0
+out_register_pm_notifier_failed:
+#endif
+	kfree(mldl_cfg->mpu_ram->ram);
+	mpu_data = NULL;
+out_alloc_ram_failed:
+	kfree(mpu);
+out_alloc_data_failed:
+out_check_functionality_failed:
+	dev_err(&client->adapter->dev, "%s failed %d", __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", __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", __func__, MPU_NAME);
+	if (res)
+		pr_err("%s failed", __func__);
+	return res;
+}
+
+static void __exit mpu_exit(void)
+{
+	pr_info("%s", __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/mpu-dev.h b/drivers/misc/inv_mpu/mpu-dev.h
new file mode 100644
index 0000000..0b352c9
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu-dev.h
@@ -0,0 +1,42 @@
+/*
+	$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_411.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));
+
+extern signed short gAccelOffset[3];
+extern struct class *sensors_class;
+extern int sensors_register(struct device *dev, void * drvdata,
+		struct device_attribute *attributes[], char *name);
+
+#endif
diff --git a/drivers/misc/inv_mpu/mpu6050b1.h b/drivers/misc/inv_mpu/mpu6050b1.h
new file mode 100644
index 0000000..c486784
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050b1.h
@@ -0,0 +1,437 @@
+/*
+	$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 "mpu6050"
+#define DEFAULT_MPU_SLAVEADDR		0x68
+extern struct acc_data cal_data;
+
+
+/*==== 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 0000000..2a850fa
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpuirq.c
@@ -0,0 +1,261 @@
+/*
+	$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_411.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)
+{
+
+	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) {
+		unsigned long flags;
+		if (BIT_ACTL_LOW == ((mldl_cfg->pdata->int_config) & BIT_ACTL))
+			flags = IRQF_TRIGGER_FALLING;
+		else
+			flags = IRQF_TRIGGER_RISING;
+
+		flags |= IRQF_SHARED;
+		res =
+		    request_irq(mpuirq_dev_data.irq, mpuirq_handler, flags,
+				interface, &mpuirq_dev_data.irq);
+
+		/* mpu_irq Interrupt isr enable */
+		if (mldl_cfg->pdata && mldl_cfg->pdata->enable_irq_handler)
+			mldl_cfg->pdata->enable_irq_handler();
+		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)
+{
+	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;
+}
+
+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 0000000..3348071
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpuirq.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 __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);
+
+#endif
diff --git a/drivers/misc/inv_mpu/pressure/Kconfig b/drivers/misc/inv_mpu/pressure/Kconfig
new file mode 100644
index 0000000..9fe7763
--- /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_411
+	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 0000000..a69ee3a
--- /dev/null
+++ b/drivers/misc/inv_mpu/pressure/Makefile
@@ -0,0 +1,8 @@
+#
+# Pressure Slaves to MPUxxxx
+#
+obj-$(CONFIG_MPU_SENSORS_BMA085_411) += 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 0000000..696d2b6
--- /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/sensors_core.c b/drivers/misc/inv_mpu/sensors_core.c
new file mode 100644
index 0000000..b652631
--- /dev/null
+++ b/drivers/misc/inv_mpu/sensors_core.c
@@ -0,0 +1,100 @@
+/*
+ *  Universal sensors core class
+ *
+ *  Author : Ryunkyun Park <ryun.park@samsung.com>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+/* #include <linux/sensors_core.h> */
+
+struct class *sensors_class;
+static atomic_t sensor_count;
+static DEFINE_MUTEX(sensors_mutex);
+
+/**
+ * Create sysfs interface
+ */
+static void set_sensor_attr(struct device *dev,
+			    struct device_attribute *attributes[])
+{
+	int i;
+
+	for (i = 0; attributes[i] != NULL; i++) {
+		if ((device_create_file(dev, attributes[i])) < 0) {
+			pr_info("[SENSOR CORE] fail!!! device_create_file" \
+				"( dev, attributes[%d] )\n", i);
+		}
+	}
+}
+
+int sensors_register(struct device *dev, void *drvdata,
+		     struct device_attribute *attributes[], char *name)
+{
+	int ret = 0;
+
+	if (!sensors_class) {
+		sensors_class = class_create(THIS_MODULE, "sensors");
+		if (IS_ERR(sensors_class))
+			return PTR_ERR(sensors_class);
+	}
+
+	mutex_lock(&sensors_mutex);
+
+	dev = device_create(sensors_class, NULL, 0, drvdata, "%s", name);
+
+	if (IS_ERR(dev)) {
+		ret = PTR_ERR(dev);
+		pr_err("[SENSORS CORE] device_create failed! [%d]\n", ret);
+		return ret;
+	}
+
+	set_sensor_attr(dev, attributes);
+
+	atomic_inc(&sensor_count);
+
+	mutex_unlock(&sensors_mutex);
+
+	return 0;
+}
+
+void sensors_unregister(struct device *dev)
+{
+	/* TODO : Unregister device */
+}
+
+static int __init sensors_class_init(void)
+{
+	pr_info("[SENSORS CORE] sensors_class_init\n");
+	sensors_class = class_create(THIS_MODULE, "sensors");
+
+	if (IS_ERR(sensors_class))
+		return PTR_ERR(sensors_class);
+
+	atomic_set(&sensor_count, 0);
+	sensors_class->dev_uevent = NULL;
+
+	return 0;
+}
+
+static void __exit sensors_class_exit(void)
+{
+	class_destroy(sensors_class);
+}
+
+EXPORT_SYMBOL_GPL(sensors_register);
+EXPORT_SYMBOL_GPL(sensors_unregister);
+
+/* exported for the APM Power driver, APM emulation */
+EXPORT_SYMBOL_GPL(sensors_class);
+
+subsys_initcall(sensors_class_init);
+module_exit(sensors_class_exit);
+
+MODULE_DESCRIPTION("Universal sensors core class");
+MODULE_AUTHOR("Ryunkyun Park <ryun.park@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/inv_mpu/slaveirq.c b/drivers/misc/inv_mpu/slaveirq.c
new file mode 100644
index 0000000..fdabcdd
--- /dev/null
+++ b/drivers/misc/inv_mpu/slaveirq.c
@@ -0,0 +1,266 @@
+/*
+	$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_411.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;
+}
+
+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/inv_mpu/slaveirq.h b/drivers/misc/inv_mpu/slaveirq.h
new file mode 100644
index 0000000..6926634
--- /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 0000000..b7b0b1e
--- /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_411.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 0000000..f69f07a
--- /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/jack.c b/drivers/misc/jack.c
new file mode 100644
index 0000000..19c9f56
--- /dev/null
+++ b/drivers/misc/jack.c
@@ -0,0 +1,220 @@
+/*
+ *  Jack Monitoring Interface
+ *
+ *  Copyright (C) 2009 Samsung Electronics
+ *  Minkyu Kang <mk7.kang@samsung.com>
+ *
+ * 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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/jack.h>
+#include <linux/slab.h>
+
+struct jack_data {
+	struct jack_platform_data	*pdata;
+};
+
+static struct platform_device *jack_dev;
+
+static void jack_set_data(struct jack_platform_data *pdata,
+		const char *name, int value)
+{
+	if (!strcmp(name, "usb"))
+		pdata->usb_online = value;
+	else if (!strcmp(name, "charger"))
+		pdata->charger_online = value;
+	else if (!strcmp(name, "hdmi"))
+		pdata->hdmi_online = value;
+	else if (!strcmp(name, "earjack"))
+		pdata->earjack_online = value;
+	else if (!strcmp(name, "earkey"))
+		pdata->earkey_online = value;
+	else if (!strcmp(name, "ums"))
+		pdata->ums_online = value;
+	else if (!strcmp(name, "cdrom"))
+		pdata->cdrom_online = value;
+	else if (!strcmp(name, "jig"))
+		pdata->jig_online = value;
+	else if (!strcmp(name, "host"))
+		pdata->host_online = value;
+	else if (!strcmp(name, "cradle"))
+		pdata->cradle_online = value;
+}
+
+int jack_get_data(const char *name)
+{
+	struct jack_data *jack = platform_get_drvdata(jack_dev);
+
+	if (!strcmp(name, "usb"))
+		return jack->pdata->usb_online;
+	else if (!strcmp(name, "charger"))
+		return jack->pdata->charger_online;
+	else if (!strcmp(name, "hdmi"))
+		return jack->pdata->hdmi_online;
+	else if (!strcmp(name, "earjack"))
+		return jack->pdata->earjack_online;
+	else if (!strcmp(name, "earkey"))
+		return jack->pdata->earkey_online;
+	else if (!strcmp(name, "ums"))
+		return jack->pdata->ums_online;
+	else if (!strcmp(name, "cdrom"))
+		return jack->pdata->cdrom_online;
+	else if (!strcmp(name, "jig"))
+		return jack->pdata->jig_online;
+	else if (!strcmp(name, "host"))
+		return jack->pdata->host_online;
+	else if (!strcmp(name, "cradle"))
+		return jack->pdata->cradle_online;
+
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(jack_get_data);
+
+void jack_event_handler(const char *name, int value)
+{
+	struct jack_data *jack;
+	char env_str[16];
+	char *envp[] = { env_str, NULL };
+
+	if (!jack_dev) {
+		printk(KERN_ERR "jack device is not allocated\n");
+		return;
+	}
+
+	jack = platform_get_drvdata(jack_dev);
+	jack_set_data(jack->pdata, name, value);
+	sprintf(env_str, "CHGDET=%s", name);
+	dev_info(&jack_dev->dev, "jack event %s\n", env_str);
+	kobject_uevent_env(&jack_dev->dev.kobj, KOBJ_CHANGE, envp);
+}
+EXPORT_SYMBOL_GPL(jack_event_handler);
+
+#define JACK_OUTPUT(name)						\
+static ssize_t jack_show_##name(struct device *dev,			\
+		struct device_attribute *attr, char *buf)		\
+{									\
+	struct jack_data *chip = dev_get_drvdata(dev);			\
+	return sprintf(buf, "%d\n", chip->pdata->name);			\
+}									\
+static DEVICE_ATTR(name, S_IRUGO, jack_show_##name, NULL);
+
+JACK_OUTPUT(usb_online);
+JACK_OUTPUT(charger_online);
+JACK_OUTPUT(hdmi_online);
+JACK_OUTPUT(earjack_online);
+JACK_OUTPUT(earkey_online);
+JACK_OUTPUT(jig_online);
+JACK_OUTPUT(host_online);
+JACK_OUTPUT(cradle_online);
+
+static int jack_device_init(struct jack_data *jack)
+{
+	struct jack_platform_data *pdata = jack->pdata;
+	int ret;
+
+	if (pdata->usb_online != -1)
+		ret = device_create_file(&jack_dev->dev,
+				&dev_attr_usb_online);
+	if (pdata->charger_online != -1)
+		ret = device_create_file(&jack_dev->dev,
+				&dev_attr_charger_online);
+	if (pdata->hdmi_online != -1)
+		ret = device_create_file(&jack_dev->dev,
+				&dev_attr_hdmi_online);
+	if (pdata->earjack_online != -1)
+		ret = device_create_file(&jack_dev->dev,
+				&dev_attr_earjack_online);
+	if (pdata->earkey_online != -1)
+		ret = device_create_file(&jack_dev->dev,
+				&dev_attr_earkey_online);
+	if (pdata->jig_online != -1)
+		ret = device_create_file(&jack_dev->dev,
+				&dev_attr_jig_online);
+	if (pdata->host_online != -1)
+		ret = device_create_file(&jack_dev->dev,
+				&dev_attr_host_online);
+	if (pdata->cradle_online != -1)
+		ret = device_create_file(&jack_dev->dev,
+				&dev_attr_cradle_online);
+
+	return 0;
+}
+
+static int __devinit jack_probe(struct platform_device *pdev)
+{
+	struct jack_platform_data *pdata = pdev->dev.platform_data;
+	struct jack_data *jack;
+
+	jack = kzalloc(sizeof(struct jack_data), GFP_KERNEL);
+	if (!jack) {
+		dev_err(&pdev->dev, "failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, jack);
+	jack_dev = pdev;
+	jack->pdata = pdata;
+
+	jack_device_init(jack);
+
+	return 0;
+}
+
+static int __devexit jack_remove(struct platform_device *pdev)
+{
+	struct jack_platform_data *pdata = pdev->dev.platform_data;
+
+	if (pdata->usb_online != -1)
+		device_remove_file(&jack_dev->dev, &dev_attr_usb_online);
+	if (pdata->charger_online != -1)
+		device_remove_file(&jack_dev->dev, &dev_attr_charger_online);
+	if (pdata->hdmi_online != -1)
+		device_remove_file(&jack_dev->dev, &dev_attr_hdmi_online);
+	if (pdata->earjack_online != -1)
+		device_remove_file(&jack_dev->dev, &dev_attr_earjack_online);
+	if (pdata->earkey_online != -1)
+		device_remove_file(&jack_dev->dev, &dev_attr_earkey_online);
+	if (pdata->jig_online != -1)
+		device_remove_file(&jack_dev->dev, &dev_attr_jig_online);
+	if (pdata->host_online != -1)
+		device_remove_file(&jack_dev->dev, &dev_attr_host_online);
+	if (pdata->cradle_online != -1)
+		device_remove_file(&jack_dev->dev, &dev_attr_cradle_online);
+
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver jack_driver = {
+	.probe		= jack_probe,
+	.remove		= __devexit_p(jack_remove),
+	.driver		= {
+		.name	= "jack",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init jack_init(void)
+{
+	return platform_driver_register(&jack_driver);
+}
+module_init(jack_init);
+
+static void __exit jack_exit(void)
+{
+	platform_driver_unregister(&jack_driver);
+}
+module_exit(jack_exit);
+
+MODULE_AUTHOR("Minkyu Kang <mk7.kang@samsung.com>");
+MODULE_DESCRIPTION("Jack Monitoring Interface");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/max77693-muic.c b/drivers/misc/max77693-muic.c
new file mode 100644
index 0000000..a902597
--- /dev/null
+++ b/drivers/misc/max77693-muic.c
@@ -0,0 +1,2520 @@
+/*
+ * max77693-muic.c - MUIC driver for the Maxim 77693
+ *
+ *  Copyright (C) 2012 Samsung Electronics
+ *  <sukdong.kim@samsung.com>
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <plat/gpio-cfg.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/input.h>
+#include <linux/mfd/max77693.h>
+#include <linux/mfd/max77693-private.h>
+#include <linux/host_notify.h>
+#include <plat/udc-hs.h>
+#ifdef CONFIG_USBHUB_USB3803
+#include <linux/usb3803.h>
+#endif
+#include <linux/delay.h>
+#include <linux/extcon.h>
+
+#define DEV_NAME	"max77693-muic"
+
+/* for providing API */
+static struct max77693_muic_info *gInfo;
+
+/* For restore charger interrupt states */
+static u8 chg_int_state;
+
+#ifdef CONFIG_LTE_VIA_SWITCH
+/* For saving uart path during CP booting */
+static int cpbooting;
+#endif
+
+/* MAX77693 MUIC CHG_TYP setting values */
+enum {
+	/* No Valid voltage at VB (Vvb < Vvbdet) */
+	CHGTYP_NO_VOLTAGE	= 0x00,
+	/* Unknown (D+/D- does not present a valid USB charger signature) */
+	CHGTYP_USB		= 0x01,
+	/* Charging Downstream Port */
+	CHGTYP_DOWNSTREAM_PORT	= 0x02,
+	/* Dedicated Charger (D+/D- shorted) */
+	CHGTYP_DEDICATED_CHGR	= 0x03,
+	/* Special 500mA charger, max current 500mA */
+	CHGTYP_500MA		= 0x04,
+	/* Special 1A charger, max current 1A */
+	CHGTYP_1A		= 0x05,
+	/* Reserved for Future Use */
+	CHGTYP_RFU		= 0x06,
+	/* Dead Battery Charging, max current 100mA */
+	CHGTYP_DB_100MA		= 0x07,
+	CHGTYP_MAX,
+
+	CHGTYP_INIT,
+	CHGTYP_MIN = CHGTYP_NO_VOLTAGE
+};
+
+enum {
+	ADC_GND			= 0x00,
+	ADC_MHL			= 0x01,
+	ADC_DOCK_PREV_KEY	= 0x04,
+	ADC_DOCK_NEXT_KEY	= 0x07,
+	ADC_DOCK_VOL_DN		= 0x0a, /* 0x01010 14.46K ohm */
+	ADC_DOCK_VOL_UP		= 0x0b, /* 0x01011 17.26K ohm */
+	ADC_DOCK_PLAY_PAUSE_KEY = 0x0d,
+	ADC_SMARTDOCK		= 0x10, /* 0x10000 40.2K ohm */
+	ADC_CEA936ATYPE1_CHG	= 0x17,	/* 0x10111 200K ohm */
+	ADC_JIG_USB_OFF		= 0x18, /* 0x11000 255K ohm */
+	ADC_JIG_USB_ON		= 0x19, /* 0x11001 301K ohm */
+	ADC_DESKDOCK		= 0x1a, /* 0x11010 365K ohm */
+	ADC_CEA936ATYPE2_CHG	= 0x1b, /* 0x11011 442K ohm */
+	ADC_JIG_UART_OFF	= 0x1c, /* 0x11100 523K ohm */
+	ADC_JIG_UART_ON		= 0x1d, /* 0x11101 619K ohm */
+	ADC_CARDOCK		= 0x1d, /* 0x11101 619K ohm */
+	ADC_OPEN		= 0x1f
+};
+
+enum {
+	DOCK_KEY_NONE			= 0,
+	DOCK_KEY_VOL_UP_PRESSED,
+	DOCK_KEY_VOL_UP_RELEASED,
+	DOCK_KEY_VOL_DOWN_PRESSED,
+	DOCK_KEY_VOL_DOWN_RELEASED,
+	DOCK_KEY_PREV_PRESSED,
+	DOCK_KEY_PREV_RELEASED,
+	DOCK_KEY_PLAY_PAUSE_PRESSED,
+	DOCK_KEY_PLAY_PAUSE_RELEASED,
+	DOCK_KEY_NEXT_PRESSED,
+	DOCK_KEY_NEXT_RELEASED,
+};
+
+struct max77693_muic_info {
+	struct device		*dev;
+	struct max77693_dev	*max77693;
+	struct i2c_client	*muic;
+	struct max77693_muic_data *muic_data;
+	int			irq_adc;
+	int			irq_chgtype;
+	int			irq_vbvolt;
+	int			irq_adc1k;
+	int			mansw;
+	bool			is_default_uart_path_cp;
+
+	enum cable_type_muic	cable_type;
+	struct delayed_work	init_work;
+	struct delayed_work	usb_work;
+	struct delayed_work	mhl_work;
+	struct mutex		mutex;
+
+	bool			is_usb_ready;
+	bool			is_mhl_ready;
+
+	struct input_dev	*input;
+	int			previous_key;
+	bool			is_adc_open_prev;
+#ifdef CONFIG_EXTCON
+	struct extcon_dev	*edev;
+#endif
+};
+
+static int if_muic_info;
+static int switch_sel;
+static int if_pmic_rev;
+
+/* func : get_if_pmic_inifo
+ * switch_sel value get from bootloader comand line
+ * switch_sel data consist 8 bits (xxxxzzzz)
+ * first 4bits(zzzz) mean path infomation.
+ * next 4bits(xxxx) mean if pmic version info
+ */
+static int get_if_pmic_inifo(char *str)
+{
+	get_option(&str, &if_muic_info);
+	switch_sel = if_muic_info & 0x0f;
+	if_pmic_rev = (if_muic_info & 0xf0) >> 4;
+	pr_info("%s %s: switch_sel: %x if_pmic_rev:%x\n",
+		__FILE__, __func__, switch_sel, if_pmic_rev);
+	return if_muic_info;
+}
+__setup("pmic_info=", get_if_pmic_inifo);
+
+int get_switch_sel(void)
+{
+	return switch_sel;
+}
+
+static int max77693_muic_get_comp2_comn1_pass2
+	(struct max77693_muic_info *info)
+{
+	int ret;
+	u8 val;
+
+	ret = max77693_read_reg(info->muic, MAX77693_MUIC_REG_CTRL1, &val);
+	val = val & CLEAR_IDBEN_MICEN_MASK;
+	dev_info(info->dev, "func:%s ret:%d val:%x\n", __func__, ret, val);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg\n", __func__);
+		return -EINVAL;
+	}
+	return val;
+}
+
+static int max77693_muic_set_comp2_comn1_pass2
+	(struct max77693_muic_info *info, int type, int path)
+{
+	/* type 1 == usb, type 2 == uart */
+	u8 cntl1_val, cntl1_msk;
+	int ret = 0;
+	int val;
+
+	dev_info(info->dev, "func: %s type: %d path: %d\n",
+		__func__, type, path);
+	if (type == 0) {
+		if (path == AP_USB_MODE) {
+			info->muic_data->sw_path = AP_USB_MODE;
+			val = MAX77693_MUIC_CTRL1_BIN_1_001;
+		} else if (path == CP_USB_MODE) {
+			info->muic_data->sw_path = CP_USB_MODE;
+			val = MAX77693_MUIC_CTRL1_BIN_4_100;
+		} else {
+			dev_err(info->dev, "func: %s invalid usb path\n"
+				, __func__);
+			return -EINVAL;
+		}
+	} else if (type == 1) {
+		if (path == UART_PATH_AP) {
+			info->muic_data->sw_path = UART_PATH_AP;
+			if (info->is_default_uart_path_cp)
+				val = MAX77693_MUIC_CTRL1_BIN_5_101;
+			else
+				val = MAX77693_MUIC_CTRL1_BIN_3_011;
+		} else if (path == UART_PATH_CP) {
+			info->muic_data->sw_path = UART_PATH_CP;
+			if (info->is_default_uart_path_cp)
+				val = MAX77693_MUIC_CTRL1_BIN_3_011;
+			else
+				val = MAX77693_MUIC_CTRL1_BIN_5_101;
+#ifdef CONFIG_LTE_VIA_SWITCH
+			dev_info(info->dev, "%s: cpbooting is %s\n",
+				__func__,
+				cpbooting ?
+				"started. skip path set" : "done. set path");
+			if (!cpbooting) {
+				if (gpio_is_valid(GPIO_LTE_VIA_UART_SEL)) {
+					gpio_set_value(GPIO_LTE_VIA_UART_SEL,
+						GPIO_LEVEL_HIGH);
+					dev_info(info->dev,
+						"%s: LTE_GPIO_LEVEL_HIGH"
+						, __func__);
+				} else {
+					dev_err(info->dev,
+						"%s: ERR_LTE_GPIO_SET_HIGH\n"
+						, __func__);
+					return -EINVAL;
+				}
+			}
+#endif
+		}
+#ifdef CONFIG_LTE_VIA_SWITCH
+		else if (path == UART_PATH_LTE) {
+			info->muic_data->sw_path = UART_PATH_LTE;
+			val = MAX77693_MUIC_CTRL1_BIN_5_101;
+			if (gpio_is_valid(GPIO_LTE_VIA_UART_SEL)) {
+				gpio_set_value(GPIO_LTE_VIA_UART_SEL,
+					GPIO_LEVEL_LOW);
+				dev_info(info->dev, "%s: LTE_GPIO_LEVEL_LOW\n"
+								, __func__);
+			} else {
+				dev_err(info->dev, "%s: ERR_LTE_GPIO_SET_LOW\n"
+								, __func__);
+				return -EINVAL;
+			}
+		}
+#endif
+		else {
+			dev_err(info->dev, "func: %s invalid uart path\n"
+				, __func__);
+			return -EINVAL;
+		}
+	} else {
+		dev_err(info->dev, "func: %s invalid path type(%d)\n"
+			, __func__, type);
+		return -EINVAL;
+	}
+
+	cntl1_val = (val << COMN1SW_SHIFT) | (val << COMP2SW_SHIFT);
+	cntl1_msk = COMN1SW_MASK | COMP2SW_MASK;
+
+	max77693_update_reg(info->muic, MAX77693_MUIC_REG_CTRL1, cntl1_val,
+			    cntl1_msk);
+
+	return ret;
+}
+
+static int max77693_muic_set_usb_path_pass2
+	(struct max77693_muic_info *info, int path)
+{
+	int ret = 0;
+	ret = max77693_muic_set_comp2_comn1_pass2
+		(info, 0/*usb*/, path);
+	sysfs_notify(&switch_dev->kobj, NULL, "usb_sel");
+	return ret;
+}
+
+static int max77693_muic_get_usb_path_pass2
+	(struct max77693_muic_info *info)
+{
+	u8 val;
+
+	val = max77693_muic_get_comp2_comn1_pass2(info);
+	if (val == CTRL1_AP_USB)
+		return AP_USB_MODE;
+	else if (val == CTRL1_CP_USB)
+		return CP_USB_MODE;
+	else if (val == CTRL1_AUDIO)
+		return AUDIO_MODE;
+	else
+		return -EINVAL;
+}
+
+static int max77693_muic_set_uart_path_pass2
+	(struct max77693_muic_info *info, int path)
+{
+	int ret = 0;
+	ret = max77693_muic_set_comp2_comn1_pass2
+		(info, 1/*uart*/, path);
+	return ret;
+
+}
+
+static int max77693_muic_get_uart_path_pass2
+	(struct max77693_muic_info *info)
+{
+	u8 val;
+
+	val = max77693_muic_get_comp2_comn1_pass2(info);
+
+	if (val == CTRL1_AP_UART) {
+		if (info->is_default_uart_path_cp)
+			return UART_PATH_CP;
+		else
+			return UART_PATH_AP;
+	} else if (val == CTRL1_CP_UART) {
+#ifdef CONFIG_LTE_VIA_SWITCH
+		if (gpio_is_valid(GPIO_LTE_VIA_UART_SEL)) {
+			if (gpio_get_value(GPIO_LTE_VIA_UART_SEL))
+				return UART_PATH_CP;
+			else
+				return UART_PATH_LTE;
+		} else {
+			dev_info(info->dev, "%s: ERR_UART_PATH_LTE\n"
+								, __func__);
+			return -EINVAL;
+		}
+#endif
+#ifndef CONFIG_LTE_VIA_SWITCH
+		if (info->is_default_uart_path_cp)
+			return UART_PATH_AP;
+		else
+			return UART_PATH_CP;
+#endif
+	} else {
+		return -EINVAL;
+	}
+}
+
+static ssize_t max77693_muic_show_usb_state(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	dev_info(info->dev, "func:%s info->cable_type:%d\n",
+		 __func__, info->cable_type);
+	switch (info->cable_type) {
+	case CABLE_TYPE_USB_MUIC:
+	case CABLE_TYPE_JIG_USB_OFF_MUIC:
+	case CABLE_TYPE_JIG_USB_ON_MUIC:
+		return sprintf(buf, "USB_STATE_CONFIGURED\n");
+	default:
+		break;
+	}
+
+	return sprintf(buf, "USB_STATE_NOTCONFIGURED\n");
+}
+
+static ssize_t max77693_muic_show_device(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	dev_info(info->dev, "func:%s info->cable_type:%d\n",
+		 __func__, info->cable_type);
+
+	switch (info->cable_type) {
+	case CABLE_TYPE_NONE_MUIC:
+		return sprintf(buf, "No cable\n");
+	case CABLE_TYPE_USB_MUIC:
+		return sprintf(buf, "USB\n");
+	case CABLE_TYPE_OTG_MUIC:
+		return sprintf(buf, "OTG\n");
+	case CABLE_TYPE_TA_MUIC:
+		return sprintf(buf, "TA\n");
+	case CABLE_TYPE_DESKDOCK_MUIC:
+		return sprintf(buf, "Desk Dock\n");
+	case CABLE_TYPE_CARDOCK_MUIC:
+		return sprintf(buf, "Car Dock\n");
+	case CABLE_TYPE_JIG_UART_OFF_MUIC:
+		return sprintf(buf, "JIG UART OFF\n");
+	case CABLE_TYPE_JIG_UART_OFF_VB_MUIC:
+		return sprintf(buf, "JIG UART OFF/VB\n");
+	case CABLE_TYPE_JIG_UART_ON_MUIC:
+		return sprintf(buf, "JIG UART ON\n");
+	case CABLE_TYPE_JIG_USB_OFF_MUIC:
+		return sprintf(buf, "JIG USB OFF\n");
+	case CABLE_TYPE_JIG_USB_ON_MUIC:
+		return sprintf(buf, "JIG USB ON\n");
+	case CABLE_TYPE_MHL_MUIC:
+		return sprintf(buf, "mHL\n");
+	case CABLE_TYPE_MHL_VB_MUIC:
+		return sprintf(buf, "mHL charging\n");
+	default:
+		break;
+	}
+
+	return sprintf(buf, "UNKNOWN\n");
+}
+
+static ssize_t max77693_muic_show_manualsw(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+
+	dev_info(info->dev, "func:%s ap(0),cp(1),vps(2)sw_path:%d(%d)\n",
+		 __func__, info->muic_data->sw_path,
+			gpio_get_value(GPIO_USB_SEL));/*For debuging*/
+
+	switch (info->muic_data->sw_path) {
+	case AP_USB_MODE:
+		return sprintf(buf, "PDA\n");
+	case CP_USB_MODE:
+		return sprintf(buf, "MODEM\n");
+	case AUDIO_MODE:
+		return sprintf(buf, "Audio\n");
+	default:
+		break;
+	}
+
+	return sprintf(buf, "UNKNOWN\n");
+}
+
+static ssize_t max77693_muic_set_manualsw(struct device *dev,
+					  struct device_attribute *attr,
+					  const char *buf, size_t count)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+
+	dev_info(info->dev, "func:%s buf:%s,count:%d\n", __func__, buf, count);
+
+	if (!strncasecmp(buf, "PDA", 3)) {
+		info->muic_data->sw_path = AP_USB_MODE;
+		dev_info(info->dev, "%s: AP_USB_MODE\n", __func__);
+	} else if (!strncasecmp(buf, "MODEM", 5)) {
+		info->muic_data->sw_path = CP_USB_MODE;
+		dev_info(info->dev, "%s: CP_USB_MODE\n", __func__);
+	} else
+		dev_warn(info->dev, "%s: Wrong command\n", __func__);
+
+	return count;
+}
+
+static ssize_t max77693_muic_show_adc(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+
+	ret = max77693_read_reg(info->muic, MAX77693_MUIC_REG_STATUS1, &val);
+	dev_info(info->dev, "func:%s ret:%d val:%x\n", __func__, ret, val);
+
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg\n", __func__);
+		return sprintf(buf, "UNKNOWN\n");
+	}
+
+	return sprintf(buf, "%x\n", (val & STATUS1_ADC_MASK));
+}
+
+static ssize_t max77693_muic_show_audio_path(struct device *dev,
+					     struct device_attribute *attr,
+					     char *buf)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+
+	ret = max77693_read_reg(info->muic, MAX77693_MUIC_REG_CTRL1, &val);
+	dev_info(info->dev, "func:%s ret:%d val:%x\n", __func__, ret, val);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg\n", __func__);
+		return sprintf(buf, "UNKNOWN\n");
+	}
+
+	return sprintf(buf, "%x\n", val);
+}
+
+static ssize_t max77693_muic_set_audio_path(struct device *dev,
+					    struct device_attribute *attr,
+					    const char *buf, size_t count)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	struct i2c_client *client = info->muic;
+	u8 cntl1_val, cntl1_msk;
+	u8 val;
+	dev_info(info->dev, "func:%s buf:%s\n", __func__, buf);
+	if (!strncmp(buf, "0", 1))
+		val = MAX77693_MUIC_CTRL1_BIN_0_000;
+	else if (!strncmp(buf, "1", 1))
+		val = MAX77693_MUIC_CTRL1_BIN_2_010;
+	else {
+		dev_warn(info->dev, "%s: Wrong command\n", __func__);
+		return count;
+	}
+
+	cntl1_val = (val << COMN1SW_SHIFT) | (val << COMP2SW_SHIFT) |
+	    (0 << MICEN_SHIFT);
+	cntl1_msk = COMN1SW_MASK | COMP2SW_MASK | MICEN_MASK;
+
+	max77693_update_reg(client, MAX77693_MUIC_REG_CTRL1, cntl1_val,
+			    cntl1_msk);
+	dev_info(info->dev, "MUIC cntl1_val:%x, cntl1_msk:%x\n", cntl1_val,
+		 cntl1_msk);
+
+	cntl1_val = MAX77693_MUIC_CTRL1_BIN_0_000;
+	max77693_read_reg(client, MAX77693_MUIC_REG_CTRL1, &cntl1_val);
+	dev_info(info->dev, "%s: CNTL1(0x%02x)\n", __func__, cntl1_val);
+
+	return count;
+}
+
+static ssize_t max77693_muic_show_otg_test(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+
+	ret = max77693_read_reg(info->muic, MAX77693_MUIC_REG_CDETCTRL1, &val);
+	dev_info(info->dev, "func:%s ret:%d val:%x buf%s\n",
+		 __func__, ret, val, buf);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg\n", __func__);
+		return sprintf(buf, "UNKNOWN\n");
+	}
+	val &= CHGDETEN_MASK;
+
+	return sprintf(buf, "%x\n", val);
+}
+
+static ssize_t max77693_muic_set_otg_test(struct device *dev,
+					  struct device_attribute *attr,
+					  const char *buf, size_t count)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	struct i2c_client *client = info->muic;
+	u8 val;
+
+	dev_info(info->dev, "func:%s buf:%s\n", __func__, buf);
+	if (!strncmp(buf, "0", 1))
+		val = 0;
+	else if (!strncmp(buf, "1", 1))
+		val = 1;
+	else {
+		dev_warn(info->dev, "%s: Wrong command\n", __func__);
+		return count;
+	}
+
+	max77693_update_reg(client, MAX77693_MUIC_REG_CDETCTRL1,
+			    val << CHGDETEN_SHIFT, CHGDETEN_MASK);
+
+	val = 0;
+	max77693_read_reg(client, MAX77693_MUIC_REG_CDETCTRL1, &val);
+	dev_info(info->dev, "%s: CDETCTRL(0x%02x)\n", __func__, val);
+
+	return count;
+}
+
+static void max77693_muic_set_adcdbset(struct max77693_muic_info *info,
+				       int value)
+{
+	int ret;
+	u8 val;
+	dev_info(info->dev, "func:%s value:%x\n", __func__, value);
+	if (value > 3) {
+		dev_err(info->dev, "%s: invalid value(%x)\n", __func__, value);
+		return;
+	}
+
+	if (!info->muic) {
+		dev_err(info->dev, "%s: no muic i2c client\n", __func__);
+		return;
+	}
+
+	val = value << CTRL3_ADCDBSET_SHIFT;
+	dev_info(info->dev, "%s: ADCDBSET(0x%02x)\n", __func__, val);
+	ret = max77693_update_reg(info->muic, MAX77693_MUIC_REG_CTRL3, val,
+				  CTRL3_ADCDBSET_MASK);
+	if (ret < 0)
+		dev_err(info->dev, "%s: fail to update reg\n", __func__);
+}
+
+static ssize_t max77693_muic_show_adc_debounce_time(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+	dev_info(info->dev, "func:%s buf:%s\n", __func__, buf);
+
+	if (!info->muic)
+		return sprintf(buf, "No I2C client\n");
+
+	ret = max77693_read_reg(info->muic, MAX77693_MUIC_REG_CTRL3, &val);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg\n", __func__);
+		return sprintf(buf, "UNKNOWN\n");
+	}
+	val &= CTRL3_ADCDBSET_MASK;
+	val = val >> CTRL3_ADCDBSET_SHIFT;
+	dev_info(info->dev, "func:%s val:%x\n", __func__, val);
+	return sprintf(buf, "%x\n", val);
+}
+
+static ssize_t max77693_muic_set_adc_debounce_time(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	int value;
+
+	sscanf(buf, "%d", &value);
+	value = (value & 0x3);
+
+	dev_info(info->dev, "%s: Do nothing\n", __func__);
+
+	return count;
+}
+
+static ssize_t max77693_muic_set_uart_sel(struct device *dev,
+					  struct device_attribute *attr,
+					  const char *buf, size_t count)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+
+	if (info->max77693->pmic_rev < MAX77693_REV_PASS2) {
+		if (!strncasecmp(buf, "AP", 2)) {
+			info->muic_data->uart_path = UART_PATH_AP;
+			if (gpio_is_valid(GPIO_UART_SEL)) {
+				gpio_set_value(GPIO_UART_SEL, GPIO_LEVEL_HIGH);
+				dev_info(info->dev, "%s: UART_PATH_AP\n"
+					, __func__);
+			} else {
+				dev_err(info->dev, "%s: Change(AP) fail!!"
+					, __func__);
+			}
+		} else if (!strncasecmp(buf, "CP", 2)) {
+			info->muic_data->uart_path = UART_PATH_CP;
+			if (gpio_is_valid(GPIO_UART_SEL)
+#ifdef CONFIG_LTE_VIA_SWITCH
+				&& gpio_is_valid(GPIO_LTE_VIA_UART_SEL)
+#endif
+				) {
+				gpio_set_value(GPIO_UART_SEL, GPIO_LEVEL_LOW);
+#ifdef CONFIG_LTE_VIA_SWITCH
+				gpio_set_value(GPIO_LTE_VIA_UART_SEL
+						, GPIO_LEVEL_HIGH);
+#endif
+				dev_info(info->dev, "%s: UART_PATH_CP\n"
+						, __func__);
+			} else {
+				dev_err(info->dev, "%s: Change(CP) fail!!"
+						, __func__);
+			}
+		}
+#ifdef CONFIG_LTE_VIA_SWITCH
+		else if (!strncasecmp(buf, "LTE", 3)) {
+			info->muic_data->uart_path = UART_PATH_LTE;
+			if (gpio_is_valid(GPIO_UART_SEL) &&
+			    gpio_is_valid(GPIO_LTE_VIA_UART_SEL)) {
+				gpio_set_value(GPIO_UART_SEL, GPIO_LEVEL_LOW);
+				gpio_set_value(GPIO_LTE_VIA_UART_SEL
+						, GPIO_LEVEL_LOW);
+				dev_info(info->dev, "%s: UART_PATH_LTE\n"
+								, __func__);
+			} else {
+				dev_err(info->dev, "%s: Change(LTE) fail!!"
+								, __func__);
+			}
+		}
+#endif
+		else
+			dev_warn(info->dev, "%s: Wrong command\n", __func__);
+	} else if (info->max77693->pmic_rev >= MAX77693_REV_PASS2) {
+		if (!strncasecmp(buf, "AP", 2)) {
+			int ret = max77693_muic_set_uart_path_pass2
+				(info, UART_PATH_AP);
+			if (ret >= 0)
+				info->muic_data->uart_path = UART_PATH_AP;
+			else
+				dev_err(info->dev, "%s: Change(AP) fail!!"
+					, __func__);
+		} else if (!strncasecmp(buf, "CP", 2)) {
+			int ret = max77693_muic_set_uart_path_pass2
+				(info, UART_PATH_CP);
+			if (ret >= 0)
+				info->muic_data->uart_path = UART_PATH_CP;
+			else
+				dev_err(info->dev, "%s: Change(CP) fail!!"
+							, __func__);
+		}
+#ifdef CONFIG_LTE_VIA_SWITCH
+		else if (!strncasecmp(buf, "LTE", 3)) {
+			int ret = max77693_muic_set_uart_path_pass2
+				(info, UART_PATH_LTE);
+			if (ret >= 0)
+				info->muic_data->uart_path = UART_PATH_LTE;
+			else
+				dev_err(info->dev, "%s: Change(LTE) fail!!"
+							, __func__);
+		}
+#endif
+		else {
+				dev_warn(info->dev, "%s: Wrong command\n"
+							, __func__);
+		}
+	}
+	return count;
+}
+
+static ssize_t max77693_muic_show_uart_sel(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	if (info->max77693->pmic_rev < MAX77693_REV_PASS2) {
+		switch (info->muic_data->uart_path) {
+		case UART_PATH_AP:
+			if (gpio_get_value(GPIO_UART_SEL) == GPIO_LEVEL_HIGH)
+				return sprintf(buf, "AP\n");
+			else
+				return sprintf(buf, "ERR_AP\n");
+			break;
+		case UART_PATH_CP:
+			if (gpio_get_value(GPIO_UART_SEL) == GPIO_LEVEL_LOW
+#ifdef CONFIG_LTE_VIA_SWITCH
+			&& gpio_get_value(GPIO_LTE_VIA_UART_SEL)
+				== GPIO_LEVEL_HIGH
+#endif
+			) {
+				return sprintf(buf, "CP\n");
+			} else {
+				return sprintf(buf, "ERR_CP\n");
+			}
+			break;
+#ifdef CONFIG_LTE_VIA_SWITCH
+		case UART_PATH_LTE:
+			if (gpio_get_value(GPIO_UART_SEL) == GPIO_LEVEL_LOW &&
+				gpio_get_value(GPIO_LTE_VIA_UART_SEL)
+					== GPIO_LEVEL_LOW) {
+				return sprintf(buf, "LTE\n");
+			} else {
+				return sprintf(buf, "ERR_LTE\n");
+			}
+			break;
+#endif
+		default:
+			break;
+		}
+	} else if (info->max77693->pmic_rev >= MAX77693_REV_PASS2) {
+		int val = max77693_muic_get_uart_path_pass2(info);
+		switch (info->muic_data->uart_path) {
+		case UART_PATH_AP:
+			if (val == UART_PATH_AP)
+				return sprintf(buf, "AP\n");
+			else
+				return sprintf(buf, "ERR_AP\n");
+			break;
+		case UART_PATH_CP:
+			if (val == UART_PATH_CP)
+				return sprintf(buf, "CP\n");
+			else
+				return sprintf(buf, "ERR_CP\n");
+			break;
+#ifdef CONFIG_LTE_VIA_SWITCH
+		case UART_PATH_LTE:
+			if (val == UART_PATH_LTE)
+				return sprintf(buf, "LTE\n");
+			else
+				return sprintf(buf, "ERR_LTE\n");
+			break;
+#endif
+		default:
+			break;
+		}
+	}
+	return sprintf(buf, "UNKNOWN\n");
+}
+
+#ifdef CONFIG_LTE_VIA_SWITCH
+static ssize_t max77693_muic_show_check_cpboot(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	if (cpbooting)
+		return sprintf(buf, "start\n");
+	else
+		return sprintf(buf, "done\n");
+}
+
+static ssize_t max77693_muic_set_check_cpboot(struct device *dev,
+					  struct device_attribute *attr,
+					  const char *buf, size_t count)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+
+	if (!strncasecmp(buf, "start", 5)) {
+		if (gpio_is_valid(GPIO_LTE_VIA_UART_SEL)) {
+			dev_info(info->dev, "%s: Fix CP-UART path to LTE during CP Booting",
+					__func__);
+			gpio_set_value(GPIO_LTE_VIA_UART_SEL, GPIO_LEVEL_LOW);
+		} else {
+			dev_err(info->dev, "%s: ERR_LTE_GPIO_SET_LOW\n",
+					__func__);
+			return -EINVAL;
+		}
+		cpbooting = 1;
+	} else if (!strncasecmp(buf, "done", 4)) {
+		if (info->muic_data->uart_path == UART_PATH_CP) {
+			if (gpio_is_valid(GPIO_LTE_VIA_UART_SEL)) {
+				dev_info(info->dev, "%s: Reroute CP-UART path to VIA after CP Booting",
+						__func__);
+				gpio_set_value(GPIO_LTE_VIA_UART_SEL,
+						GPIO_LEVEL_HIGH);
+			} else {
+				dev_err(info->dev, "%s: ERR_LTE_GPIO_SET_HIGH\n",
+						__func__);
+				return -EINVAL;
+			}
+		}
+		cpbooting = 0;
+	} else {
+		dev_warn(info->dev, "%s: Wrong command : %s\n", __func__, buf);
+	}
+
+	return count;
+}
+#endif
+
+static DEVICE_ATTR(uart_sel, 0664, max77693_muic_show_uart_sel,
+		max77693_muic_set_uart_sel);
+static DEVICE_ATTR(usb_state, S_IRUGO, max77693_muic_show_usb_state, NULL);
+static DEVICE_ATTR(device, S_IRUGO, max77693_muic_show_device, NULL);
+static DEVICE_ATTR(usb_sel, 0664,
+		max77693_muic_show_manualsw, max77693_muic_set_manualsw);
+static DEVICE_ATTR(adc, S_IRUGO, max77693_muic_show_adc, NULL);
+static DEVICE_ATTR(audio_path, 0664,
+		max77693_muic_show_audio_path, max77693_muic_set_audio_path);
+static DEVICE_ATTR(otg_test, 0664,
+		max77693_muic_show_otg_test, max77693_muic_set_otg_test);
+static DEVICE_ATTR(adc_debounce_time, 0664,
+		max77693_muic_show_adc_debounce_time,
+		max77693_muic_set_adc_debounce_time);
+#ifdef CONFIG_LTE_VIA_SWITCH
+static DEVICE_ATTR(check_cpboot, 0664,
+		max77693_muic_show_check_cpboot,
+		max77693_muic_set_check_cpboot);
+#endif
+
+static struct attribute *max77693_muic_attributes[] = {
+	&dev_attr_uart_sel.attr,
+	&dev_attr_usb_state.attr,
+	&dev_attr_device.attr,
+	&dev_attr_usb_sel.attr,
+	&dev_attr_adc.attr,
+	&dev_attr_audio_path.attr,
+	&dev_attr_otg_test.attr,
+	&dev_attr_adc_debounce_time.attr,
+#ifdef CONFIG_LTE_VIA_SWITCH
+	&dev_attr_check_cpboot.attr,
+#endif
+	NULL
+};
+
+static const struct attribute_group max77693_muic_group = {
+	.attrs = max77693_muic_attributes,
+};
+
+#if defined(CONFIG_MACH_M0_CTC)
+/*0 : usb is not conneted to CP	1 : usb is connected to CP */
+int cp_usb_state;
+
+int max7693_muic_cp_usb_state(void)
+{
+	return cp_usb_state;
+}
+EXPORT_SYMBOL(max7693_muic_cp_usb_state);
+#endif
+
+static int max77693_muic_set_usb_path(struct max77693_muic_info *info, int path)
+{
+	struct i2c_client *client = info->muic;
+	struct max77693_muic_data *mdata = info->muic_data;
+	int ret;
+	int gpio_val;
+	u8 cntl1_val, cntl1_msk;
+	int val;
+	dev_info(info->dev, "func:%s path:%d\n", __func__, path);
+	if (mdata->set_safeout) {
+		ret = mdata->set_safeout(path);
+		if (ret) {
+			dev_err(info->dev, "%s: fail to set safout!\n",
+				__func__);
+			return ret;
+		}
+	}
+	switch (path) {
+	case AP_USB_MODE:
+		dev_info(info->dev, "%s: AP_USB_MODE\n", __func__);
+		gpio_val = 0;
+		val = MAX77693_MUIC_CTRL1_BIN_1_001;
+		cntl1_val = (val << COMN1SW_SHIFT) | (val << COMP2SW_SHIFT);
+		cntl1_msk = COMN1SW_MASK | COMP2SW_MASK;
+		break;
+	case CP_USB_MODE:
+		dev_info(info->dev, "%s: CP_USB_MODE\n", __func__);
+		gpio_val = 1;
+		if (info->max77693->pmic_rev >= MAX77693_REV_PASS2)
+			val = MAX77693_MUIC_CTRL1_BIN_4_100;
+		else
+			val = MAX77693_MUIC_CTRL1_BIN_3_011;
+		cntl1_val = (val << COMN1SW_SHIFT) | (val << COMP2SW_SHIFT);
+		cntl1_msk = COMN1SW_MASK | COMP2SW_MASK;
+		break;
+	case AUDIO_MODE:
+		dev_info(info->dev, "%s: AUDIO_MODE\n", __func__);
+		gpio_val = 0;
+		/* SL1, SR2 */
+		cntl1_val = (MAX77693_MUIC_CTRL1_BIN_2_010 << COMN1SW_SHIFT)
+			| (MAX77693_MUIC_CTRL1_BIN_2_010 << COMP2SW_SHIFT) |
+			(0 << MICEN_SHIFT);
+		cntl1_msk = COMN1SW_MASK | COMP2SW_MASK | MICEN_MASK;
+		break;
+	default:
+		dev_warn(info->dev, "%s: invalid path(%d)\n", __func__, path);
+		return -EINVAL;
+	}
+	if (info->max77693->pmic_rev < MAX77693_REV_PASS2) {
+		if (gpio_is_valid(info->muic_data->gpio_usb_sel))
+			gpio_direction_output(mdata->gpio_usb_sel, gpio_val);
+	}
+
+	dev_info(info->dev, "%s: Set manual path\n", __func__);
+#if defined(CONFIG_SND_USE_MUIC_SWITCH)
+	if (info->cable_type != CABLE_TYPE_CARDOCK_MUIC
+		&& info->cable_type != CABLE_TYPE_DESKDOCK_MUIC)
+#endif
+		max77693_update_reg(client, MAX77693_MUIC_REG_CTRL1, cntl1_val,
+			    cntl1_msk);
+	max77693_update_reg(client, MAX77693_MUIC_REG_CTRL2,
+				CTRL2_CPEn1_LOWPWD0,
+				CTRL2_CPEn_MASK | CTRL2_LOWPWD_MASK);
+
+	cntl1_val = MAX77693_MUIC_CTRL1_BIN_0_000;
+	max77693_read_reg(client, MAX77693_MUIC_REG_CTRL1, &cntl1_val);
+	dev_info(info->dev, "%s: CNTL1(0x%02x)\n", __func__, cntl1_val);
+
+	cntl1_val = MAX77693_MUIC_CTRL1_BIN_0_000;
+	max77693_read_reg(client, MAX77693_MUIC_REG_CTRL2, &cntl1_val);
+	dev_info(info->dev, "%s: CNTL2(0x%02x)\n", __func__, cntl1_val);
+
+	sysfs_notify(&switch_dev->kobj, NULL, "usb_sel");
+	return 0;
+}
+
+int max77693_muic_get_charging_type(void)
+{
+	return gInfo->cable_type;
+}
+
+static int max77693_muic_set_charging_type(struct max77693_muic_info *info,
+					   bool force_disable)
+{
+	struct max77693_muic_data *mdata = info->muic_data;
+	int ret = 0;
+	dev_info(info->dev, "func:%s force_disable:%d\n",
+		 __func__, force_disable);
+	if (mdata->charger_cb) {
+		if (force_disable)
+			ret = mdata->charger_cb(CABLE_TYPE_NONE_MUIC);
+		else
+			ret = mdata->charger_cb(info->cable_type);
+	}
+
+	if (ret) {
+		dev_err(info->dev, "%s: error from charger_cb(%d)\n", __func__,
+			ret);
+		return ret;
+	}
+	return 0;
+}
+
+static int max77693_muic_handle_dock_vol_key(struct max77693_muic_info *info,
+					     u8 status1)
+{
+	struct input_dev *input = info->input;
+	int pre_key = info->previous_key;
+	unsigned int code;
+	int state;
+	u8 adc;
+
+	adc = status1 & STATUS1_ADC_MASK;
+	dev_info(info->dev,
+		 "func:%s status1:%x adc:%x cable_type:%d\n",
+		 __func__, status1, adc, info->cable_type);
+	if (info->cable_type != CABLE_TYPE_DESKDOCK_MUIC)
+		return 0;
+
+	if (adc == ADC_OPEN) {
+		switch (pre_key) {
+		case DOCK_KEY_VOL_UP_PRESSED:
+			code = KEY_VOLUMEUP;
+			state = 0;
+			info->previous_key = DOCK_KEY_VOL_UP_RELEASED;
+			break;
+		case DOCK_KEY_VOL_DOWN_PRESSED:
+			code = KEY_VOLUMEDOWN;
+			state = 0;
+			info->previous_key = DOCK_KEY_VOL_DOWN_RELEASED;
+			break;
+		case DOCK_KEY_PREV_PRESSED:
+			code = KEY_PREVIOUSSONG;
+			state = 0;
+			info->previous_key = DOCK_KEY_PREV_RELEASED;
+			break;
+		case DOCK_KEY_PLAY_PAUSE_PRESSED:
+			code = KEY_PLAYPAUSE;
+			state = 0;
+			info->previous_key = DOCK_KEY_PLAY_PAUSE_RELEASED;
+			break;
+		case DOCK_KEY_NEXT_PRESSED:
+			code = KEY_NEXTSONG;
+			state = 0;
+			info->previous_key = DOCK_KEY_NEXT_RELEASED;
+			break;
+		default:
+			return 0;
+		}
+		input_event(input, EV_KEY, code, state);
+		input_sync(input);
+		return 0;
+	}
+
+	if (pre_key == DOCK_KEY_NONE) {
+		/*
+		if (adc != ADC_DOCK_VOL_UP && adc != ADC_DOCK_VOL_DN && \
+		adc != ADC_DOCK_PREV_KEY && adc != ADC_DOCK_PLAY_PAUSE_KEY \
+		&& adc != ADC_DOCK_NEXT_KEY)
+		*/
+		if ((adc < 0x03) || (adc > 0x0d))
+			return 0;
+	}
+
+	dev_info(info->dev, "%s: dock vol key(%d)\n", __func__, pre_key);
+
+	switch (adc) {
+	case ADC_DOCK_VOL_UP:
+		code = KEY_VOLUMEUP;
+		state = 1;
+		info->previous_key = DOCK_KEY_VOL_UP_PRESSED;
+		break;
+	case ADC_DOCK_VOL_DN:
+		code = KEY_VOLUMEDOWN;
+		state = 1;
+		info->previous_key = DOCK_KEY_VOL_DOWN_PRESSED;
+		break;
+	case ADC_DOCK_PREV_KEY-1 ... ADC_DOCK_PREV_KEY+1:
+		code = KEY_PREVIOUSSONG;
+		state = 1;
+		info->previous_key = DOCK_KEY_PREV_PRESSED;
+		break;
+	case ADC_DOCK_PLAY_PAUSE_KEY-1 ... ADC_DOCK_PLAY_PAUSE_KEY+1:
+		code = KEY_PLAYPAUSE;
+		state = 1;
+		info->previous_key = DOCK_KEY_PLAY_PAUSE_PRESSED;
+		break;
+	case ADC_DOCK_NEXT_KEY-1 ... ADC_DOCK_NEXT_KEY+1:
+		code = KEY_NEXTSONG;
+		state = 1;
+		info->previous_key = DOCK_KEY_NEXT_PRESSED;
+		break;
+	case ADC_DESKDOCK: /* key release routine */
+		if (pre_key == DOCK_KEY_VOL_UP_PRESSED) {
+			code = KEY_VOLUMEUP;
+			state = 0;
+			info->previous_key = DOCK_KEY_VOL_UP_RELEASED;
+		} else if (pre_key == DOCK_KEY_VOL_DOWN_PRESSED) {
+			code = KEY_VOLUMEDOWN;
+			state = 0;
+			info->previous_key = DOCK_KEY_VOL_DOWN_RELEASED;
+		} else if (pre_key == DOCK_KEY_PREV_PRESSED) {
+			code = KEY_PREVIOUSSONG;
+			state = 0;
+			info->previous_key = DOCK_KEY_PREV_RELEASED;
+		} else if (pre_key == DOCK_KEY_PLAY_PAUSE_PRESSED) {
+			code = KEY_PLAYPAUSE;
+			state = 0;
+			info->previous_key = DOCK_KEY_PLAY_PAUSE_RELEASED;
+		} else if (pre_key == DOCK_KEY_NEXT_PRESSED) {
+			code = KEY_NEXTSONG;
+			state = 0;
+			info->previous_key = DOCK_KEY_NEXT_RELEASED;
+		} else {
+			dev_warn(info->dev, "%s:%d should not reach here\n",
+				 __func__, __LINE__);
+			return 0;
+		}
+		break;
+	default:
+		dev_warn(info->dev, "%s: unsupported ADC(0x%02x)\n", __func__,
+			 adc);
+		return 0;
+	}
+
+	input_event(input, EV_KEY, code, state);
+	input_sync(input);
+
+	return 1;
+}
+
+static int max77693_muic_attach_usb_type(struct max77693_muic_info *info,
+					 int adc)
+{
+	struct max77693_muic_data *mdata = info->muic_data;
+	int ret, path;
+	dev_info(info->dev, "func:%s adc:%x cable_type:%d\n",
+		 __func__, adc, info->cable_type);
+	if (info->cable_type == CABLE_TYPE_MHL_MUIC
+	    || info->cable_type == CABLE_TYPE_MHL_VB_MUIC) {
+		dev_warn(info->dev, "%s: mHL was attached!\n", __func__);
+		return 0;
+	}
+
+	switch (adc) {
+	case ADC_JIG_USB_OFF:
+		if (info->cable_type == CABLE_TYPE_JIG_USB_OFF_MUIC) {
+			dev_info(info->dev, "%s: duplicated(JIG USB OFF)\n",
+				 __func__);
+			return 0;
+		}
+
+		dev_info(info->dev, "%s:JIG USB BOOTOFF\n", __func__);
+		info->cable_type = CABLE_TYPE_JIG_USB_OFF_MUIC;
+		path = AP_USB_MODE;
+		break;
+	case ADC_JIG_USB_ON:
+		if (info->cable_type == CABLE_TYPE_JIG_USB_ON_MUIC) {
+			dev_info(info->dev, "%s: duplicated(JIG USB ON)\n",
+				 __func__);
+			return 0;
+		}
+
+		dev_info(info->dev, "%s:JIG USB BOOTON\n", __func__);
+		info->cable_type = CABLE_TYPE_JIG_USB_ON_MUIC;
+		path = AP_USB_MODE;
+		break;
+	case ADC_OPEN:
+		if (info->cable_type == CABLE_TYPE_USB_MUIC) {
+			dev_info(info->dev, "%s: duplicated(USB)\n", __func__);
+			return 0;
+		}
+
+		dev_info(info->dev, "%s:USB\n", __func__);
+		info->cable_type = CABLE_TYPE_USB_MUIC;
+		path = AP_USB_MODE;
+		break;
+	default:
+		dev_info(info->dev, "%s: Unkown cable(0x%x)\n", __func__, adc);
+		return 0;
+	}
+
+	ret = max77693_muic_set_charging_type(info, false);
+	if (ret) {
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+		return ret;
+	}
+
+	if (mdata->sw_path == CP_USB_MODE) {
+		info->cable_type = CABLE_TYPE_USB_MUIC;
+#if defined(CONFIG_MACH_M0_CTC)
+		if (system_rev < 11) {
+			gpio_direction_output(GPIO_USB_BOOT_EN, 1);
+		} else if (system_rev == 11) {
+			gpio_direction_output(GPIO_USB_BOOT_EN, 1);
+			gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 1);
+		} else {
+			gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 1);
+		}
+		cp_usb_state = 1;
+#endif
+		max77693_muic_set_usb_path(info, CP_USB_MODE);
+		return 0;
+	}
+
+	max77693_muic_set_usb_path(info, path);
+
+	if (path == AP_USB_MODE) {
+		if (mdata->usb_cb && info->is_usb_ready)
+#ifdef CONFIG_USBHUB_USB3803
+			/* setting usb hub in Diagnostic(hub) mode */
+			usb3803_set_mode(USB_3803_MODE_HUB);
+#endif				/* CONFIG_USBHUB_USB3803 */
+		mdata->usb_cb(USB_CABLE_ATTACHED);
+	}
+
+	return 0;
+}
+
+static int max77693_muic_attach_dock_type(struct max77693_muic_info *info,
+					  int adc)
+{
+	struct max77693_muic_data *mdata = info->muic_data;
+	int path;
+	dev_info(info->dev, "func:%s adc:%x, open(%d)\n",
+			__func__, adc, info->is_adc_open_prev);
+	/*Workaround for unstable adc*/
+	if (info->is_adc_open_prev == false) {
+		return 0;
+	}
+	switch (adc) {
+	case ADC_DESKDOCK:
+		/* Desk Dock */
+		if (info->cable_type == CABLE_TYPE_DESKDOCK_MUIC) {
+			dev_info(info->dev, "%s: duplicated(DeskDock)\n",
+				 __func__);
+			return 0;
+		}
+		dev_info(info->dev, "%s:DeskDock\n", __func__);
+		info->cable_type = CABLE_TYPE_DESKDOCK_MUIC;
+		path = AUDIO_MODE;
+
+		if (mdata->deskdock_cb)
+			mdata->deskdock_cb(MAX77693_MUIC_ATTACHED);
+		break;
+	case ADC_CARDOCK:
+		/* Car Dock */
+		if (info->cable_type == CABLE_TYPE_CARDOCK_MUIC) {
+			dev_info(info->dev, "%s: duplicated(CarDock)\n",
+				 __func__);
+			return 0;
+		}
+		dev_info(info->dev, "%s:CarDock\n", __func__);
+		info->cable_type = CABLE_TYPE_CARDOCK_MUIC;
+		path = AUDIO_MODE;
+
+		if (mdata->cardock_cb)
+			mdata->cardock_cb(MAX77693_MUIC_ATTACHED);
+		break;
+	default:
+		dev_info(info->dev, "%s: should not reach here(0x%x)\n",
+			 __func__, adc);
+		return 0;
+	}
+
+	max77693_muic_set_usb_path(info, path);
+
+	return 0;
+}
+
+static void max77693_muic_attach_mhl(struct max77693_muic_info *info, u8 chgtyp)
+{
+	struct max77693_muic_data *mdata = info->muic_data;
+
+	dev_info(info->dev, "func:%s chgtyp:%x\n", __func__, chgtyp);
+
+	if (info->cable_type == CABLE_TYPE_USB_MUIC) {
+		if (mdata->usb_cb && info->is_usb_ready)
+			mdata->usb_cb(USB_CABLE_DETACHED);
+
+		max77693_muic_set_charging_type(info, true);
+	}
+#if 0
+	if (info->cable_type == CABLE_TYPE_MHL) {
+		dev_info(info->dev, "%s: duplicated(MHL)\n", __func__);
+		return;
+	}
+#endif
+	info->cable_type = CABLE_TYPE_MHL_MUIC;
+
+#ifdef CONFIG_EXTCON
+	if (info->edev && info->is_mhl_ready)
+		extcon_set_cable_state(info->edev, "MHL", true);
+#else
+	if (mdata->mhl_cb && info->is_mhl_ready)
+		mdata->mhl_cb(MAX77693_MUIC_ATTACHED);
+#endif
+
+	if (chgtyp == CHGTYP_USB) {
+		info->cable_type = CABLE_TYPE_MHL_VB_MUIC;
+		max77693_muic_set_charging_type(info, false);
+	}
+}
+
+static void max77693_muic_handle_jig_uart(struct max77693_muic_info *info,
+					  u8 vbvolt)
+{
+	struct max77693_muic_data *mdata = info->muic_data;
+	enum cable_type_muic prev_ct = info->cable_type;
+	bool is_otgtest = false;
+	u8 cntl1_val, cntl1_msk;
+	u8 val = MAX77693_MUIC_CTRL1_BIN_3_011;
+	dev_info(info->dev, "func:%s vbvolt:%x cable_type:%d\n",
+		 __func__, vbvolt, info->cable_type);
+	dev_info(info->dev, "%s: JIG UART/BOOTOFF(0x%x)\n", __func__, vbvolt);
+
+	if (info->max77693->pmic_rev >= MAX77693_REV_PASS2) {
+		if (info->muic_data->uart_path == UART_PATH_AP) {
+			if (info->is_default_uart_path_cp)
+				val = MAX77693_MUIC_CTRL1_BIN_5_101;
+			else
+				val = MAX77693_MUIC_CTRL1_BIN_3_011;
+		} else if (info->muic_data->uart_path == UART_PATH_CP) {
+			if (info->is_default_uart_path_cp)
+				val = MAX77693_MUIC_CTRL1_BIN_3_011;
+			else
+				val = MAX77693_MUIC_CTRL1_BIN_5_101;
+#ifdef CONFIG_LTE_VIA_SWITCH
+			dev_info(info->dev, "%s: cpbooting is %s\n",
+				__func__,
+				cpbooting ?
+				"started. skip path set" : "done. set path");
+			if (!cpbooting) {
+				if (gpio_is_valid(GPIO_LTE_VIA_UART_SEL)) {
+					gpio_set_value(GPIO_LTE_VIA_UART_SEL,
+						GPIO_LEVEL_HIGH);
+					dev_info(info->dev,
+						"%s: LTE_GPIO_LEVEL_HIGH"
+						, __func__);
+				} else {
+					dev_err(info->dev,
+						"%s: ERR_LTE_GPIO_SET_HIGH\n"
+						, __func__);
+				}
+			}
+#endif
+		}
+#ifdef CONFIG_LTE_VIA_SWITCH
+		else if (info->muic_data->uart_path == UART_PATH_LTE) {
+			val = MAX77693_MUIC_CTRL1_BIN_5_101;
+			/*TODO must modify H/W rev.5*/
+			if (gpio_is_valid(GPIO_LTE_VIA_UART_SEL)) {
+				gpio_set_value(GPIO_LTE_VIA_UART_SEL,
+					GPIO_LEVEL_LOW);
+				dev_info(info->dev, "%s: LTE_GPIO_LEVEL_LOW\n"
+								, __func__);
+			} else
+				dev_err(info->dev, "%s: ERR_LTE_GPIO_SET_LOW\n"
+								, __func__);
+		}
+#endif
+		else
+			val = MAX77693_MUIC_CTRL1_BIN_3_011;
+	} else
+		val = MAX77693_MUIC_CTRL1_BIN_3_011;
+
+	cntl1_val = (val << COMN1SW_SHIFT) | (val << COMP2SW_SHIFT);
+	cntl1_msk = COMN1SW_MASK | COMP2SW_MASK;
+	max77693_update_reg(info->muic, MAX77693_MUIC_REG_CTRL1, cntl1_val,
+			    cntl1_msk);
+
+	max77693_update_reg(info->muic, MAX77693_MUIC_REG_CTRL2,
+				CTRL2_CPEn1_LOWPWD0,
+				CTRL2_CPEn_MASK | CTRL2_LOWPWD_MASK);
+
+	if (vbvolt & STATUS2_VBVOLT_MASK) {
+		if (mdata->host_notify_cb) {
+			if (mdata->host_notify_cb(1) == NOTIFY_TEST_MODE) {
+				is_otgtest = true;
+				dev_info(info->dev, "%s: OTG TEST\n", __func__);
+			}
+		}
+
+		info->cable_type = CABLE_TYPE_JIG_UART_OFF_VB_MUIC;
+		max77693_muic_set_charging_type(info, is_otgtest);
+
+	} else {
+		info->cable_type = CABLE_TYPE_JIG_UART_OFF_MUIC;
+#if 0
+		if (mdata->uart_path == UART_PATH_CP && mdata->jig_uart_cb)
+			mdata->jig_uart_cb(UART_PATH_CP);
+#endif
+		if (prev_ct == CABLE_TYPE_JIG_UART_OFF_VB_MUIC) {
+			max77693_muic_set_charging_type(info, false);
+
+			if (mdata->host_notify_cb)
+				mdata->host_notify_cb(0);
+		}
+	}
+}
+
+void max77693_otg_control(struct max77693_muic_info *info, int enable)
+{
+	u8 int_mask, cdetctrl1, chg_cnfg_00;
+	pr_info("%s: enable(%d)\n", __func__, enable);
+
+	if (enable) {
+		/* disable charger interrupt */
+		max77693_read_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_INT_MASK, &int_mask);
+		chg_int_state = int_mask;
+		int_mask |= (1 << 4);	/* disable chgin intr */
+		int_mask |= (1 << 6);	/* disable chg */
+		int_mask &= ~(1 << 0);	/* enable byp intr */
+		max77693_write_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_INT_MASK, int_mask);
+
+		/* disable charger detection */
+		max77693_read_reg(info->max77693->muic,
+			MAX77693_MUIC_REG_CDETCTRL1, &cdetctrl1);
+		cdetctrl1 &= ~(1 << 0);
+		max77693_write_reg(info->max77693->muic,
+			MAX77693_MUIC_REG_CDETCTRL1, cdetctrl1);
+
+		/* OTG on, boost on, DIS_MUIC_CTRL=1 */
+		max77693_read_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_CNFG_00, &chg_cnfg_00);
+		chg_cnfg_00 &= ~(CHG_CNFG_00_CHG_MASK
+				| CHG_CNFG_00_OTG_MASK
+				| CHG_CNFG_00_BUCK_MASK
+				| CHG_CNFG_00_BOOST_MASK
+				| CHG_CNFG_00_DIS_MUIC_CTRL_MASK);
+		chg_cnfg_00 |= (CHG_CNFG_00_OTG_MASK
+				| CHG_CNFG_00_BOOST_MASK
+				| CHG_CNFG_00_DIS_MUIC_CTRL_MASK);
+		max77693_write_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_CNFG_00, chg_cnfg_00);
+	} else {
+		/* OTG off, boost off, (buck on),
+		   DIS_MUIC_CTRL = 0 unless CHG_ENA = 1 */
+		max77693_read_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_CNFG_00, &chg_cnfg_00);
+		chg_cnfg_00 &= ~(CHG_CNFG_00_OTG_MASK
+				| CHG_CNFG_00_BOOST_MASK
+				| CHG_CNFG_00_DIS_MUIC_CTRL_MASK);
+		chg_cnfg_00 |= CHG_CNFG_00_BUCK_MASK;
+		max77693_write_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_CNFG_00, chg_cnfg_00);
+
+		mdelay(50);
+
+		/* enable charger detection */
+		max77693_read_reg(info->max77693->muic,
+			MAX77693_MUIC_REG_CDETCTRL1, &cdetctrl1);
+		cdetctrl1 |= (1 << 0);
+		max77693_write_reg(info->max77693->muic,
+			MAX77693_MUIC_REG_CDETCTRL1, cdetctrl1);
+
+		/* enable charger interrupt */
+		max77693_write_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_INT_MASK, chg_int_state);
+		max77693_read_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_INT_MASK, &int_mask);
+	}
+
+	pr_info("%s: INT_MASK(0x%x), CDETCTRL1(0x%x), CHG_CNFG_00(0x%x)\n",
+				__func__, int_mask, cdetctrl1, chg_cnfg_00);
+}
+
+void max77693_powered_otg_control(struct max77693_muic_info *info, int enable)
+{
+	pr_info("%s: enable(%d)\n", __func__, enable);
+
+	if (enable) {
+		/* OTG on, boost on */
+		max77693_write_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_CNFG_00, 0x05);
+
+		max77693_write_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_CNFG_02, 0x0E);
+	} else {
+		/* OTG off, boost off, (buck on) */
+		max77693_write_reg(info->max77693->i2c,
+			MAX77693_CHG_REG_CHG_CNFG_00, 0x04);
+	}
+}
+/* use in mach for otg */
+void otg_control(int enable)
+{
+	pr_debug("%s: enable(%d)\n", __func__, enable);
+
+	max77693_otg_control(gInfo, enable);
+}
+
+/* use in mach for powered-otg */
+void powered_otg_control(int enable)
+{
+	pr_debug("%s: enable(%d)\n", __func__, enable);
+
+	max77693_powered_otg_control(gInfo, enable);
+}
+
+static int max77693_muic_handle_attach(struct max77693_muic_info *info,
+				       u8 status1, u8 status2, int irq)
+{
+	struct max77693_muic_data *mdata = info->muic_data;
+	u8 adc, vbvolt, chgtyp, chgdetrun, adc1k;
+	int ret = 0;
+
+	adc = status1 & STATUS1_ADC_MASK;
+	adc1k = status1 & STATUS1_ADC1K_MASK;
+	chgtyp = status2 & STATUS2_CHGTYP_MASK;
+	vbvolt = status2 & STATUS2_VBVOLT_MASK;
+	chgdetrun = status2 & STATUS2_CHGDETRUN_MASK;
+
+	dev_info(info->dev, "func:%s st1:%x st2:%x cable_type:%d\n",
+		 __func__, status1, status2, info->cable_type);
+	/* Workaround for Factory mode.
+	 * Abandon adc interrupt of approximately +-100K range
+	 * if previous cable status was JIG UART BOOT OFF.
+	 */
+	if (info->cable_type == CABLE_TYPE_JIG_UART_OFF_MUIC ||
+	    info->cable_type == CABLE_TYPE_JIG_UART_OFF_VB_MUIC) {
+		if (adc == (ADC_JIG_UART_OFF + 1) ||
+		    adc == (ADC_JIG_UART_OFF - 1)) {
+			/* Workaround for factory mode in MUIC PASS2
+			* In uart path cp, adc is unstable state
+			* MUIC PASS2 turn to AP_UART mode automatically
+			* So, in this state set correct path manually.
+			* !! NEEDED ONLY IF PMIC PASS2 !!
+			*/
+			if (info->muic_data->uart_path == UART_PATH_CP
+			&& info->max77693->pmic_rev >= MAX77693_REV_PASS2)
+				max77693_muic_handle_jig_uart(info, vbvolt);
+			dev_warn(info->dev, "%s: abandon ADC\n", __func__);
+			return 0;
+		}
+	}
+
+	if (info->cable_type == CABLE_TYPE_DESKDOCK_MUIC
+		&& adc != ADC_DESKDOCK) {
+		dev_warn(info->dev, "%s: assume deskdock detach\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+
+		max77693_muic_set_charging_type(info, false);
+		info->is_adc_open_prev = false;
+		if (mdata->deskdock_cb)
+			mdata->deskdock_cb(MAX77693_MUIC_DETACHED);
+	} else if (info->cable_type == CABLE_TYPE_CARDOCK_MUIC
+		   && adc != ADC_CARDOCK) {
+		dev_warn(info->dev, "%s: assume cardock detach\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+
+		max77693_muic_set_charging_type(info, false);
+		info->is_adc_open_prev = false;
+		if (mdata->cardock_cb)
+			mdata->cardock_cb(MAX77693_MUIC_DETACHED);
+	}
+
+	/* 1Kohm ID regiter detection (mHL)
+	 * Old MUIC : ADC value:0x00 or 0x01, ADCLow:1
+	 * New MUIC : ADC value is not set(Open), ADCLow:1, ADCError:1
+	 */
+	if (adc1k) {
+		if (irq == info->irq_adc
+			|| irq == info->irq_chgtype
+			|| irq == info->irq_vbvolt) {
+			dev_warn(info->dev,
+				 "%s: Ignore irq:%d at MHL detection\n",
+				 __func__, irq);
+			if (vbvolt) {
+				dev_info(info->dev, "%s: call charger_cb(%d)"
+					, __func__, vbvolt);
+				max77693_muic_set_charging_type(info, false);
+			} else {
+				dev_info(info->dev, "%s: call charger_cb(%d)"
+					, __func__, vbvolt);
+				max77693_muic_set_charging_type(info, true);
+			}
+			return 0;
+		}
+		max77693_muic_attach_mhl(info, chgtyp);
+		return 0;
+	}
+
+	switch (adc) {
+	case ADC_GND:
+		if (chgtyp == CHGTYP_NO_VOLTAGE) {
+			if (info->cable_type == CABLE_TYPE_OTG_MUIC) {
+				dev_info(info->dev,
+					 "%s: duplicated(OTG)\n", __func__);
+				break;
+			}
+
+			info->cable_type = CABLE_TYPE_OTG_MUIC;
+			if (mdata->usb_cb && info->is_usb_ready)
+				mdata->usb_cb(USB_OTGHOST_ATTACHED);
+
+			msleep(40);
+
+			max77693_muic_set_usb_path(info, AP_USB_MODE);
+		} else if (chgtyp == CHGTYP_USB ||
+			   chgtyp == CHGTYP_DOWNSTREAM_PORT ||
+			   chgtyp == CHGTYP_DEDICATED_CHGR ||
+			   chgtyp == CHGTYP_500MA || chgtyp == CHGTYP_1A) {
+			dev_info(info->dev, "%s: OTG charging pump\n",
+				 __func__);
+			ret = max77693_muic_set_charging_type(info, false);
+		}
+		break;
+	case ADC_SMARTDOCK:
+		if (info->cable_type == CABLE_TYPE_SMARTDOCK_MUIC) {
+			dev_info(info->dev,
+			"%s: duplicated(SMARTDOCK)\n", __func__);
+				break;
+		}
+		dev_info(info->dev, "func:%s Attach SmartDock\n", __func__);
+		info->cable_type = CABLE_TYPE_SMARTDOCK_MUIC;
+		max77693_muic_set_usb_path(info, AP_USB_MODE);
+		msleep(40);
+		if (mdata->usb_cb && info->is_usb_ready)
+			mdata->usb_cb(USB_POWERED_HOST_ATTACHED);
+#ifdef CONFIG_EXTCON
+		if (info->edev && info->is_mhl_ready)
+			extcon_set_cable_state(info->edev, "MHL", true);
+#else
+		if (mdata->mhl_cb && info->is_mhl_ready)
+			mdata->mhl_cb(MAX77693_MUIC_ATTACHED);
+#endif
+		max77693_muic_set_charging_type(info, false);
+		break;
+	case ADC_JIG_UART_OFF:
+		max77693_muic_handle_jig_uart(info, vbvolt);
+#if defined(CONFIG_SEC_MODEM_M0_TD)
+		gpio_set_value(GPIO_AP_CP_INT1, GPIO_LEVEL_HIGH);
+#endif
+		mdata->jig_state(true);
+		break;
+	case ADC_JIG_USB_OFF:
+	case ADC_JIG_USB_ON:
+		if (vbvolt & STATUS2_VBVOLT_MASK) {
+			dev_info(info->dev, "%s: SKIP_JIG_USB\n", __func__);
+			ret = max77693_muic_attach_usb_type(info, adc);
+		}
+#if defined(CONFIG_SEC_MODEM_M0_TD)
+		gpio_set_value(GPIO_AP_CP_INT1, GPIO_LEVEL_HIGH);
+#endif
+		mdata->jig_state(true);
+		break;
+	case ADC_DESKDOCK:
+	case ADC_CARDOCK:
+		max77693_muic_attach_dock_type(info, adc);
+		if (chgtyp == CHGTYP_USB ||
+			chgtyp == CHGTYP_DOWNSTREAM_PORT ||
+			chgtyp == CHGTYP_DEDICATED_CHGR ||
+			chgtyp == CHGTYP_500MA || chgtyp == CHGTYP_1A)
+			ret = max77693_muic_set_charging_type(info, false);
+		else if (chgtyp == CHGTYP_NO_VOLTAGE && !chgdetrun)
+			ret = max77693_muic_set_charging_type(info, !vbvolt);
+			/* For MAX77693 IC doesn`t occur chgtyp IRQ
+			* because of audio noise prevention.
+			* So, If below condition is set,
+			* we do charging at CARDOCK.
+			*/
+			break;
+	case ADC_CEA936ATYPE1_CHG:
+	case ADC_CEA936ATYPE2_CHG:
+	case ADC_OPEN:
+		switch (chgtyp) {
+		case CHGTYP_USB:
+		case CHGTYP_DOWNSTREAM_PORT:
+			if (adc == ADC_CEA936ATYPE1_CHG
+			    || adc == ADC_CEA936ATYPE2_CHG)
+				break;
+			if (info->cable_type == CABLE_TYPE_MHL_MUIC) {
+				dev_info(info->dev, "%s: MHL(charging)\n",
+					 __func__);
+				info->cable_type = CABLE_TYPE_MHL_VB_MUIC;
+				ret = max77693_muic_set_charging_type(info,
+								      false);
+				return ret;
+			}
+#ifdef CONFIG_EXTCON
+			if (info->edev)
+				extcon_set_cable_state(info->edev,
+					"USB", true);
+#endif
+			ret = max77693_muic_attach_usb_type(info, adc);
+			break;
+		case CHGTYP_DEDICATED_CHGR:
+		case CHGTYP_500MA:
+		case CHGTYP_1A:
+			dev_info(info->dev, "%s:TA\n", __func__);
+			info->cable_type = CABLE_TYPE_TA_MUIC;
+#ifdef CONFIG_EXTCON
+			if (info->edev)
+				extcon_set_cable_state(info->edev,
+					"TA", true);
+#endif
+#ifdef CONFIG_USBHUB_USB3803
+			/* setting usb hub in default mode (standby) */
+			usb3803_set_mode(USB_3803_MODE_STANDBY);
+#endif				/* CONFIG_USBHUB_USB3803 */
+			ret = max77693_muic_set_charging_type(info, false);
+			if (ret)
+				info->cable_type = CABLE_TYPE_NONE_MUIC;
+			break;
+		default:
+			break;
+		}
+		break;
+	default:
+		dev_warn(info->dev, "%s: unsupported adc=0x%x\n", __func__,
+			 adc);
+		break;
+	}
+	return ret;
+}
+
+static int max77693_muic_handle_detach(struct max77693_muic_info *info, int irq)
+{
+	struct i2c_client *client = info->muic;
+	struct max77693_muic_data *mdata = info->muic_data;
+	enum cable_type_muic prev_ct = CABLE_TYPE_NONE_MUIC;
+	u8 cntl2_val;
+	int ret = 0;
+	dev_info(info->dev, "func:%s\n", __func__);
+
+	info->is_adc_open_prev = true;
+	/* Workaround: irq doesn't occur after detaching mHL cable */
+	max77693_write_reg(client, MAX77693_MUIC_REG_CTRL1,
+				MAX77693_MUIC_CTRL1_BIN_0_000);
+
+	/* Enable Factory Accessory Detection State Machine */
+	max77693_update_reg(client, MAX77693_MUIC_REG_CTRL2,
+			    (1 << CTRL2_ACCDET_SHIFT), CTRL2_ACCDET_MASK);
+
+	max77693_update_reg(client, MAX77693_MUIC_REG_CTRL2,
+				CTRL2_CPEn0_LOWPWD1,
+				CTRL2_CPEn_MASK | CTRL2_LOWPWD_MASK);
+
+	max77693_read_reg(client, MAX77693_MUIC_REG_CTRL2, &cntl2_val);
+	dev_info(info->dev, "%s: CNTL2(0x%02x)\n", __func__, cntl2_val);
+
+#if defined(CONFIG_MACH_M0_CTC)
+	if (cp_usb_state != 0) {
+		if (system_rev < 11) {
+			gpio_direction_output(GPIO_USB_BOOT_EN, 0);
+		} else if (system_rev == 11) {
+			gpio_direction_output(GPIO_USB_BOOT_EN, 0);
+			gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 0);
+		} else {
+			gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 0);
+		}
+	cp_usb_state = 0;
+	}
+#endif
+
+#ifdef CONFIG_USBHUB_USB3803
+	/* setting usb hub in default mode (standby) */
+	usb3803_set_mode(USB_3803_MODE_STANDBY);
+#endif  /* CONFIG_USBHUB_USB3803 */
+	info->previous_key = DOCK_KEY_NONE;
+
+	if (info->cable_type == CABLE_TYPE_NONE_MUIC) {
+		dev_info(info->dev, "%s: duplicated(NONE)\n", __func__);
+		return 0;
+	}
+#if 0
+	if (mdata->jig_uart_cb)
+		mdata->jig_uart_cb(UART_PATH_AP);
+#endif
+
+	switch (info->cable_type) {
+	case CABLE_TYPE_OTG_MUIC:
+		dev_info(info->dev, "%s: OTG\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+
+		if (mdata->usb_cb && info->is_usb_ready)
+			mdata->usb_cb(USB_OTGHOST_DETACHED);
+		break;
+	case CABLE_TYPE_USB_MUIC:
+	case CABLE_TYPE_JIG_USB_OFF_MUIC:
+	case CABLE_TYPE_JIG_USB_ON_MUIC:
+#ifdef CONFIG_EXTCON
+		if (info->edev)
+			extcon_set_cable_state(info->edev, "USB", false);
+#endif
+		dev_info(info->dev, "%s: USB(0x%x)\n", __func__,
+			 info->cable_type);
+		prev_ct = info->cable_type;
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+
+		ret = max77693_muic_set_charging_type(info, false);
+		if (ret) {
+			info->cable_type = prev_ct;
+			break;
+		}
+
+		if (mdata->sw_path == CP_USB_MODE)
+			return 0;
+
+		if (mdata->usb_cb && info->is_usb_ready)
+			mdata->usb_cb(USB_CABLE_DETACHED);
+		break;
+	case CABLE_TYPE_DESKDOCK_MUIC:
+		dev_info(info->dev, "%s: DESKDOCK\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+
+		ret = max77693_muic_set_charging_type(info, false);
+		if (ret) {
+			info->cable_type = CABLE_TYPE_DESKDOCK_MUIC;
+			break;
+		}
+		if (mdata->deskdock_cb)
+			mdata->deskdock_cb(MAX77693_MUIC_DETACHED);
+		break;
+	case CABLE_TYPE_CARDOCK_MUIC:
+		dev_info(info->dev, "%s: CARDOCK\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+
+		ret = max77693_muic_set_charging_type(info, false);
+		if (ret) {
+			info->cable_type = CABLE_TYPE_CARDOCK_MUIC;
+			break;
+		}
+		if (mdata->cardock_cb)
+			mdata->cardock_cb(MAX77693_MUIC_DETACHED);
+		break;
+	case CABLE_TYPE_TA_MUIC:
+#ifdef CONFIG_EXTCON
+		if (info->edev)
+			extcon_set_cable_state(info->edev, "TA", false);
+#endif
+		dev_info(info->dev, "%s: TA\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+		ret = max77693_muic_set_charging_type(info, false);
+		if (ret)
+			info->cable_type = CABLE_TYPE_TA_MUIC;
+		break;
+	case CABLE_TYPE_JIG_UART_ON_MUIC:
+		dev_info(info->dev, "%s: JIG UART/BOOTON\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+		break;
+	case CABLE_TYPE_JIG_UART_OFF_MUIC:
+		dev_info(info->dev, "%s: JIG UART/BOOTOFF\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+		break;
+	case CABLE_TYPE_SMARTDOCK_MUIC:
+		dev_info(info->dev, "%s: SMARTDOCK\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+		if (mdata->usb_cb && info->is_usb_ready)
+			mdata->usb_cb(USB_POWERED_HOST_DETACHED);
+		ret = max77693_muic_set_charging_type(info, false);
+#ifdef CONFIG_EXTCON
+		if (info->edev && info->is_mhl_ready)
+			extcon_set_cable_state(info->edev, "MHL", false);
+#else
+		if (mdata->mhl_cb && info->is_mhl_ready)
+			mdata->mhl_cb(MAX77693_MUIC_DETACHED);
+#endif
+		max77693_muic_set_charging_type(info, false);
+		break;
+	case CABLE_TYPE_JIG_UART_OFF_VB_MUIC:
+		dev_info(info->dev, "%s: JIG UART/OFF/VB\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+		ret = max77693_muic_set_charging_type(info, false);
+		if (ret)
+			info->cable_type = CABLE_TYPE_JIG_UART_OFF_VB_MUIC;
+		break;
+	case CABLE_TYPE_MHL_MUIC:
+		if (irq == info->irq_adc || irq == info->irq_chgtype) {
+			dev_warn(info->dev, "Detech mhl: Ignore irq:%d\n", irq);
+			break;
+		}
+		dev_info(info->dev, "%s: MHL\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+		max77693_muic_set_charging_type(info, false);
+#ifdef CONFIG_EXTCON
+		if (info->edev && info->is_mhl_ready)
+			extcon_set_cable_state(info->edev, "MHL", false);
+#else
+		if (mdata->mhl_cb && info->is_mhl_ready)
+			mdata->mhl_cb(MAX77693_MUIC_DETACHED);
+#endif
+
+		break;
+	case CABLE_TYPE_MHL_VB_MUIC:
+		if (irq == info->irq_adc || irq == info->irq_chgtype) {
+			dev_warn(info->dev,
+				 "Detech vbMhl: Ignore irq:%d\n", irq);
+			break;
+		}
+		dev_info(info->dev, "%s: MHL VBUS\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+		max77693_muic_set_charging_type(info, false);
+
+#ifdef CONFIG_EXTCON
+		if (info->edev && info->is_mhl_ready)
+			extcon_set_cable_state(info->edev, "MHL", false);
+#else
+		if (mdata->mhl_cb && info->is_mhl_ready)
+			mdata->mhl_cb(MAX77693_MUIC_DETACHED);
+#endif
+		break;
+	case CABLE_TYPE_UNKNOWN_MUIC:
+		dev_info(info->dev, "%s: UNKNOWN\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE_MUIC;
+
+		ret = max77693_muic_set_charging_type(info, false);
+		if (ret)
+			info->cable_type = CABLE_TYPE_UNKNOWN_MUIC;
+		break;
+	default:
+		dev_info(info->dev, "%s:invalid cable type %d\n",
+			 __func__, info->cable_type);
+		break;
+	}
+
+	/* jig state clear */
+	mdata->jig_state(false);
+#if defined(CONFIG_SEC_MODEM_M0_TD)
+	gpio_set_value(GPIO_AP_CP_INT1, GPIO_LEVEL_LOW);
+#endif
+	return ret;
+}
+
+static void max77693_muic_detect_dev(struct max77693_muic_info *info, int irq)
+{
+	struct i2c_client *client = info->muic;
+	u8 status[2];
+	u8 adc, chgtyp, adcerr;
+	int intr = INT_ATTACH;
+	int ret;
+	u8 cntl1_val;
+
+	ret = max77693_read_reg(client, MAX77693_MUIC_REG_CTRL1, &cntl1_val);
+	dev_info(info->dev, "func:%s CONTROL1:%x\n", __func__, cntl1_val);
+
+	ret = max77693_bulk_read(client, MAX77693_MUIC_REG_STATUS1, 2, status);
+	dev_info(info->dev, "func:%s irq:%d ret:%d\n", __func__, irq, ret);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg(%d)\n", __func__,
+			ret);
+		return;
+	}
+
+	dev_info(info->dev, "%s: STATUS1:0x%x, 2:0x%x\n", __func__,
+		 status[0], status[1]);
+
+	if ((irq == info->irq_adc) &&
+	    max77693_muic_handle_dock_vol_key(info, status[0])) {
+		dev_info(info->dev,
+			 "max77693_muic_handle_dock_vol_key(irq_adc:%x)", irq);
+		return;
+	}
+
+	adc = status[0] & STATUS1_ADC_MASK;
+	adcerr = status[0] & STATUS1_ADCERR_MASK;
+	chgtyp = status[1] & STATUS2_CHGTYP_MASK;
+
+	dev_info(info->dev, "adc:%x adcerr:%x chgtyp:%xcable_type:%x\n", adc,
+		 adcerr, chgtyp, info->cable_type);
+
+	if (!adcerr && adc == ADC_OPEN) {
+		if (chgtyp == CHGTYP_NO_VOLTAGE)
+			intr = INT_DETACH;
+		else if (chgtyp == CHGTYP_USB ||
+			 chgtyp == CHGTYP_DOWNSTREAM_PORT ||
+			 chgtyp == CHGTYP_DEDICATED_CHGR ||
+			 chgtyp == CHGTYP_500MA || chgtyp == CHGTYP_1A) {
+			if (info->cable_type == CABLE_TYPE_OTG_MUIC ||
+			    info->cable_type == CABLE_TYPE_DESKDOCK_MUIC ||
+			    info->cable_type == CABLE_TYPE_CARDOCK_MUIC)
+				intr = INT_DETACH;
+		}
+	}
+
+	if (intr == INT_ATTACH) {
+		dev_info(info->dev, "%s: ATTACHED\n", __func__);
+		max77693_muic_handle_attach(info, status[0], status[1], irq);
+	} else {
+		dev_info(info->dev, "%s: DETACHED\n", __func__);
+		max77693_muic_handle_detach(info, irq);
+	}
+	return;
+}
+static irqreturn_t max77693_muic_irq(int irq, void *data)
+{
+	struct max77693_muic_info *info = data;
+	dev_info(info->dev, "%s: irq:%d\n", __func__, irq);
+
+	mutex_lock(&info->mutex);
+	max77693_muic_detect_dev(info, irq);
+	mutex_unlock(&info->mutex);
+
+	return IRQ_HANDLED;
+}
+
+#define REQUEST_IRQ(_irq, _name)					\
+do {									\
+	ret = request_threaded_irq(_irq, NULL, max77693_muic_irq,	\
+				    0, _name, info);			\
+	if (ret < 0)							\
+		dev_err(info->dev, "Failed to request IRQ #%d: %d\n",	\
+			_irq, ret);					\
+} while (0)
+
+static int max77693_muic_irq_init(struct max77693_muic_info *info)
+{
+	int ret;
+	u8 val;
+
+	dev_info(info->dev, "func:%s\n", __func__);
+	dev_info(info->dev, "%s: system_rev=%x\n", __func__, system_rev);
+
+	/* INTMASK1  3:ADC1K 2:ADCErr 1:ADCLow 0:ADC */
+	/* INTMASK2  0:Chgtype */
+	max77693_write_reg(info->muic, MAX77693_MUIC_REG_INTMASK1, 0x09);
+	max77693_write_reg(info->muic, MAX77693_MUIC_REG_INTMASK2, 0x11);
+	max77693_write_reg(info->muic, MAX77693_MUIC_REG_INTMASK3, 0x00);
+
+	REQUEST_IRQ(info->irq_adc, "muic-adc");
+	REQUEST_IRQ(info->irq_chgtype, "muic-chgtype");
+	REQUEST_IRQ(info->irq_vbvolt, "muic-vbvolt");
+	REQUEST_IRQ(info->irq_adc1k, "muic-adc1k");
+
+	dev_info(info->dev, "adc:%d chgtype:%d adc1k:%d vbvolt:%d",
+		info->irq_adc, info->irq_chgtype,
+		info->irq_adc1k, info->irq_vbvolt);
+
+	max77693_read_reg(info->muic, MAX77693_MUIC_REG_INTMASK1, &val);
+	dev_info(info->dev, "%s: reg=%x, val=%x\n", __func__,
+		 MAX77693_MUIC_REG_INTMASK1, val);
+
+	max77693_read_reg(info->muic, MAX77693_MUIC_REG_INTMASK2, &val);
+	dev_info(info->dev, "%s: reg=%x, val=%x\n", __func__,
+		 MAX77693_MUIC_REG_INTMASK2, val);
+
+	max77693_read_reg(info->muic, MAX77693_MUIC_REG_INTMASK3, &val);
+	dev_info(info->dev, "%s: reg=%x, val=%x\n", __func__,
+		 MAX77693_MUIC_REG_INTMASK3, val);
+
+	max77693_read_reg(info->muic, MAX77693_MUIC_REG_INT1, &val);
+	dev_info(info->dev, "%s: reg=%x, val=%x\n", __func__,
+		 MAX77693_MUIC_REG_INT1, val);
+	max77693_read_reg(info->muic, MAX77693_MUIC_REG_INT2, &val);
+	dev_info(info->dev, "%s: reg=%x, val=%x\n", __func__,
+		 MAX77693_MUIC_REG_INT2, val);
+	max77693_read_reg(info->muic, MAX77693_MUIC_REG_INT3, &val);
+	dev_info(info->dev, "%s: reg=%x, val=%x\n", __func__,
+		 MAX77693_MUIC_REG_INT3, val);
+	return 0;
+}
+
+#define CHECK_GPIO(_gpio, _name)					\
+do {									\
+	if (!_gpio) {							\
+		dev_err(&pdev->dev, _name " GPIO defined as 0 !\n");	\
+		WARN_ON(!_gpio);					\
+		ret = -EIO;						\
+		goto err_kfree;						\
+	}								\
+} while (0)
+
+static void max77693_muic_init_detect(struct work_struct *work)
+{
+	struct max77693_muic_info *info =
+	    container_of(work, struct max77693_muic_info, init_work.work);
+	dev_info(info->dev, "func:%s\n", __func__);
+	mutex_lock(&info->mutex);
+	max77693_muic_detect_dev(info, -1);
+	mutex_unlock(&info->mutex);
+}
+
+static void max77693_muic_usb_detect(struct work_struct *work)
+{
+	struct max77693_muic_info *info =
+	    container_of(work, struct max77693_muic_info, usb_work.work);
+	struct max77693_muic_data *mdata = info->muic_data;
+
+	dev_info(info->dev, "func:%s info->muic_data->sw_path:%d\n",
+		 __func__, info->muic_data->sw_path);
+
+	mutex_lock(&info->mutex);
+	info->is_usb_ready = true;
+
+	if (info->muic_data->sw_path != CP_USB_MODE) {
+		if (mdata->usb_cb) {
+			switch (info->cable_type) {
+			case CABLE_TYPE_USB_MUIC:
+			case CABLE_TYPE_JIG_USB_OFF_MUIC:
+			case CABLE_TYPE_JIG_USB_ON_MUIC:
+#ifdef CONFIG_USBHUB_USB3803
+				/* setting usb hub in Diagnostic(hub) mode */
+				usb3803_set_mode(USB_3803_MODE_HUB);
+#endif				/* CONFIG_USBHUB_USB3803 */
+				mdata->usb_cb(USB_CABLE_ATTACHED);
+				break;
+			case CABLE_TYPE_OTG_MUIC:
+				mdata->usb_cb(USB_OTGHOST_ATTACHED);
+				break;
+			case CABLE_TYPE_SMARTDOCK_MUIC:
+				mdata->usb_cb(USB_POWERED_HOST_ATTACHED);
+				break;
+			default:
+				break;
+			}
+		}
+	}
+	mutex_unlock(&info->mutex);
+}
+
+static void max77693_muic_mhl_detect(struct work_struct *work)
+{
+	struct max77693_muic_info *info =
+	    container_of(work, struct max77693_muic_info, mhl_work.work);
+	struct max77693_muic_data *mdata = info->muic_data;
+
+	dev_info(info->dev, "func:%s cable_type:%d\n", __func__,
+		 info->cable_type);
+	mutex_lock(&info->mutex);
+	info->is_mhl_ready = true;
+
+	if (info->cable_type == CABLE_TYPE_MHL_MUIC ||
+	    info->cable_type == CABLE_TYPE_MHL_VB_MUIC) {
+#ifdef CONFIG_EXTCON
+		if (info->edev)
+			extcon_set_cable_state(info->edev, "MHL", true);
+#else
+		if (mdata->mhl_cb)
+			mdata->mhl_cb(MAX77693_MUIC_ATTACHED);
+#endif
+	}
+	mutex_unlock(&info->mutex);
+}
+
+static int uart_switch_init(struct max77693_muic_info *info)
+{
+	int ret, val;
+
+	if (info->max77693->pmic_rev < MAX77693_REV_PASS2) {
+		ret = gpio_request(GPIO_UART_SEL, "UART_SEL");
+		if (ret < 0) {
+			pr_err("Failed to request GPIO_UART_SEL!\n");
+			return -ENODEV;
+		}
+		s3c_gpio_setpull(GPIO_UART_SEL, S3C_GPIO_PULL_NONE);
+		val = gpio_get_value(GPIO_UART_SEL);
+		gpio_direction_output(GPIO_UART_SEL, val);
+		pr_info("func: %s uart_gpio val: %d\n", __func__, val);
+	}
+#ifdef CONFIG_LTE_VIA_SWITCH
+	ret = gpio_request(GPIO_LTE_VIA_UART_SEL, "LTE_VIA_SEL");
+	if (ret < 0) {
+		pr_err("Failed to request GPIO_LTE_VIA_UART_SEL!\n");
+		return -ENODEV;
+	}
+	s3c_gpio_setpull(GPIO_LTE_VIA_UART_SEL, S3C_GPIO_PULL_NONE);
+	val = gpio_get_value(GPIO_LTE_VIA_UART_SEL);
+	gpio_direction_output(GPIO_LTE_VIA_UART_SEL, val);
+	pr_info("func: %s lte_gpio val: %d\n", __func__, val);
+#endif
+/*
+#ifndef CONFIG_LTE_VIA_SWITCH
+	gpio_export(GPIO_UART_SEL, 1);
+	gpio_export_link(switch_dev, "uart_sel", GPIO_UART_SEL);
+#endif
+*/
+	return 0;
+}
+
+int max77693_muic_get_status1_adc1k_value(void)
+{
+	u8 adc1k;
+	int ret;
+
+	ret = max77693_read_reg(gInfo->muic,
+					MAX77693_MUIC_REG_STATUS1, &adc1k);
+	if (ret) {
+		dev_err(gInfo->dev, "%s: fail to read muic reg(%d)\n",
+					__func__, ret);
+		return -EINVAL;
+	}
+	adc1k = adc1k & STATUS1_ADC1K_MASK ? 1 : 0;
+
+	pr_info("func:%s, adc1k: %d\n", __func__, adc1k);
+	/* -1:err, 0:adc1k not detected, 1:adc1k detected */
+	return adc1k;
+}
+
+int max77693_muic_get_status1_adc_value(void)
+{
+	u8 adc;
+	int ret;
+
+	ret = max77693_read_reg(gInfo->muic,
+		MAX77693_MUIC_REG_STATUS1, &adc);
+	if (ret) {
+		dev_err(gInfo->dev, "%s: fail to read muic reg(%d)\n",
+			__func__, ret);
+		return -EINVAL;
+	}
+
+	return adc & STATUS1_ADC_MASK;
+}
+
+/*
+* func: max77693_muic_set_audio_switch
+* arg: bool enable(true:set vps path, false:set path open)
+* return: only 0 success
+*/
+int max77693_muic_set_audio_switch(bool enable)
+{
+	struct i2c_client *client = gInfo->muic;
+	u8 cntl1_val, cntl1_msk;
+	int ret;
+	pr_info("func:%s enable(%d)", __func__, enable);
+
+	if (enable) {
+		cntl1_val = (MAX77693_MUIC_CTRL1_BIN_2_010 << COMN1SW_SHIFT)
+		| (MAX77693_MUIC_CTRL1_BIN_2_010 << COMP2SW_SHIFT) |
+		(0 << MICEN_SHIFT);
+	} else {
+		cntl1_val = 0x3f;
+	}
+	cntl1_msk = COMN1SW_MASK | COMP2SW_MASK | MICEN_MASK;
+
+	ret = max77693_update_reg(client, MAX77693_MUIC_REG_CTRL1, cntl1_val,
+			    cntl1_msk);
+	cntl1_val = MAX77693_MUIC_CTRL1_BIN_0_000;
+	max77693_read_reg(client, MAX77693_MUIC_REG_CTRL1, &cntl1_val);
+	dev_info(gInfo->dev, "%s: CNTL1(0x%02x)\n", __func__, cntl1_val);
+	return ret;
+}
+
+void max77693_update_jig_state(struct max77693_muic_info *info)
+{
+	struct i2c_client *client = info->muic;
+	struct max77693_muic_data *mdata = info->muic_data;
+	u8 reg_data, adc;
+	int ret, jig_state;
+
+	ret = max77693_read_reg(client, MAX77693_MUIC_REG_STATUS1, &reg_data);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg(%d)\n",
+					__func__, ret);
+		return;
+	}
+	adc = reg_data & STATUS1_ADC_MASK;
+
+	switch (adc) {
+	case ADC_JIG_UART_OFF:
+	case ADC_JIG_USB_OFF:
+	case ADC_JIG_USB_ON:
+		jig_state = true;
+		break;
+	default:
+		jig_state = false;
+		break;
+	}
+
+	mdata->jig_state(jig_state);
+}
+
+static int __devinit max77693_muic_probe(struct platform_device *pdev)
+{
+	struct max77693_dev *max77693 = dev_get_drvdata(pdev->dev.parent);
+	struct max77693_platform_data *pdata = dev_get_platdata(max77693->dev);
+	struct max77693_muic_info *info;
+	struct input_dev *input;
+	int ret;
+	pr_info("func:%s\n", __func__);
+	info = kzalloc(sizeof(struct max77693_muic_info), GFP_KERNEL);
+	if (!info) {
+		dev_err(&pdev->dev, "%s: failed to allocate info\n", __func__);
+		ret = -ENOMEM;
+		goto err_return;
+	}
+	input = input_allocate_device();
+	if (!input) {
+		dev_err(&pdev->dev, "%s: failed to allocate input\n", __func__);
+		ret = -ENOMEM;
+		goto err_kfree;
+	}
+	info->dev = &pdev->dev;
+	info->max77693 = max77693;
+	info->muic = max77693->muic;
+	info->input = input;
+	info->irq_adc = max77693->irq_base + MAX77693_MUIC_IRQ_INT1_ADC;
+	info->irq_chgtype = max77693->irq_base + MAX77693_MUIC_IRQ_INT2_CHGTYP;
+	info->irq_vbvolt = max77693->irq_base + MAX77693_MUIC_IRQ_INT2_VBVOLT;
+	info->irq_adc1k = max77693->irq_base + MAX77693_MUIC_IRQ_INT1_ADC1K;
+	info->muic_data = pdata->muic;
+	info->is_adc_open_prev = true;
+
+	if (pdata->is_default_uart_path_cp)
+		info->is_default_uart_path_cp =
+			pdata->is_default_uart_path_cp();
+	else
+		info->is_default_uart_path_cp = false;
+	info->cable_type = CABLE_TYPE_UNKNOWN_MUIC;
+	info->muic_data->sw_path = AP_USB_MODE;
+	gInfo = info;
+
+	platform_set_drvdata(pdev, info);
+	dev_info(info->dev, "adc:%d chgtype:%d, adc1k%d\n",
+		 info->irq_adc, info->irq_chgtype, info->irq_adc1k);
+
+	input->name = pdev->name;
+	input->phys = "deskdock-key/input0";
+	input->dev.parent = &pdev->dev;
+
+	input->id.bustype = BUS_HOST;
+	input->id.vendor = 0x0001;
+	input->id.product = 0x0001;
+	input->id.version = 0x0001;
+
+	/* Enable auto repeat feature of Linux input subsystem */
+	__set_bit(EV_REP, input->evbit);
+
+	input_set_capability(input, EV_KEY, KEY_VOLUMEUP);
+	input_set_capability(input, EV_KEY, KEY_VOLUMEDOWN);
+	input_set_capability(input, EV_KEY, KEY_PLAYPAUSE);
+	input_set_capability(input, EV_KEY, KEY_PREVIOUSSONG);
+	input_set_capability(input, EV_KEY, KEY_NEXTSONG);
+
+	ret = input_register_device(input);
+	if (ret) {
+		dev_err(info->dev, "%s: Unable to register input device, "
+			"error: %d\n", __func__, ret);
+		goto err_input;
+	}
+
+	ret = uart_switch_init(info);
+	if (ret) {
+		pr_err("Failed to initialize uart\n");
+		goto err_input;
+	}
+
+	if (info->is_default_uart_path_cp)
+		dev_info(info->dev, "H/W rev connected UT1 UR2 pin to CP UART");
+	else
+		dev_info(info->dev, "H/W rev connected UT1 UR2 pin to AP UART");
+	/*PASS1 */
+	if (max77693->pmic_rev < MAX77693_REV_PASS2 &&
+	    gpio_is_valid(info->muic_data->gpio_usb_sel)) {
+		CHECK_GPIO(info->muic_data->gpio_usb_sel, "USB_SEL");
+
+		if (info->muic_data->cfg_uart_gpio)
+			info->muic_data->uart_path =
+			    info->muic_data->cfg_uart_gpio();
+
+		ret = gpio_request(info->muic_data->gpio_usb_sel, "USB_SEL");
+		if (ret) {
+			dev_info(info->dev, "%s: fail to request gpio(%d)\n",
+				 __func__, ret);
+			goto err_input;
+		}
+		if (gpio_get_value(info->muic_data->gpio_usb_sel)) {
+			dev_info(info->dev, "%s: CP USB\n", __func__);
+			info->muic_data->sw_path = CP_USB_MODE;
+		}
+	} else if (max77693->pmic_rev >= MAX77693_REV_PASS2) {
+		/*PASS2 */
+		int switch_sel = get_switch_sel();
+		if (switch_sel & MAX77693_SWITCH_SEL_1st_BIT_USB)
+			info->muic_data->sw_path = AP_USB_MODE;
+		else
+			info->muic_data->sw_path = CP_USB_MODE;
+		if (switch_sel & MAX77693_SWITCH_SEL_2nd_BIT_UART)
+			info->muic_data->uart_path = UART_PATH_AP;
+		else {
+			info->muic_data->uart_path = UART_PATH_CP;
+#ifdef CONFIG_LTE_VIA_SWITCH
+			if (switch_sel & MAX77693_SWITCH_SEL_3rd_BIT_LTE_UART)
+				info->muic_data->uart_path = UART_PATH_LTE;
+#endif
+		}
+		pr_info("%s: switch_sel: %x\n", __func__, switch_sel);
+	}
+	/* create sysfs group */
+	ret = sysfs_create_group(&switch_dev->kobj, &max77693_muic_group);
+	dev_set_drvdata(switch_dev, info);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"failed to create max77693 muic attribute group\n");
+		goto fail;
+	}
+
+#ifdef CONFIG_EXTCON
+	/* External connector */
+	info->edev = kzalloc(sizeof(struct extcon_dev), GFP_KERNEL);
+	if (!info->edev) {
+		pr_err("Failed to allocate memory for extcon device\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+	info->edev->name = DEV_NAME;
+	info->edev->supported_cable = extcon_cable_name;
+	ret = extcon_dev_register(info->edev, NULL);
+	if (ret) {
+		pr_err("Failed to register extcon device\n");
+		kfree(info->edev);
+		goto fail;
+	}
+#endif
+
+	if (info->muic_data->init_cb)
+		info->muic_data->init_cb();
+
+	mutex_init(&info->mutex);
+
+	/* Set ADC debounce time: 25ms */
+	max77693_muic_set_adcdbset(info, 2);
+
+	ret = max77693_muic_irq_init(info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to initialize MUIC irq:%d\n", ret);
+		goto fail;
+	}
+
+	/* init jig state */
+	max77693_update_jig_state(info);
+
+	/* initial cable detection */
+	INIT_DELAYED_WORK(&info->init_work, max77693_muic_init_detect);
+	schedule_delayed_work(&info->init_work, msecs_to_jiffies(3000));
+
+	INIT_DELAYED_WORK(&info->usb_work, max77693_muic_usb_detect);
+	schedule_delayed_work(&info->usb_work, msecs_to_jiffies(17000));
+
+	INIT_DELAYED_WORK(&info->mhl_work, max77693_muic_mhl_detect);
+	schedule_delayed_work(&info->mhl_work, msecs_to_jiffies(25000));
+
+	return 0;
+
+ fail:
+	if (info->irq_adc)
+		free_irq(info->irq_adc, NULL);
+	if (info->irq_chgtype)
+		free_irq(info->irq_chgtype, NULL);
+	if (info->irq_vbvolt)
+		free_irq(info->irq_vbvolt, NULL);
+	if (info->irq_adc1k)
+		free_irq(info->irq_adc1k, NULL);
+	mutex_destroy(&info->mutex);
+ err_input:
+	platform_set_drvdata(pdev, NULL);
+	input_free_device(input);
+ err_kfree:
+	kfree(info);
+ err_return:
+	return ret;
+}
+
+static int __devexit max77693_muic_remove(struct platform_device *pdev)
+{
+	struct max77693_muic_info *info = platform_get_drvdata(pdev);
+	sysfs_remove_group(&switch_dev->kobj, &max77693_muic_group);
+
+	if (info) {
+		dev_info(info->dev, "func:%s\n", __func__);
+		input_unregister_device(info->input);
+		cancel_delayed_work(&info->init_work);
+		cancel_delayed_work(&info->usb_work);
+		cancel_delayed_work(&info->mhl_work);
+		free_irq(info->irq_adc, info);
+		free_irq(info->irq_chgtype, info);
+		free_irq(info->irq_vbvolt, info);
+		free_irq(info->irq_adc1k, info);
+#ifndef CONFIG_TARGET_LOCALE_NA
+		gpio_free(info->muic_data->gpio_usb_sel);
+#endif /* CONFIG_TARGET_LOCALE_NA */
+		mutex_destroy(&info->mutex);
+		kfree(info);
+	}
+	return 0;
+}
+
+void max77693_muic_shutdown(struct device *dev)
+{
+	struct max77693_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+	dev_info(info->dev, "func:%s\n", __func__);
+	if (!info->muic) {
+		dev_err(info->dev, "%s: no muic i2c client\n", __func__);
+		return;
+	}
+
+	dev_info(info->dev, "%s: JIGSet: auto detection\n", __func__);
+	val = (0 << CTRL3_JIGSET_SHIFT) | (0 << CTRL3_BOOTSET_SHIFT);
+
+	ret = max77693_update_reg(info->muic, MAX77693_MUIC_REG_CTRL3, val,
+			CTRL3_JIGSET_MASK | CTRL3_BOOTSET_MASK);
+	if (ret < 0) {
+		dev_err(info->dev, "%s: fail to update reg\n", __func__);
+		return;
+	}
+}
+
+static struct platform_driver max77693_muic_driver = {
+	.driver		= {
+		.name	= DEV_NAME,
+		.owner	= THIS_MODULE,
+		.shutdown = max77693_muic_shutdown,
+	},
+	.probe		= max77693_muic_probe,
+	.remove		= __devexit_p(max77693_muic_remove),
+};
+
+static int __init max77693_muic_init(void)
+{
+	pr_info("func:%s\n", __func__);
+	return platform_driver_register(&max77693_muic_driver);
+}
+module_init(max77693_muic_init);
+
+static void __exit max77693_muic_exit(void)
+{
+	pr_info("func:%s\n", __func__);
+	platform_driver_unregister(&max77693_muic_driver);
+}
+module_exit(max77693_muic_exit);
+
+
+MODULE_DESCRIPTION("Maxim MAX77693 MUIC driver");
+MODULE_AUTHOR("<sukdong.kim@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/max8997-muic.c b/drivers/misc/max8997-muic.c
new file mode 100644
index 0000000..3321545
--- /dev/null
+++ b/drivers/misc/max8997-muic.c
@@ -0,0 +1,1834 @@
+/*
+ * max8997-muic.c - MUIC driver for the Maxim 8997
+ *
+ *  Copyright (C) 2010 Samsung Electronics
+ *  <ms925.kim@samsung.com>
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/input.h>
+#include <linux/mfd/max8997.h>
+#include <linux/mfd/max8997-private.h>
+#include <plat/udc-hs.h>
+#ifdef CONFIG_USBHUB_USB3803
+#include <linux/usb3803.h>
+#endif
+
+
+/* MAX8997 STATUS1 register */
+#define STATUS1_ADC_SHIFT		0
+#define STATUS1_ADCLOW_SHIFT		5
+#define STATUS1_ADCERR_SHIFT		6
+#define STATUS1_ADC_MASK		(0x1f << STATUS1_ADC_SHIFT)
+#define STATUS1_ADCLOW_MASK		(0x1 << STATUS1_ADCLOW_SHIFT)
+#define STATUS1_ADCERR_MASK		(0x1 << STATUS1_ADCERR_SHIFT)
+
+/* MAX8997 STATUS2 register */
+#define STATUS2_CHGTYP_SHIFT		0
+#define STATUS2_CHGDETRUN_SHIFT		3
+#define STATUS2_VBVOLT_SHIFT		6
+#define STATUS2_CHGTYP_MASK		(0x7 << STATUS2_CHGTYP_SHIFT)
+#define STATUS2_CHGDETRUN_MASK		(0x1 << STATUS2_CHGDETRUN_SHIFT)
+#define STATUS2_VBVOLT_MASK		(0x1 << STATUS2_VBVOLT_SHIFT)
+
+/* MAX8997 CDETCTRL register */
+#define CHGDETEN_SHIFT			0
+#define CHGTYPM_SHIFT			1
+#define CHGDETEN_MASK			(0x1 << CHGDETEN_SHIFT)
+#define CHGTYPM_MASK			(0x1 << CHGTYPM_SHIFT)
+
+/* MAX8997 CONTROL1 register */
+#define COMN1SW_SHIFT			0
+#define COMP2SW_SHIFT			3
+#define MICEN_SHIFT			6
+#define COMN1SW_MASK			(0x7 << COMN1SW_SHIFT)
+#define COMP2SW_MASK			(0x7 << COMP2SW_SHIFT)
+#define MICEN_MASK			(0x1 << MICEN_SHIFT)
+
+/* MAX8997 CONTROL2 register */
+#define CTRL2_ACCDET_SHIFT		5
+#define CTRL2_ACCDET_MASK		(0x1 << CTRL2_ACCDET_SHIFT)
+
+/* MAX8997 CONTROL3 register */
+#define CTRL3_JIGSET_SHIFT		0
+#define CTRL3_BOOTSET_SHIFT		2
+#define CTRL3_ADCDBSET_SHIFT		4
+#define CTRL3_JIGSET_MASK		(0x3 << CTRL3_JIGSET_SHIFT)
+#define CTRL3_BOOTSET_MASK		(0x3 << CTRL3_BOOTSET_SHIFT)
+#define CTRL3_ADCDBSET_MASK		(0x3 << CTRL3_ADCDBSET_SHIFT)
+
+/* Interrupt 1 */
+#define INT_DETACH		(0x1 << 1)
+#define INT_ATTACH		(0x1 << 0)
+
+/* MAX8997 MUIC CHG_TYP setting values */
+enum {
+	/* No Valid voltage at VB (Vvb < Vvbdet) */
+	CHGTYP_NO_VOLTAGE	= 0x00,
+	/* Unknown (D+/D- does not present a valid USB charger signature) */
+	CHGTYP_USB		= 0x01,
+	/* Charging Downstream Port */
+	CHGTYP_DOWNSTREAM_PORT	= 0x02,
+	/* Dedicated Charger (D+/D- shorted) */
+	CHGTYP_DEDICATED_CHGR	= 0x03,
+	/* Special 500mA charger, max current 500mA */
+	CHGTYP_500MA		= 0x04,
+	/* Special 1A charger, max current 1A */
+	CHGTYP_1A		= 0x05,
+	/* Reserved for Future Use */
+	CHGTYP_RFU		= 0x06,
+	/* Dead Battery Charging, max current 100mA */
+	CHGTYP_DB_100MA		= 0x07,
+	CHGTYP_MAX,
+
+	CHGTYP_INIT,
+	CHGTYP_MIN = CHGTYP_NO_VOLTAGE
+};
+
+enum {
+	ADC_GND			= 0x00,
+	ADC_MHL			= 0x01,
+	ADC_DOCK_PREV_KEY	= 0x04,
+	ADC_DOCK_NEXT_KEY	= 0x07,
+	ADC_DOCK_VOL_DN		= 0x0a, /* 0x01010 14.46K ohm */
+	ADC_DOCK_VOL_UP		= 0x0b, /* 0x01011 17.26K ohm */
+	ADC_DOCK_PLAY_PAUSE_KEY = 0x0d,
+	ADC_CEA936ATYPE1_CHG	= 0x17,	/* 0x10111 200K ohm */
+	ADC_JIG_USB_OFF		= 0x18, /* 0x11000 255K ohm */
+	ADC_JIG_USB_ON		= 0x19, /* 0x11001 301K ohm */
+	ADC_DESKDOCK		= 0x1a, /* 0x11010 365K ohm */
+	ADC_CEA936ATYPE2_CHG	= 0x1b, /* 0x11011 442K ohm */
+	ADC_JIG_UART_OFF	= 0x1c, /* 0x11100 523K ohm */
+	ADC_JIG_UART_ON		= 0x1d, /* 0x11101 619K ohm */
+	ADC_CARDOCK		= 0x1d, /* 0x11101 619K ohm */
+	ADC_OPEN		= 0x1f
+};
+
+enum {
+	DOCK_KEY_NONE			= 0,
+	DOCK_KEY_VOL_UP_PRESSED,
+	DOCK_KEY_VOL_UP_RELEASED,
+	DOCK_KEY_VOL_DOWN_PRESSED,
+	DOCK_KEY_VOL_DOWN_RELEASED,
+	DOCK_KEY_PREV_PRESSED,
+	DOCK_KEY_PREV_RELEASED,
+	DOCK_KEY_PLAY_PAUSE_PRESSED,
+	DOCK_KEY_PLAY_PAUSE_RELEASED,
+	DOCK_KEY_NEXT_PRESSED,
+	DOCK_KEY_NEXT_RELEASED,
+};
+
+struct max8997_muic_info {
+	struct device		*dev;
+	struct max8997_dev	*max8997;
+	struct i2c_client	*muic;
+	struct max8997_muic_data *muic_data;
+	int			irq_adc;
+	int			irq_chgtype;
+	int			irq_vbvolt;
+	int			irq_adcerr;
+	int			mansw;
+
+	enum cable_type		cable_type;
+	struct delayed_work	init_work;
+	struct delayed_work	usb_work;
+	struct delayed_work	mhl_work;
+	struct mutex		mutex;
+#if defined(CONFIG_MUIC_MAX8997_OVPUI)
+	int			irq_chgins;
+	int			irq_chgrm;
+	bool			is_ovp_state;
+#endif
+
+	bool			is_usb_ready;
+	bool			is_mhl_ready;
+
+	struct input_dev	*input;
+	int			previous_key;
+};
+
+#if 0
+static void max8997_muic_dump_regs(struct max8997_muic_info *info)
+{
+	int i, ret;
+	u8 val;
+
+	for (i = 0; i < MAX8997_MUIC_REG_END; i++) {
+		ret = max8997_read_reg(info->muic, i, &val);
+		if (ret) {
+			dev_err(info->dev, "%s: fail to read reg(0x%x)\n",
+					__func__, i);
+			continue;
+		}
+		dev_info(info->dev, "%s: ADDR : 0x%02x, DATA : 0x%02x\n",
+				__func__, i, val);
+	}
+}
+#endif
+
+static ssize_t max8997_muic_show_usb_state(struct device *dev,
+				   struct device_attribute *attr,
+				   char *buf)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+
+	switch (info->cable_type) {
+	case CABLE_TYPE_USB:
+	case CABLE_TYPE_JIG_USB_OFF:
+	case CABLE_TYPE_JIG_USB_ON:
+		return sprintf(buf, "USB_STATE_CONFIGURED\n");
+	default:
+		break;
+	}
+
+	return sprintf(buf, "USB_STATE_NOTCONFIGURED\n");
+}
+
+static ssize_t max8997_muic_show_device(struct device *dev,
+				   struct device_attribute *attr,
+				   char *buf)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+
+	switch (info->cable_type) {
+	case CABLE_TYPE_NONE:
+		return sprintf(buf, "No cable\n");
+	case CABLE_TYPE_USB:
+		return sprintf(buf, "USB\n");
+	case CABLE_TYPE_OTG:
+		return sprintf(buf, "OTG\n");
+	case CABLE_TYPE_TA:
+		return sprintf(buf, "TA\n");
+	case CABLE_TYPE_DESKDOCK:
+		return sprintf(buf, "Desk Dock\n");
+	case CABLE_TYPE_CARDOCK:
+		return sprintf(buf, "Car Dock\n");
+	case CABLE_TYPE_JIG_UART_OFF:
+		return sprintf(buf, "JIG UART OFF\n");
+	case CABLE_TYPE_JIG_UART_OFF_VB:
+		return sprintf(buf, "JIG UART OFF/VB\n");
+	case CABLE_TYPE_JIG_UART_ON:
+		return sprintf(buf, "JIG UART ON\n");
+	case CABLE_TYPE_JIG_USB_OFF:
+		return sprintf(buf, "JIG USB OFF\n");
+	case CABLE_TYPE_JIG_USB_ON:
+		return sprintf(buf, "JIG USB ON\n");
+	case CABLE_TYPE_MHL:
+		return sprintf(buf, "mHL\n");
+	case CABLE_TYPE_MHL_VB:
+		return sprintf(buf, "mHL charging\n");
+	default:
+		break;
+	}
+
+	return sprintf(buf, "UNKNOWN\n");
+}
+
+static ssize_t max8997_muic_show_manualsw(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+
+	switch (info->muic_data->sw_path) {
+	case AP_USB_MODE:
+		return sprintf(buf, "PDA\n");
+	case CP_USB_MODE:
+		return sprintf(buf, "MODEM\n");
+	case AUDIO_MODE:
+		return sprintf(buf, "Audio\n");
+	default:
+		break;
+	}
+
+	return sprintf(buf, "UNKNOWN\n");
+}
+
+static ssize_t max8997_muic_set_manualsw(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+
+	if (!strncasecmp(buf, "PDA", 3)) {
+		info->muic_data->sw_path = AP_USB_MODE;
+		dev_info(info->dev, "%s: AP_USB_MODE\n", __func__);
+	} else if (!strncasecmp(buf, "MODEM", 5)) {
+		info->muic_data->sw_path = CP_USB_MODE;
+		dev_info(info->dev, "%s: CP_USB_MODE\n", __func__);
+	} else
+		dev_warn(info->dev, "%s: Wrong command\n", __func__);
+
+	return count;
+}
+
+static ssize_t max8997_muic_show_adc(struct device *dev,
+				   struct device_attribute *attr,
+				   char *buf)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+
+	ret = max8997_read_reg(info->muic, MAX8997_MUIC_REG_STATUS1, &val);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg\n", __func__);
+		return sprintf(buf, "UNKNOWN\n");
+	}
+
+	return sprintf(buf, "%x\n", (val & STATUS1_ADC_MASK));
+}
+
+static ssize_t max8997_muic_show_audio_path(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+
+	ret = max8997_read_reg(info->muic, MAX8997_MUIC_REG_CTRL1, &val);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg\n", __func__);
+		return sprintf(buf, "UNKNOWN\n");
+	}
+
+	return sprintf(buf, "%x\n", val);
+}
+
+static ssize_t max8997_muic_set_audio_path(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	struct i2c_client *client = info->muic;
+	u8 cntl1_val, cntl1_msk;
+	u8 val;
+
+	if (!strncmp(buf, "0", 1))
+		val = 0;
+	else if (!strncmp(buf, "1", 1))
+		val = 2;
+	else {
+		dev_warn(info->dev, "%s: Wrong command\n", __func__);
+		return count;
+	}
+
+	cntl1_val = (val << COMN1SW_SHIFT) | (val << COMP2SW_SHIFT) |
+			    (0 << MICEN_SHIFT);
+	cntl1_msk = COMN1SW_MASK | COMP2SW_MASK | MICEN_MASK;
+
+	max8997_update_reg(client, MAX8997_MUIC_REG_CTRL1, cntl1_val,
+				   cntl1_msk);
+
+	cntl1_val = 0;
+	max8997_read_reg(client, MAX8997_MUIC_REG_CTRL1, &cntl1_val);
+	dev_info(info->dev, "%s: CNTL1(0x%02x)\n", __func__, cntl1_val);
+
+	return count;
+}
+
+static ssize_t max8997_muic_show_otg_test(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+
+	ret = max8997_read_reg(info->muic, MAX8997_MUIC_REG_CDETCTRL, &val);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg\n", __func__);
+		return sprintf(buf, "UNKNOWN\n");
+	}
+	val &= CHGDETEN_MASK;
+
+	return sprintf(buf, "%x\n", val);
+}
+
+static ssize_t max8997_muic_set_otg_test(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	struct i2c_client *client = info->muic;
+	u8 val;
+
+	if (!strncmp(buf, "0", 1))
+		val = 0;
+	else if (!strncmp(buf, "1", 1))
+		val = 1;
+	else {
+		dev_warn(info->dev, "%s: Wrong command\n", __func__);
+		return count;
+	}
+
+	max8997_update_reg(client, MAX8997_MUIC_REG_CDETCTRL,
+			val << CHGDETEN_SHIFT, CHGDETEN_MASK);
+
+	val = 0;
+	max8997_read_reg(client, MAX8997_MUIC_REG_CDETCTRL, &val);
+	dev_info(info->dev, "%s: CDETCTRL(0x%02x)\n", __func__, val);
+
+	return count;
+}
+
+static void max8997_muic_set_adcdbset(struct max8997_muic_info *info,
+					int value)
+{
+	int ret;
+	u8 val;
+
+	if (value > 3) {
+		dev_err(info->dev, "%s: invalid value(%d)\n", __func__, value);
+		return;
+	}
+
+	if (!info->muic) {
+		dev_err(info->dev, "%s: no muic i2c client\n", __func__);
+		return;
+	}
+
+	val = value << CTRL3_ADCDBSET_SHIFT;
+	dev_info(info->dev, "%s: ADCDBSET(0x%02x)\n", __func__, val);
+
+	ret = max8997_update_reg(info->muic, MAX8997_MUIC_REG_CTRL3, val,
+			CTRL3_ADCDBSET_MASK);
+	if (ret < 0)
+		dev_err(info->dev, "%s: fail to update reg\n", __func__);
+}
+
+static ssize_t max8997_muic_show_adc_debounce_time(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+
+	if (!info->muic)
+		return sprintf(buf, "No I2C client\n");
+
+	ret = max8997_read_reg(info->muic, MAX8997_MUIC_REG_CTRL3, &val);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg\n", __func__);
+		return sprintf(buf, "UNKNOWN\n");
+	}
+	val &= CTRL3_ADCDBSET_MASK;
+	val = val >> CTRL3_ADCDBSET_SHIFT;
+
+	return sprintf(buf, "%x\n", val);
+}
+
+static ssize_t max8997_muic_set_adc_debounce_time(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	int value;
+
+	sscanf(buf, "%d", &value);
+
+	value = (value & 0x3);
+
+#if 0
+	max8997_muic_set_adcdbset(info, value);
+#else
+	dev_info(info->dev, "%s: Do nothing\n", __func__);
+#endif
+
+	return count;
+}
+
+static ssize_t max8997_muic_show_status(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	u8 devid, int_value[3], status[3], intmask[3], ctrl[3], cdetctrl;
+
+	max8997_read_reg(info->muic, MAX8997_MUIC_REG_ID, &devid);
+	max8997_bulk_read(info->muic, MAX8997_MUIC_REG_INT1, 3, int_value);
+	max8997_bulk_read(info->muic, MAX8997_MUIC_REG_STATUS1, 3, status);
+	max8997_bulk_read(info->muic, MAX8997_MUIC_REG_INTMASK1, 3, intmask);
+	max8997_bulk_read(info->muic, MAX8997_MUIC_REG_CTRL1, 3, ctrl);
+	max8997_read_reg(info->muic, MAX8997_MUIC_REG_CDETCTRL, &cdetctrl);
+
+	return sprintf(buf,
+		       "Device ID(0x%02x)\n"
+			   "INT1(0x%02x), INT2(0x%02x), INT3(0x%02x)\n"
+		       "STATUS1(0x%02x), STATUS2(0x%02x), STATUS3(0x%02x)\n"
+		       "INTMASK1(0x%02x), INTMASK2(0x%02x), INTMASK3(0x%02x)\n"
+		       "CTRL1(0x%02x), CTRL2(0x%02x), CTRL3(0x%02x)\n"
+		       "CDETCTRL(0x%02x)\n",
+		       devid, int_value[0], int_value[1], int_value[2],
+		       status[0], status[1], status[2],
+		       intmask[0], intmask[1], intmask[2],
+		       ctrl[0], ctrl[1], ctrl[2],
+		       cdetctrl);
+}
+
+static ssize_t max8997_muic_set_status(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	u8  reg_value[3];
+	char reg_name[5];
+
+	if (sscanf(buf, "%4s:%02x,%02x,%02x" , reg_name, (u32 *)&reg_value[0],
+		(u32 *)&reg_value[1], (u32 *)&reg_value[2])) {
+		if (!strncmp(reg_name, "INTM", 4)) {
+			dev_info(dev, "Manual Set INTMASK to 0x%02x, 0x%02x, 0x%02x\n",
+				reg_value[0], reg_value[1], reg_value[2]);
+			max8997_bulk_write(info->muic,
+				MAX8997_MUIC_REG_INTMASK1, 3, reg_value);
+		} else if (!strncmp(reg_name, "CONT", 4)) {
+			dev_info(dev, "Manual Set CONTROL to 0x%02x, 0x%02x, 0x%02x\n",
+				reg_value[0], reg_value[1], reg_value[2]);
+			max8997_bulk_write(info->muic,
+				MAX8997_MUIC_REG_CTRL1, 3, reg_value);
+		} else if (!strncmp(reg_name, "CDET", 4)) {
+			dev_info(dev, "Manual Set CDETCTRL to 0x%02x\n",
+				reg_value[0]);
+			max8997_write_reg(info->muic,
+				MAX8997_MUIC_REG_CDETCTRL, reg_value[0]);
+		} else {
+			dev_info(dev, "Unsupported CMD format\n");
+		}
+	} else {
+		dev_info(dev, "Read CMD fail\n");
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(usb_state, S_IRUGO, max8997_muic_show_usb_state, NULL);
+static DEVICE_ATTR(device, S_IRUGO, max8997_muic_show_device, NULL);
+static DEVICE_ATTR(usb_sel, 0664,
+		max8997_muic_show_manualsw, max8997_muic_set_manualsw);
+static DEVICE_ATTR(adc, S_IRUGO, max8997_muic_show_adc, NULL);
+static DEVICE_ATTR(audio_path, 0664,
+		max8997_muic_show_audio_path, max8997_muic_set_audio_path);
+static DEVICE_ATTR(otg_test, 0664,
+		max8997_muic_show_otg_test, max8997_muic_set_otg_test);
+static DEVICE_ATTR(adc_debounce_time, 0664,
+		max8997_muic_show_adc_debounce_time,
+		max8997_muic_set_adc_debounce_time);
+static DEVICE_ATTR(status, 0664,
+			max8997_muic_show_status, max8997_muic_set_status);
+
+static struct attribute *max8997_muic_attributes[] = {
+	&dev_attr_usb_state.attr,
+	&dev_attr_device.attr,
+	&dev_attr_usb_sel.attr,
+	&dev_attr_adc.attr,
+	&dev_attr_audio_path.attr,
+	&dev_attr_otg_test.attr,
+	&dev_attr_adc_debounce_time.attr,
+	&dev_attr_status.attr,
+	NULL
+};
+
+static const struct attribute_group max8997_muic_group = {
+	.attrs = max8997_muic_attributes,
+};
+
+static int max8997_muic_set_usb_path(struct max8997_muic_info *info, int path)
+{
+	struct i2c_client *client = info->muic;
+	struct max8997_muic_data *mdata = info->muic_data;
+	int ret;
+	int gpio_val;
+	u8 accdet, cntl1_val, cntl1_msk = 0, cntl2_val;
+
+	if (mdata->set_safeout) {
+		ret = mdata->set_safeout(path);
+		if (ret) {
+			dev_err(info->dev, "%s: fail to set safout!\n",
+					__func__);
+			return ret;
+		}
+	}
+
+	switch (path) {
+	case AP_USB_MODE:
+		dev_info(info->dev, "%s: AP_USB_MODE\n", __func__);
+		gpio_val = 0;
+		accdet = 1;
+
+		if (info->cable_type == CABLE_TYPE_OTG) {
+			accdet = 0;
+			/* DN1, DP2 */
+			cntl1_val = (1 << COMN1SW_SHIFT) | (1 << COMP2SW_SHIFT);
+			cntl1_msk = COMN1SW_MASK | COMP2SW_MASK;
+		}
+		break;
+	case CP_USB_MODE:
+		dev_info(info->dev, "%s: CP_USB_MODE\n", __func__);
+		gpio_val = 1;
+		accdet = 0;
+		/* UT1, UR2 */
+		cntl1_val = (3 << COMN1SW_SHIFT) | (3 << COMP2SW_SHIFT);
+		cntl1_msk = COMN1SW_MASK | COMP2SW_MASK;
+		break;
+	case AUDIO_MODE:
+		dev_info(info->dev, "%s: AUDIO_MODE\n", __func__);
+		gpio_val = 0;
+		accdet = 0;
+		/* SL1, SR2 */
+		cntl1_val = (2 << COMN1SW_SHIFT) | (2 << COMP2SW_SHIFT) |
+			    (0 << MICEN_SHIFT);
+		cntl1_msk = COMN1SW_MASK | COMP2SW_MASK | MICEN_MASK;
+		break;
+	default:
+		dev_warn(info->dev, "%s: invalid path(%d)\n", __func__, path);
+		return -EINVAL;
+	}
+
+#if !defined(CONFIG_TARGET_LOCALE_NA) && !defined(CONFIG_MACH_U1CAMERA_BD)
+	if (gpio_is_valid(info->muic_data->gpio_usb_sel))
+		gpio_direction_output(mdata->gpio_usb_sel, gpio_val);
+#endif /* !CONFIG_TARGET_LOCALE_NA && !CONFIG_MACH_U1CAMERA_BD */
+	/* Enable/Disable Factory Accessory Detection State Machine */
+	cntl2_val = accdet << CTRL2_ACCDET_SHIFT;
+	max8997_update_reg(client, MAX8997_MUIC_REG_CTRL2, cntl2_val,
+			CTRL2_ACCDET_MASK);
+
+	if (!accdet) {
+		dev_info(info->dev, "%s: Set manual path\n", __func__);
+		max8997_update_reg(client, MAX8997_MUIC_REG_CTRL1, cntl1_val,
+				   cntl1_msk);
+
+		cntl1_val = 0;
+		max8997_read_reg(client, MAX8997_MUIC_REG_CTRL1, &cntl1_val);
+		dev_info(info->dev, "%s: CNTL1(0x%02x)\n", __func__, cntl1_val);
+	}
+
+	return 0;
+}
+
+static int max8997_muic_set_charging_type(struct max8997_muic_info *info,
+					  bool force_disable)
+{
+	struct max8997_muic_data *mdata = info->muic_data;
+	int ret = 0;
+
+	if (mdata->charger_cb) {
+		if (force_disable)
+			ret = mdata->charger_cb(CABLE_TYPE_NONE);
+		else
+			ret = mdata->charger_cb(info->cable_type);
+	}
+
+	if (ret) {
+		dev_err(info->dev, "%s: error from charger_cb(%d)\n", __func__,
+				ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int max8997_muic_handle_dock_vol_key(struct max8997_muic_info *info,
+					u8 status1)
+{
+	struct input_dev *input = info->input;
+	int pre_key = info->previous_key;
+	unsigned int code;
+	int state;
+	u8 adc;
+
+	adc = status1 & STATUS1_ADC_MASK;
+
+	if (info->cable_type != CABLE_TYPE_DESKDOCK)
+		return 0;
+
+	if (adc == ADC_OPEN) {
+		switch (pre_key) {
+		case DOCK_KEY_VOL_UP_PRESSED:
+			code = KEY_VOLUMEUP;
+			state = 0;
+			info->previous_key = DOCK_KEY_VOL_UP_RELEASED;
+			break;
+		case DOCK_KEY_VOL_DOWN_PRESSED:
+			code = KEY_VOLUMEDOWN;
+			state = 0;
+			info->previous_key = DOCK_KEY_VOL_DOWN_RELEASED;
+			break;
+		case DOCK_KEY_PREV_PRESSED:
+			code = KEY_PREVIOUSSONG;
+			state = 0;
+			info->previous_key = DOCK_KEY_PREV_RELEASED;
+			break;
+		case DOCK_KEY_PLAY_PAUSE_PRESSED:
+			code = KEY_PLAYPAUSE;
+			state = 0;
+			info->previous_key = DOCK_KEY_PLAY_PAUSE_RELEASED;
+			break;
+		case DOCK_KEY_NEXT_PRESSED:
+			code = KEY_NEXTSONG;
+			state = 0;
+			info->previous_key = DOCK_KEY_NEXT_RELEASED;
+			break;
+		default:
+			return 0;
+		}
+		input_event(input, EV_KEY, code, state);
+		input_sync(input);
+		return 0;
+	}
+
+	if (pre_key == DOCK_KEY_NONE) {
+		/*
+		if (adc != ADC_DOCK_VOL_UP && adc != ADC_DOCK_VOL_DN && \
+		adc != ADC_DOCK_PREV_KEY && adc != ADC_DOCK_PLAY_PAUSE_KEY \
+		&& adc != ADC_DOCK_NEXT_KEY)
+		*/
+		if ((adc < 0x03) || (adc > 0x0d))
+			return 0;
+	}
+
+	dev_info(info->dev, "%s: dock vol key(%d)\n", __func__, pre_key);
+
+	switch (adc) {
+	case ADC_DOCK_VOL_UP:
+		code = KEY_VOLUMEUP;
+		state = 1;
+		info->previous_key = DOCK_KEY_VOL_UP_PRESSED;
+		break;
+	case ADC_DOCK_VOL_DN:
+		code = KEY_VOLUMEDOWN;
+		state = 1;
+		info->previous_key = DOCK_KEY_VOL_DOWN_PRESSED;
+		break;
+	case ADC_DOCK_PREV_KEY-1 ... ADC_DOCK_PREV_KEY+1:
+		code = KEY_PREVIOUSSONG;
+		state = 1;
+		info->previous_key = DOCK_KEY_PREV_PRESSED;
+		break;
+	case ADC_DOCK_PLAY_PAUSE_KEY-1 ... ADC_DOCK_PLAY_PAUSE_KEY+1:
+		code = KEY_PLAYPAUSE;
+		state = 1;
+		info->previous_key = DOCK_KEY_PLAY_PAUSE_PRESSED;
+		break;
+	case ADC_DOCK_NEXT_KEY-1 ... ADC_DOCK_NEXT_KEY+1:
+		code = KEY_NEXTSONG;
+		state = 1;
+		info->previous_key = DOCK_KEY_NEXT_PRESSED;
+		break;
+	case ADC_DESKDOCK: /* key release routine */
+		if (pre_key == DOCK_KEY_VOL_UP_PRESSED) {
+			code = KEY_VOLUMEUP;
+			state = 0;
+			info->previous_key = DOCK_KEY_VOL_UP_RELEASED;
+		} else if (pre_key == DOCK_KEY_VOL_DOWN_PRESSED) {
+			code = KEY_VOLUMEDOWN;
+			state = 0;
+			info->previous_key = DOCK_KEY_VOL_DOWN_RELEASED;
+		} else if (pre_key == DOCK_KEY_PREV_PRESSED) {
+			code = KEY_PREVIOUSSONG;
+			state = 0;
+			info->previous_key = DOCK_KEY_PREV_RELEASED;
+		} else if (pre_key == DOCK_KEY_PLAY_PAUSE_PRESSED) {
+			code = KEY_PLAYPAUSE;
+			state = 0;
+			info->previous_key = DOCK_KEY_PLAY_PAUSE_RELEASED;
+		} else if (pre_key == DOCK_KEY_NEXT_PRESSED) {
+			code = KEY_NEXTSONG;
+			state = 0;
+			info->previous_key = DOCK_KEY_NEXT_RELEASED;
+		} else {
+			dev_warn(info->dev, "%s:%d should not reach here\n",
+					__func__, __LINE__);
+			return 0;
+		}
+		break;
+	default:
+		dev_warn(info->dev, "%s: unsupported ADC(0x%02x)\n", __func__,
+				adc);
+		return 0;
+	}
+
+	input_event(input, EV_KEY, code, state);
+	input_sync(input);
+
+	return 1;
+}
+
+static int max8997_muic_attach_usb_type(struct max8997_muic_info *info, int adc)
+{
+	struct max8997_muic_data *mdata = info->muic_data;
+	int ret, path;
+
+	if (info->cable_type == CABLE_TYPE_MHL ||
+			info->cable_type == CABLE_TYPE_MHL_VB) {
+		dev_warn(info->dev, "%s: mHL was attached!\n", __func__);
+		return 0;
+	}
+
+	switch (adc) {
+	case ADC_JIG_USB_OFF:
+		if (info->cable_type == CABLE_TYPE_JIG_USB_OFF) {
+			dev_info(info->dev, "%s: duplicated(JIG USB OFF)\n",
+					__func__);
+			return 0;
+		}
+
+		dev_info(info->dev, "%s:JIG USB BOOTOFF\n", __func__);
+		info->cable_type = CABLE_TYPE_JIG_USB_OFF;
+		path = AP_USB_MODE;
+		break;
+	case ADC_JIG_USB_ON:
+		if (info->cable_type == CABLE_TYPE_JIG_USB_ON) {
+			dev_info(info->dev, "%s: duplicated(JIG USB ON)\n",
+					__func__);
+			return 0;
+		}
+
+		dev_info(info->dev, "%s:JIG USB BOOTON\n", __func__);
+		info->cable_type = CABLE_TYPE_JIG_USB_ON;
+		path = AP_USB_MODE;
+		break;
+	case ADC_OPEN:
+		if (info->cable_type == CABLE_TYPE_USB) {
+			dev_info(info->dev, "%s: duplicated(USB)\n", __func__);
+			return 0;
+		}
+
+		dev_info(info->dev, "%s:USB\n", __func__);
+		info->cable_type = CABLE_TYPE_USB;
+		path = AP_USB_MODE;
+		break;
+	default:
+		dev_info(info->dev, "%s: Unkown cable(0x%x)\n", __func__, adc);
+		return 0;
+	}
+
+	ret = max8997_muic_set_charging_type(info, false);
+	if (ret) {
+		info->cable_type = CABLE_TYPE_NONE;
+		return ret;
+	}
+
+	if (mdata->sw_path == CP_USB_MODE) {
+		info->cable_type = CABLE_TYPE_USB;
+		max8997_muic_set_usb_path(info, CP_USB_MODE);
+		return 0;
+	}
+
+	max8997_muic_set_usb_path(info, path);
+
+	if ((path == AP_USB_MODE) && (adc == ADC_OPEN)) {
+		if (mdata->usb_cb && info->is_usb_ready)
+			mdata->usb_cb(USB_CABLE_ATTACHED);
+	}
+
+	return 0;
+}
+
+static int max8997_muic_attach_dock_type(struct max8997_muic_info *info,
+					 int adc)
+{
+	struct max8997_muic_data *mdata = info->muic_data;
+	int path;
+
+	switch (adc) {
+	case ADC_DESKDOCK:
+		/* Desk Dock */
+		if (info->cable_type == CABLE_TYPE_DESKDOCK) {
+			dev_info(info->dev, "%s: duplicated(DeskDock)\n",
+					__func__);
+			return 0;
+		}
+		dev_info(info->dev, "%s:DeskDock\n", __func__);
+		info->cable_type = CABLE_TYPE_DESKDOCK;
+		path = AUDIO_MODE;
+
+		if (mdata->deskdock_cb)
+			mdata->deskdock_cb(MAX8997_MUIC_ATTACHED);
+		break;
+	case ADC_CARDOCK:
+		/* Car Dock */
+		if (info->cable_type == CABLE_TYPE_CARDOCK) {
+			dev_info(info->dev, "%s: duplicated(CarDock)\n",
+					__func__);
+			return 0;
+		}
+		dev_info(info->dev, "%s:CarDock\n", __func__);
+		info->cable_type = CABLE_TYPE_CARDOCK;
+		path = AUDIO_MODE;
+
+		if (mdata->cardock_cb)
+			mdata->cardock_cb(MAX8997_MUIC_ATTACHED);
+		break;
+	default:
+		dev_info(info->dev, "%s: should not reach here(0x%x)\n",
+				__func__, adc);
+		return 0;
+	}
+
+	max8997_muic_set_usb_path(info, path);
+
+	return 0;
+}
+
+static void max8997_muic_attach_mhl(struct max8997_muic_info *info, u8 chgtyp)
+{
+	struct max8997_muic_data *mdata = info->muic_data;
+
+	dev_info(info->dev, "%s\n", __func__);
+
+	if (info->cable_type == CABLE_TYPE_USB) {
+		if (mdata->usb_cb && info->is_usb_ready)
+			mdata->usb_cb(USB_CABLE_DETACHED);
+
+		max8997_muic_set_charging_type(info, true);
+	}
+#if 0
+	if (info->cable_type == CABLE_TYPE_MHL) {
+		dev_info(info->dev, "%s: duplicated(MHL)\n", __func__);
+		return;
+	}
+#endif
+	info->cable_type = CABLE_TYPE_MHL;
+
+	if (mdata->mhl_cb && info->is_mhl_ready)
+		mdata->mhl_cb(MAX8997_MUIC_ATTACHED);
+
+	if (chgtyp == CHGTYP_USB) {
+		info->cable_type = CABLE_TYPE_MHL_VB;
+		max8997_muic_set_charging_type(info, false);
+	}
+}
+
+/* TODO : should be removed */
+#define NOTIFY_TEST_MODE	3
+
+static void max8997_muic_handle_jig_uart(struct max8997_muic_info *info,
+					 u8 vbvolt)
+{
+	struct max8997_muic_data *mdata = info->muic_data;
+	enum cable_type prev_ct = info->cable_type;
+	bool is_otgtest = false;
+	u8 cntl1_val, cntl1_msk;
+
+	dev_info(info->dev, "%s: JIG UART/BOOTOFF(0x%x)\n", __func__, vbvolt);
+
+#if defined(CONFIG_SEC_MODEM_M0_TD)
+	gpio_set_value(GPIO_AP_CP_INT1, 1);
+#endif
+
+	/* UT1, UR2 */
+	cntl1_val = (3 << COMN1SW_SHIFT) | (3 << COMP2SW_SHIFT);
+	cntl1_msk = COMN1SW_MASK | COMP2SW_MASK;
+	max8997_update_reg(info->muic, MAX8997_MUIC_REG_CTRL1, cntl1_val,
+			   cntl1_msk);
+
+	if (vbvolt & STATUS2_VBVOLT_MASK) {
+		if (mdata->host_notify_cb) {
+			if (mdata->host_notify_cb(1) == NOTIFY_TEST_MODE) {
+				is_otgtest = true;
+				dev_info(info->dev, "%s: OTG TEST\n", __func__);
+			}
+		}
+
+		info->cable_type = CABLE_TYPE_JIG_UART_OFF_VB;
+		max8997_muic_set_charging_type(info, is_otgtest);
+
+	} else {
+		info->cable_type = CABLE_TYPE_JIG_UART_OFF;
+#if 0
+		if (mdata->uart_path == UART_PATH_CP &&
+				mdata->jig_uart_cb)
+			mdata->jig_uart_cb(UART_PATH_CP);
+#endif
+		if (prev_ct == CABLE_TYPE_JIG_UART_OFF_VB) {
+			max8997_muic_set_charging_type(info, false);
+
+			if (mdata->host_notify_cb)
+				mdata->host_notify_cb(0);
+		}
+	}
+}
+
+static int max8997_muic_handle_attach(struct max8997_muic_info *info,
+		u8 status1, u8 status2)
+{
+	struct max8997_muic_data *mdata = info->muic_data;
+	u8 adc, adclow, adcerr, chgtyp, vbvolt, chgdetrun;
+	int ret = 0;
+#ifdef CONFIG_USBHUB_USB3803
+	/* setting usb hub in Diagnostic(hub) mode */
+	usb3803_set_mode(USB_3803_MODE_HUB);
+#endif /* CONFIG_USBHUB_USB3803 */
+
+	adc = status1 & STATUS1_ADC_MASK;
+	adclow = status1 & STATUS1_ADCLOW_MASK;
+	adcerr = status1 & STATUS1_ADCERR_MASK;
+	chgtyp = status2 & STATUS2_CHGTYP_MASK;
+	vbvolt = status2 & STATUS2_VBVOLT_MASK;
+	chgdetrun = status2 & STATUS2_CHGDETRUN_MASK;
+
+	switch (info->cable_type) {
+	case CABLE_TYPE_JIG_UART_OFF:
+	case CABLE_TYPE_JIG_UART_OFF_VB:
+		/* Workaround for Factory mode.
+		 * Abandon adc interrupt of approximately +-100K range
+		 * if previous cable status was JIG UART BOOT OFF.
+		 */
+		if (adc == (ADC_JIG_UART_OFF + 1) ||
+				adc == (ADC_JIG_UART_OFF - 1)) {
+			dev_warn(info->dev, "%s: abandon ADC\n", __func__);
+			return 0;
+		}
+
+		if (adcerr) {
+			dev_warn(info->dev, "%s: current state is jig_uart_off,"
+				"just ignore\n", __func__);
+			return 0;
+		}
+
+		if (adc != ADC_JIG_UART_OFF) {
+			if (info->cable_type == CABLE_TYPE_JIG_UART_OFF_VB) {
+				dev_info(info->dev, "%s: adc != JIG_UART_OFF, remove JIG UART/OFF/VB\n", __func__);
+				info->cable_type = CABLE_TYPE_NONE;
+				max8997_muic_set_charging_type(info, false);
+			} else {
+				dev_info(info->dev, "%s: adc != JIG_UART_OFF, remove JIG UART/BOOTOFF\n", __func__);
+				info->cable_type = CABLE_TYPE_NONE;
+			}
+		}
+		break;
+
+	case CABLE_TYPE_DESKDOCK:
+		if (adcerr || (adc != ADC_DESKDOCK)) {
+			if (adcerr)
+				dev_err(info->dev, "%s: ADC err occured(DESKDOCK)\n", __func__);
+			else
+				dev_warn(info->dev, "%s: ADC != DESKDOCK, remove DESKDOCK\n", __func__);
+
+			info->cable_type = CABLE_TYPE_NONE;
+
+			max8997_muic_set_charging_type(info, false);
+
+			if (mdata->deskdock_cb)
+				mdata->deskdock_cb(MAX8997_MUIC_DETACHED);
+
+			if (adcerr)
+				return 0;
+		}
+		break;
+
+	case CABLE_TYPE_CARDOCK:
+		if (adcerr || (adc != ADC_CARDOCK)) {
+			if (adcerr)
+				dev_err(info->dev, "%s: ADC err occured(CARDOCK)\n", __func__);
+			else
+				dev_warn(info->dev, "%s: ADC != CARDOCK, remove CARDOCK\n", __func__);
+
+			info->cable_type = CABLE_TYPE_NONE;
+
+			max8997_muic_set_charging_type(info, false);
+
+			if (mdata->cardock_cb)
+				mdata->cardock_cb(MAX8997_MUIC_DETACHED);
+
+			if (adcerr)
+				return 0;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	/* 1Kohm ID regiter detection (mHL)
+	 * Old MUIC : ADC value:0x00 or 0x01, ADCLow:1
+	 * New MUIC : ADC value is not set(Open), ADCLow:1, ADCError:1
+	 */
+	if (adclow && adcerr) {
+		max8997_muic_attach_mhl(info, chgtyp);
+		return 0;
+	}
+
+	switch (adc) {
+	case ADC_GND:
+#if defined(CONFIG_MACH_U1)
+		/* This is for support old MUIC */
+		if (adclow) {
+			max8997_muic_attach_mhl(info, chgtyp);
+			break;
+		}
+#endif
+
+		if (chgtyp == CHGTYP_NO_VOLTAGE) {
+			if (info->cable_type == CABLE_TYPE_OTG) {
+				dev_info(info->dev,
+						"%s: duplicated(OTG)\n",
+						__func__);
+				break;
+			}
+
+			info->cable_type = CABLE_TYPE_OTG;
+			max8997_muic_set_usb_path(info, AP_USB_MODE);
+			if (mdata->usb_cb && info->is_usb_ready)
+				mdata->usb_cb(USB_OTGHOST_ATTACHED);
+		} else if (chgtyp == CHGTYP_USB ||
+				chgtyp == CHGTYP_DOWNSTREAM_PORT ||
+				chgtyp == CHGTYP_DEDICATED_CHGR ||
+				chgtyp == CHGTYP_500MA	||
+				chgtyp == CHGTYP_1A) {
+			dev_info(info->dev, "%s: OTG charging pump\n",
+					__func__);
+			ret = max8997_muic_set_charging_type(info, false);
+		}
+		break;
+	case ADC_MHL:
+#if defined(CONFIG_MACH_U1)
+		/* This is for support old MUIC */
+		max8997_muic_attach_mhl(info, chgtyp);
+#endif
+		break;
+	case ADC_JIG_UART_OFF:
+		max8997_muic_handle_jig_uart(info, vbvolt);
+		break;
+	case ADC_JIG_USB_OFF:
+	case ADC_JIG_USB_ON:
+		if (vbvolt & STATUS2_VBVOLT_MASK)
+			ret = max8997_muic_attach_usb_type(info, adc);
+		break;
+	case ADC_DESKDOCK:
+	case ADC_CARDOCK:
+		max8997_muic_attach_dock_type(info, adc);
+		if (chgtyp == CHGTYP_USB ||
+				chgtyp == CHGTYP_DOWNSTREAM_PORT ||
+				chgtyp == CHGTYP_DEDICATED_CHGR ||
+				chgtyp == CHGTYP_500MA	||
+				chgtyp == CHGTYP_1A)
+			ret = max8997_muic_set_charging_type(info, false);
+		else if (chgtyp == CHGTYP_NO_VOLTAGE && !chgdetrun)
+			ret = max8997_muic_set_charging_type(info, true);
+		break;
+	case ADC_CEA936ATYPE1_CHG:
+	case ADC_CEA936ATYPE2_CHG:
+	case ADC_OPEN:
+		switch (chgtyp) {
+		case CHGTYP_USB:
+			if (adc == ADC_CEA936ATYPE1_CHG
+					|| adc == ADC_CEA936ATYPE2_CHG)
+				break;
+			if (mdata->is_mhl_attached
+					&& mdata->is_mhl_attached() &&
+					info->cable_type == CABLE_TYPE_MHL) {
+				dev_info(info->dev, "%s: MHL(charging)\n",
+						__func__);
+				info->cable_type = CABLE_TYPE_MHL_VB;
+				ret = max8997_muic_set_charging_type(info,
+						false);
+				return ret;
+			}
+			ret = max8997_muic_attach_usb_type(info, adc);
+			break;
+		case CHGTYP_DOWNSTREAM_PORT:
+		case CHGTYP_DEDICATED_CHGR:
+		case CHGTYP_500MA:
+		case CHGTYP_1A:
+			dev_info(info->dev, "%s:TA\n", __func__);
+			info->cable_type = CABLE_TYPE_TA;
+#ifdef CONFIG_USBHUB_USB3803
+			/* setting usb hub in default mode (standby) */
+			usb3803_set_mode(USB_3803_MODE_STANDBY);
+#endif  /* CONFIG_USBHUB_USB3803 */
+			ret = max8997_muic_set_charging_type(info, false);
+			if (ret)
+				info->cable_type = CABLE_TYPE_NONE;
+			break;
+		default:
+			break;
+		}
+		break;
+	default:
+		dev_warn(info->dev, "%s: unsupported adc=0x%x\n", __func__,
+				adc);
+		break;
+	}
+	return ret;
+}
+
+static int max8997_muic_handle_detach(struct max8997_muic_info *info)
+{
+	struct i2c_client *client = info->muic;
+	struct max8997_muic_data *mdata = info->muic_data;
+	enum cable_type prev_ct = CABLE_TYPE_NONE;
+	int ret = 0;
+
+#if defined(CONFIG_SEC_MODEM_M0_TD)
+	gpio_set_value(GPIO_AP_CP_INT1, 0);
+#endif
+
+	/*
+	 * MAX8996/8997-MUIC bug:
+	 *
+	 * Auto-switching COMN/P is not restored automatically when detached and
+	 * remains undetermined state. UART(UT1, UR2) will be short (because TA
+	 * D+/D- is short) if charger(TA) insertion is followed right after the
+	 * JIG off. Reset CONTROL1 is needed when detaching cable.
+	 */
+	max8997_write_reg(client, MAX8997_MUIC_REG_CTRL1, 0x00);
+
+	if (info->cable_type == CABLE_TYPE_MHL) {
+
+		/* Enable Factory Accessory Detection State Machine */
+		max8997_update_reg(client, MAX8997_MUIC_REG_CTRL2,
+				(1 << CTRL2_ACCDET_SHIFT), CTRL2_ACCDET_MASK);
+	}
+
+#ifdef CONFIG_USBHUB_USB3803
+	/* setting usb hub in default mode (standby) */
+	usb3803_set_mode(USB_3803_MODE_STANDBY);
+#endif  /* CONFIG_USBHUB_USB3803 */
+	info->previous_key = DOCK_KEY_NONE;
+
+	if (info->cable_type == CABLE_TYPE_NONE) {
+		dev_info(info->dev, "%s: duplicated(NONE)\n", __func__);
+		return 0;
+	}
+#if 0
+	if (mdata->jig_uart_cb)
+		mdata->jig_uart_cb(UART_PATH_AP);
+#endif
+	if (mdata->is_mhl_attached && mdata->is_mhl_attached()
+			&& info->cable_type == CABLE_TYPE_MHL) {
+		dev_info(info->dev, "%s: MHL attached. Do Nothing\n",
+				__func__);
+		return 0;
+	}
+
+	switch (info->cable_type) {
+	case CABLE_TYPE_OTG:
+		dev_info(info->dev, "%s: OTG\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+
+		if (mdata->usb_cb && info->is_usb_ready)
+			mdata->usb_cb(USB_OTGHOST_DETACHED);
+		break;
+	case CABLE_TYPE_USB:
+	case CABLE_TYPE_JIG_USB_OFF:
+	case CABLE_TYPE_JIG_USB_ON:
+		dev_info(info->dev, "%s: USB(0x%x)\n", __func__,
+				info->cable_type);
+		prev_ct = info->cable_type;
+		info->cable_type = CABLE_TYPE_NONE;
+
+		ret = max8997_muic_set_charging_type(info, false);
+		if (ret) {
+			info->cable_type = prev_ct;
+			break;
+		}
+
+		if (mdata->sw_path == CP_USB_MODE)
+			return 0;
+
+		if (mdata->usb_cb && info->is_usb_ready)
+			mdata->usb_cb(USB_CABLE_DETACHED);
+		break;
+	case CABLE_TYPE_DESKDOCK:
+		dev_info(info->dev, "%s: DESKDOCK\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+
+		ret = max8997_muic_set_charging_type(info, false);
+		if (ret) {
+			info->cable_type = CABLE_TYPE_DESKDOCK;
+			break;
+		}
+
+		if (mdata->deskdock_cb)
+			mdata->deskdock_cb(MAX8997_MUIC_DETACHED);
+		break;
+	case CABLE_TYPE_CARDOCK:
+		dev_info(info->dev, "%s: CARDOCK\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+
+		ret = max8997_muic_set_charging_type(info, false);
+		if (ret) {
+			info->cable_type = CABLE_TYPE_CARDOCK;
+			break;
+		}
+
+		if (mdata->cardock_cb)
+			mdata->cardock_cb(MAX8997_MUIC_DETACHED);
+		break;
+	case CABLE_TYPE_TA:
+		dev_info(info->dev, "%s: TA\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+		ret = max8997_muic_set_charging_type(info, false);
+		if (ret)
+			info->cable_type = CABLE_TYPE_TA;
+		break;
+	case CABLE_TYPE_JIG_UART_ON:
+		dev_info(info->dev, "%s: JIG UART/BOOTON\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+		break;
+	case CABLE_TYPE_JIG_UART_OFF:
+		dev_info(info->dev, "%s: JIG UART/BOOTOFF\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+		break;
+	case CABLE_TYPE_JIG_UART_OFF_VB:
+		dev_info(info->dev, "%s: JIG UART/OFF/VB\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+		ret = max8997_muic_set_charging_type(info, false);
+		if (ret)
+			info->cable_type = CABLE_TYPE_JIG_UART_OFF_VB;
+		break;
+	case CABLE_TYPE_MHL:
+		dev_info(info->dev, "%s: MHL\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+		break;
+	case CABLE_TYPE_MHL_VB:
+		dev_info(info->dev, "%s: MHL VBUS\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+		max8997_muic_set_charging_type(info, false);
+
+		if (mdata->is_mhl_attached && mdata->is_mhl_attached()) {
+			if (mdata->mhl_cb && info->is_mhl_ready)
+				mdata->mhl_cb(MAX8997_MUIC_DETACHED);
+		}
+		break;
+	case CABLE_TYPE_UNKNOWN:
+		dev_info(info->dev, "%s: UNKNOWN\n", __func__);
+		info->cable_type = CABLE_TYPE_NONE;
+
+		ret = max8997_muic_set_charging_type(info, false);
+		if (ret)
+			info->cable_type = CABLE_TYPE_UNKNOWN;
+		break;
+	default:
+		dev_info(info->dev, "%s:invalid cable type %d\n",
+				__func__, info->cable_type);
+		break;
+	}
+	return ret;
+}
+
+static void max8997_muic_detect_dev(struct max8997_muic_info *info, int irq)
+{
+	struct i2c_client *client = info->muic;
+	u8 status[2];
+	u8 adc, chgtyp, adcerr;
+	int intr = INT_ATTACH;
+	int ret;
+
+	ret = max8997_bulk_read(client, MAX8997_MUIC_REG_STATUS1, 2, status);
+	if (ret) {
+		dev_err(info->dev, "%s: fail to read muic reg(%d)\n", __func__,
+				ret);
+		return;
+	}
+
+	dev_info(info->dev, "%s: STATUS1:0x%x, 2:0x%x\n", __func__,
+			status[0], status[1]);
+
+	if ((irq == info->irq_adc) &&
+			max8997_muic_handle_dock_vol_key(info, status[0]))
+		return;
+
+	adc = status[0] & STATUS1_ADC_MASK;
+	adcerr = status[0] & STATUS1_ADCERR_MASK;
+	chgtyp = status[1] & STATUS2_CHGTYP_MASK;
+
+	switch (adc) {
+	case ADC_MHL:
+#if defined(CONFIG_MACH_U1)
+		break;
+#endif
+	case (ADC_MHL + 1):
+	case (ADC_DOCK_VOL_DN - 1):
+	case (ADC_DOCK_PLAY_PAUSE_KEY + 2) ... (ADC_CEA936ATYPE1_CHG - 1):
+	case (ADC_CARDOCK + 1):
+		dev_warn(info->dev, "%s: unsupported ADC(0x%02x)\n", __func__, adc);
+		intr = INT_DETACH;
+		break;
+	case ADC_OPEN:
+		if (!adcerr) {
+			if (chgtyp == CHGTYP_NO_VOLTAGE)
+				intr = INT_DETACH;
+			else if (chgtyp == CHGTYP_USB ||
+					chgtyp == CHGTYP_DOWNSTREAM_PORT ||
+					chgtyp == CHGTYP_DEDICATED_CHGR ||
+					chgtyp == CHGTYP_500MA	||
+					chgtyp == CHGTYP_1A) {
+				if (info->cable_type == CABLE_TYPE_OTG ||
+					info->cable_type == CABLE_TYPE_DESKDOCK ||
+					info->cable_type == CABLE_TYPE_CARDOCK)
+					intr = INT_DETACH;
+			}
+		}
+		break;
+	default:
+		break;
+	}
+
+#if defined(CONFIG_MUIC_MAX8997_OVPUI)
+	if (intr == INT_ATTACH) {
+		if (irq == info->irq_chgins) {
+			if (info->is_ovp_state) {
+				max8997_muic_handle_attach(info, status[0],
+					status[1]);
+				info->is_ovp_state = false;
+				dev_info(info->dev, "OVP recovered\n");
+				return;
+			} else {
+				dev_info(info->dev, "Just inserted TA/USB\n");
+				return;
+			}
+		} else if (irq == info->irq_chgrm) {
+			max8997_muic_handle_detach(info);
+			info->is_ovp_state = true;
+			dev_info(info->dev, "OVP occured\n");
+			return;
+		}
+
+	} else	{
+		info->is_ovp_state = false;
+
+		if (irq == info->irq_chgrm) {
+			dev_info(info->dev, "Just removed TA/USB\n");
+			return;
+		}
+	}
+#endif
+
+	if (intr == INT_ATTACH) {
+		dev_info(info->dev, "%s: ATTACHED\n", __func__);
+		max8997_muic_handle_attach(info, status[0], status[1]);
+	} else {
+		dev_info(info->dev, "%s: DETACHED\n", __func__);
+		max8997_muic_handle_detach(info);
+	}
+	return;
+}
+
+static irqreturn_t max8997_muic_irq(int irq, void *data)
+{
+	struct max8997_muic_info *info = data;
+	dev_info(info->dev, "%s: irq:%d\n", __func__, irq);
+
+	mutex_lock(&info->mutex);
+	max8997_muic_detect_dev(info, irq);
+	mutex_unlock(&info->mutex);
+
+	return IRQ_HANDLED;
+}
+
+#define REQUEST_IRQ(_irq, _name)					\
+do {									\
+	ret = request_threaded_irq(_irq, NULL, max8997_muic_irq,	\
+				    0, _name, info);			\
+	if (ret < 0)							\
+		dev_err(info->dev, "Failed to request IRQ #%d: %d\n",	\
+			_irq, ret);					\
+} while (0)
+
+static int max8997_muic_irq_init(struct max8997_muic_info *info)
+{
+	int ret;
+#if 0
+#if !defined(CONFIG_MACH_U1_REV00)
+	dev_info(info->dev, "%s: system_rev=%d\n", __func__, system_rev);
+#if !defined(CONFIG_MACH_P6_REV02) && !defined(CONFIG_MACH_U1_C210) \
+		&& !defined(CONFIG_MACH_U1HD_C210)
+	if (system_rev < 0x3) {
+		dev_info(info->dev,
+			"Caution !!! This system_rev does not support ALL irq\n");
+		return 0;
+	}
+#endif
+#endif
+#endif
+
+	REQUEST_IRQ(info->irq_adc, "muic-adc");
+	REQUEST_IRQ(info->irq_chgtype, "muic-chgtype");
+	REQUEST_IRQ(info->irq_vbvolt, "muic-vbvolt");
+	REQUEST_IRQ(info->irq_adcerr, "muic-adcerr");
+#if defined(CONFIG_MUIC_MAX8997_OVPUI)
+	REQUEST_IRQ(info->irq_chgins, "chg-insert");
+	REQUEST_IRQ(info->irq_chgrm, "chg-remove");
+#endif
+	return 0;
+}
+
+#define CHECK_GPIO(_gpio, _name)					\
+do {									\
+	if (!_gpio) {							\
+		dev_err(&pdev->dev, _name " GPIO defined as 0 !\n");	\
+		WARN_ON(!_gpio);					\
+		ret = -EIO;						\
+		goto err_kfree;						\
+	}								\
+} while (0)
+
+static void max8997_muic_init_detect(struct work_struct *work)
+{
+	struct max8997_muic_info *info = container_of(work,
+			struct max8997_muic_info, init_work.work);
+
+	dev_info(info->dev, "%s\n", __func__);
+	if (!info->muic_data)
+		return;
+
+	mutex_lock(&info->mutex);
+	max8997_muic_detect_dev(info, -1);
+	mutex_unlock(&info->mutex);
+}
+
+static void max8997_muic_usb_detect(struct work_struct *work)
+{
+	struct max8997_muic_info *info = container_of(work,
+			struct max8997_muic_info, usb_work.work);
+	struct max8997_muic_data *mdata = info->muic_data;
+
+	dev_info(info->dev, "%s\n", __func__);
+	if (!mdata)
+		return;
+
+	mutex_lock(&info->mutex);
+	info->is_usb_ready = true;
+
+	if (info->muic_data->sw_path != CP_USB_MODE) {
+		if (mdata->usb_cb) {
+			switch (info->cable_type) {
+			case CABLE_TYPE_USB:
+			case CABLE_TYPE_JIG_USB_OFF:
+			case CABLE_TYPE_JIG_USB_ON:
+				mdata->usb_cb(USB_CABLE_ATTACHED);
+				break;
+			case CABLE_TYPE_OTG:
+				mdata->usb_cb(USB_OTGHOST_ATTACHED);
+				break;
+			default:
+				break;
+			}
+		}
+	}
+	mutex_unlock(&info->mutex);
+}
+
+static void max8997_muic_mhl_detect(struct work_struct *work)
+{
+	struct max8997_muic_info *info = container_of(work,
+			struct max8997_muic_info, mhl_work.work);
+	struct max8997_muic_data *mdata = info->muic_data;
+
+	dev_info(info->dev, "%s\n", __func__);
+	if (!mdata)
+		return;
+
+	mutex_lock(&info->mutex);
+	info->is_mhl_ready = true;
+#ifndef CONFIG_MACH_U1
+	if (mdata->is_mhl_attached) {
+		if (!mdata->is_mhl_attached())
+			goto out;
+	}
+#endif
+	if (info->cable_type == CABLE_TYPE_MHL || \
+		info->cable_type == CABLE_TYPE_MHL_VB) {
+		if (mdata->mhl_cb)
+			mdata->mhl_cb(MAX8997_MUIC_ATTACHED);
+	}
+out:
+	mutex_unlock(&info->mutex);
+}
+extern struct device *switch_dev;
+
+static int __devinit max8997_muic_probe(struct platform_device *pdev)
+{
+	struct max8997_dev *max8997 = dev_get_drvdata(pdev->dev.parent);
+	struct max8997_platform_data *pdata = dev_get_platdata(max8997->dev);
+	struct max8997_muic_info *info;
+	struct input_dev *input;
+	int ret;
+
+	info = kzalloc(sizeof(struct max8997_muic_info), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!info || !input) {
+		dev_err(&pdev->dev, "%s: failed to allocate state\n", __func__);
+		ret = -ENOMEM;
+		goto err_kfree;
+	}
+
+	info->dev = &pdev->dev;
+	info->max8997 = max8997;
+	info->muic = max8997->muic;
+	info->input = input;
+	info->irq_adc = max8997->irq_base + MAX8997_IRQ_ADC;
+	info->irq_chgtype = max8997->irq_base + MAX8997_IRQ_CHGTYP;
+	info->irq_vbvolt = max8997->irq_base + MAX8997_IRQ_VBVOLT;
+	info->irq_adcerr = max8997->irq_base + MAX8997_IRQ_ADCERR;
+#if defined(CONFIG_MUIC_MAX8997_OVPUI)
+	info->irq_chgins = max8997->irq_base + MAX8997_IRQ_CHGINS;
+	info->irq_chgrm = max8997->irq_base + MAX8997_IRQ_CHGRM;
+#endif
+	if (pdata->muic) {
+		info->muic_data = pdata->muic;
+		info->muic_data->sw_path = AP_USB_MODE;
+	}
+	info->cable_type = CABLE_TYPE_UNKNOWN;
+
+
+	platform_set_drvdata(pdev, info);
+
+	input->name = pdev->name;
+	input->phys = "deskdock-key/input0";
+	input->dev.parent = &pdev->dev;
+
+	input->id.bustype = BUS_HOST;
+	input->id.vendor = 0x0001;
+	input->id.product = 0x0001;
+	input->id.version = 0x0001;
+
+	/* Enable auto repeat feature of Linux input subsystem */
+	__set_bit(EV_REP, input->evbit);
+
+	input_set_capability(input, EV_KEY, KEY_VOLUMEUP);
+	input_set_capability(input, EV_KEY, KEY_VOLUMEDOWN);
+	input_set_capability(input, EV_KEY, KEY_PLAYPAUSE);
+	input_set_capability(input, EV_KEY, KEY_PREVIOUSSONG);
+	input_set_capability(input, EV_KEY, KEY_NEXTSONG);
+
+	ret = input_register_device(input);
+	if (ret) {
+		dev_err(info->dev, "%s: Unable to register input device, "
+				"error: %d\n", __func__, ret);
+		goto err_input;
+	}
+
+#if !defined(CONFIG_MACH_U1CAMERA_BD)
+	if (info->muic_data && gpio_is_valid(info->muic_data->gpio_usb_sel)) {
+		CHECK_GPIO(info->muic_data->gpio_usb_sel, "USB_SEL");
+
+		if (info->muic_data->cfg_uart_gpio)
+			info->muic_data->uart_path =
+				info->muic_data->cfg_uart_gpio();
+
+#ifndef CONFIG_TARGET_LOCALE_NA
+		ret = gpio_request(info->muic_data->gpio_usb_sel, "USB_SEL");
+		if (ret) {
+			dev_info(info->dev, "%s: fail to request gpio(%d)\n",
+					__func__, ret);
+			goto err_kfree;
+		}
+		if (gpio_get_value(info->muic_data->gpio_usb_sel)) {
+			dev_info(info->dev, "%s: CP USB\n", __func__);
+			info->muic_data->sw_path = CP_USB_MODE;
+		}
+#endif /* CONFIG_TARGET_LOCALE_NA */
+	}
+#endif /* CONFIG_MACH_U1CAMERA_BD */
+
+	/* create sysfs group*/
+	ret = sysfs_create_group(&switch_dev->kobj, &max8997_muic_group);
+	dev_set_drvdata(switch_dev, info);
+
+	if (ret) {
+		dev_err(&pdev->dev,
+			"failed to create max8997 muic attribute group\n");
+		goto fail;
+	}
+
+	if (info->muic_data && info->muic_data->init_cb)
+		info->muic_data->init_cb();
+
+	mutex_init(&info->mutex);
+
+	/* Set ADC debounce time: 25ms */
+	max8997_muic_set_adcdbset(info, 2);
+
+	ret = max8997_muic_irq_init(info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to initialize MUIC irq:%d\n", ret);
+		goto fail;
+	}
+
+	/* initial cable detection */
+	INIT_DELAYED_WORK(&info->init_work, max8997_muic_init_detect);
+	schedule_delayed_work(&info->init_work, msecs_to_jiffies(3000));
+
+	INIT_DELAYED_WORK(&info->usb_work, max8997_muic_usb_detect);
+	schedule_delayed_work(&info->usb_work, msecs_to_jiffies(17000));
+
+	INIT_DELAYED_WORK(&info->mhl_work, max8997_muic_mhl_detect);
+	schedule_delayed_work(&info->mhl_work, msecs_to_jiffies(25000));
+
+	return 0;
+
+fail:
+	if (info->irq_adc)
+		free_irq(info->irq_adc, NULL);
+	if (info->irq_chgtype)
+		free_irq(info->irq_chgtype, NULL);
+	if (info->irq_vbvolt)
+		free_irq(info->irq_vbvolt, NULL);
+	if (info->irq_adcerr)
+		free_irq(info->irq_adcerr, NULL);
+	mutex_destroy(&info->mutex);
+err_input:
+	platform_set_drvdata(pdev, NULL);
+err_kfree:
+	input_free_device(input);
+	kfree(info);
+	return ret;
+}
+
+static int __devexit max8997_muic_remove(struct platform_device *pdev)
+{
+	struct max8997_muic_info *info = platform_get_drvdata(pdev);
+
+	sysfs_remove_group(&switch_dev->kobj, &max8997_muic_group);
+
+	if (info) {
+		input_unregister_device(info->input);
+		cancel_delayed_work(&info->init_work);
+		cancel_delayed_work(&info->usb_work);
+		cancel_delayed_work(&info->mhl_work);
+		free_irq(info->irq_adc, info);
+		free_irq(info->irq_chgtype, info);
+		free_irq(info->irq_vbvolt, info);
+		free_irq(info->irq_adcerr, info);
+#if !defined(CONFIG_TARGET_LOCALE_NA) && !defined(CONFIG_MACH_U1CAMERA_BD)
+		gpio_free(info->muic_data->gpio_usb_sel);
+#endif /* CONFIG_TARGET_LOCALE_NA && CONFIG_MACH_U1CAMERA_BD */
+		mutex_destroy(&info->mutex);
+		kfree(info);
+	}
+
+	return 0;
+}
+
+static u8 max8997_dumpaddr_muic[] = {
+	MAX8997_MUIC_REG_INTMASK1,
+	MAX8997_MUIC_REG_CDETCTRL,
+	MAX8997_MUIC_REG_CTRL2,
+};
+
+#ifdef CONFIG_PM
+static int max8997_muic_freeze(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct max8997_muic_info *info;
+	int i;
+	info = platform_get_drvdata(pdev);
+
+	for (i = 0; i < ARRAY_SIZE(max8997_dumpaddr_muic); i++)
+		max8997_read_reg(info->max8997->muic, max8997_dumpaddr_muic[i],
+			&info->max8997->reg_dump[i + MAX8997_REG_PMIC_END]);
+
+	cancel_delayed_work(&info->init_work);
+	cancel_delayed_work(&info->usb_work);
+	cancel_delayed_work(&info->mhl_work);
+
+	/* hibernation state, disconnect usb state*/
+	dev_info(info->dev, "%s: DETACHED\n", __func__);
+	mutex_lock(&info->mutex);
+	max8997_muic_handle_detach(info);
+	mutex_unlock(&info->mutex);
+
+	return 0;
+}
+
+static int max8997_muic_restore(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct max8997_muic_info *info;
+	int i;
+	info = platform_get_drvdata(pdev);
+
+	for (i = 0; i < ARRAY_SIZE(max8997_dumpaddr_muic); i++)
+		max8997_write_reg(info->max8997->muic, max8997_dumpaddr_muic[i],
+			info->max8997->reg_dump[i + MAX8997_REG_PMIC_END]);
+
+	mutex_lock(&info->mutex);
+	max8997_muic_detect_dev(info, -1);
+	mutex_unlock(&info->mutex);
+
+	return 0;
+}
+
+static const struct dev_pm_ops max8997_dev_pm_ops = {
+	.freeze		= max8997_muic_freeze,
+	.restore	= max8997_muic_restore,
+};
+
+#define MAX8997_DEV_PM_OPS	(&max8997_dev_pm_ops)
+#else
+#define MAX8997_DEV_PM_OPS	NULL
+#endif
+
+void max8997_muic_shutdown(struct device *dev)
+{
+	struct max8997_muic_info *info = dev_get_drvdata(dev);
+	int ret;
+	u8 val;
+
+	if (!info->muic) {
+		dev_err(info->dev, "%s: no muic i2c client\n", __func__);
+		return;
+	}
+
+	dev_info(info->dev, "%s: JIGSet: auto detection\n", __func__);
+	val = (0 << CTRL3_JIGSET_SHIFT) | (0 << CTRL3_BOOTSET_SHIFT);
+
+	ret = max8997_update_reg(info->muic, MAX8997_MUIC_REG_CTRL3, val,
+			CTRL3_JIGSET_MASK | CTRL3_BOOTSET_MASK);
+	if (ret < 0) {
+		dev_err(info->dev, "%s: fail to update reg\n", __func__);
+		return;
+	}
+}
+
+static struct platform_driver max8997_muic_driver = {
+	.driver		= {
+		.name	= "max8997-muic",
+		.owner	= THIS_MODULE,
+		.pm	= MAX8997_DEV_PM_OPS,
+		.shutdown = max8997_muic_shutdown,
+	},
+	.probe		= max8997_muic_probe,
+	.remove		= __devexit_p(max8997_muic_remove),
+};
+
+static int __init max8997_muic_init(void)
+{
+	return platform_driver_register(&max8997_muic_driver);
+}
+module_init(max8997_muic_init);
+
+static void __exit max8997_muic_exit(void)
+{
+	platform_driver_unregister(&max8997_muic_driver);
+}
+module_exit(max8997_muic_exit);
+
+
+MODULE_DESCRIPTION("Maxim MAX8997 MUIC driver");
+MODULE_AUTHOR("<ms925.kim@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/modem_if/Kconfig b/drivers/misc/modem_if/Kconfig
new file mode 100644
index 0000000..94cb48c
--- /dev/null
+++ b/drivers/misc/modem_if/Kconfig
@@ -0,0 +1,75 @@
+menuconfig SEC_MODEM
+	bool "Samsung Mobile Modem Interface"
+	default n
+	---help---
+	  Samsung Modem Interface Driver.
+
+config UMTS_MODEM_XMM6260
+	bool "modem chip : IMC XMM6260"
+	depends on SEC_MODEM
+	default n
+
+config UMTS_MODEM_XMM6262
+	bool "modem chip : IMC XMM6262"
+	depends on SEC_MODEM
+	default n
+
+config CDMA_MODEM_CBP71
+	bool "modem chip : VIA CBP7.1"
+	depends on SEC_MODEM
+	default n
+
+config CDMA_MODEM_CBP72
+	bool "modem chip : VIA CBP7.2"
+	depends on SEC_MODEM
+	default n
+
+config LTE_MODEM_CMC221
+	bool "modem chip : SEC CMC221"
+	depends on SEC_MODEM
+	default n
+
+config CDMA_MODEM_MDM6600
+	bool "modem chip : QC MDM6600"
+	depends on SEC_MODEM
+	default n
+
+config LINK_DEVICE_MIPI
+	bool "modem driver link device MIPI-HSI"
+	depends on SEC_MODEM
+	default n
+
+config LINK_DEVICE_DPRAM
+	bool "modem driver link device DPRAM"
+	depends on SEC_MODEM
+	default n
+
+config LINK_DEVICE_USB
+	bool "modem driver link device USB"
+	depends on SEC_MODEM
+	default n
+
+config LINK_DEVICE_HSIC
+	bool "modem driver link device HSIC"
+	depends on SEC_MODEM
+	default n
+
+config LINK_DEVICE_C2C
+	bool "modem driver link device C2C"
+	depends on SEC_MODEM
+	default n
+
+config IPC_CMC22x_OLD_RFS
+	bool "IPC: CMC22x ancient RFS"
+	depends on SEC_MODEM
+	default n
+
+config SIPC_VER_5
+	bool "IPC: Samsung IPC 5.0"
+	depends on SEC_MODEM
+	default n
+
+config SIM_DETECT
+	bool "SIM_DETECT pin"
+	depends on SEC_MODEM
+	default n
diff --git a/drivers/misc/modem_if/Makefile b/drivers/misc/modem_if/Makefile
new file mode 100644
index 0000000..dafc8c0
--- /dev/null
+++ b/drivers/misc/modem_if/Makefile
@@ -0,0 +1,20 @@
+# Makefile of modem_if
+
+EXTRA_CFLAGS += -Idrivers/misc/modem_if
+
+obj-y += sipc5_modem.o sipc5_io_device.o
+obj-y += sipc4_modem.o sipc4_io_device.o
+obj-y += modem_net_flowcontrol_device.o modem_utils.o modem_debug.o
+
+obj-$(CONFIG_UMTS_MODEM_XMM6260) += modem_modemctl_device_xmm6260.o
+obj-$(CONFIG_UMTS_MODEM_XMM6262) += modem_modemctl_device_xmm6262.o
+obj-$(CONFIG_CDMA_MODEM_CBP71) += modem_modemctl_device_cbp71.o
+obj-$(CONFIG_CDMA_MODEM_CBP72) += modem_modemctl_device_cbp72.o
+obj-$(CONFIG_LTE_MODEM_CMC221) += modem_modemctl_device_cmc221.o lte_modem_bootloader.o
+obj-$(CONFIG_CDMA_MODEM_MDM6600) += modem_modemctl_device_mdm6600.o
+
+obj-$(CONFIG_LINK_DEVICE_MIPI) += modem_link_device_mipi.o
+obj-$(CONFIG_LINK_DEVICE_DPRAM) += modem_link_device_dpram.o
+obj-$(CONFIG_LINK_DEVICE_USB) += modem_link_device_usb.o modem_link_pm_usb.o
+obj-$(CONFIG_LINK_DEVICE_HSIC) += modem_link_device_hsic.o
+obj-$(CONFIG_LINK_DEVICE_C2C) += modem_link_device_c2c.o
diff --git a/drivers/misc/modem_if/lte_modem_bootloader.c b/drivers/misc/modem_if/lte_modem_bootloader.c
new file mode 100644
index 0000000..f259aae
--- /dev/null
+++ b/drivers/misc/modem_if/lte_modem_bootloader.c
@@ -0,0 +1,313 @@
+/* Lte modem bootloader support for Samsung Tuna Board.
+ *
+ * Copyright (C) 2011 Google, Inc.
+ * Copyright (C) 2011 Samsung Electronics.
+ *
+ * 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/init.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/wakelock.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+
+#include <linux/platform_data/modem.h>
+#include <linux/platform_data/lte_modem_bootloader.h>
+
+#define LEN_XMIT_DELEY	100
+
+#ifdef AIRPLAIN_MODE_TEST
+int lte_airplain_mode;
+#endif
+
+enum xmit_bootloader_status {
+	XMIT_BOOT_READY = 0,
+	XMIT_LOADER_READY,
+};
+
+struct lte_modem_bootloader {
+	struct spi_device *spi_dev;
+	struct miscdevice dev;
+
+	struct mutex lock;
+
+	unsigned int gpio_lte2ap_status;
+	enum xmit_bootloader_status xmit_status;
+};
+#define to_loader(misc)	container_of(misc, struct lte_modem_bootloader, dev);
+
+static inline
+int spi_xmit(struct lte_modem_bootloader *loader,
+		const unsigned char val)
+{
+	unsigned char buf[1];
+	int ret;
+	struct spi_message msg;
+
+	struct spi_transfer xfer = {
+		.len = 1,
+		.tx_buf = buf,
+	};
+
+	buf[0] = val;
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer, &msg);
+
+	ret = spi_sync(loader->spi_dev, &msg);
+
+	if (ret < 0)
+		mif_err("error %d\n", ret);
+
+	return ret;
+}
+
+static
+int bootloader_write(struct lte_modem_bootloader *loader,
+		const char *addr, const int len)
+{
+	int i;
+	int ret = 0;
+	unsigned char lenbuf[4];
+
+	if (loader->xmit_status == XMIT_LOADER_READY) {
+		memcpy(lenbuf, &len, ARRAY_SIZE(lenbuf));
+		for (i = 0 ; i < ARRAY_SIZE(lenbuf) ; i++) {
+			ret = spi_xmit(loader, lenbuf[i]);
+			if (ret < 0)
+				return ret;
+		}
+		msleep(LEN_XMIT_DELEY);
+	}
+
+	for (i = 0 ; i < len ; i++) {
+		ret = spi_xmit(loader, addr[i]);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+
+static
+int bootloader_open(struct inode *inode, struct file *flip)
+{
+	struct lte_modem_bootloader *loader = to_loader(flip->private_data);
+	flip->private_data = loader;
+
+	return 0;
+}
+
+static
+long bootloader_ioctl(struct file *flip,
+		unsigned int cmd, unsigned long arg)
+{
+	int ret = 0;
+	int status;
+	struct lte_modem_bootloader_param param;
+	struct lte_modem_bootloader *loader = flip->private_data;
+
+	mutex_lock(&loader->lock);
+	switch (cmd) {
+	case IOCTL_LTE_MODEM_XMIT_BOOT:
+
+		ret = copy_from_user(&param, (const void __user *)arg,
+						sizeof(param));
+		if (ret) {
+			mif_err("can not copy userdata\n");
+			ret = -EFAULT;
+			goto exit_err;
+		}
+
+		dev_info(&loader->spi_dev->dev,
+				"IOCTL_LTE_MODEM_XMIT_BOOT - bin size: %d\n",
+				param.len);
+
+		ret = bootloader_write(loader, param.buf, param.len);
+		if (ret < 0)
+			mif_err("failed to xmit boot bin\n");
+		else {
+			if (loader->xmit_status == XMIT_BOOT_READY)
+				loader->xmit_status = XMIT_LOADER_READY;
+		else
+				loader->xmit_status = XMIT_BOOT_READY;
+		}
+
+		break;
+	case IOCTL_LTE_MODEM_LTE2AP_STATUS:
+		status = gpio_get_value(loader->gpio_lte2ap_status);
+		mif_debug("LTE2AP status :%d\n", status);
+		ret = copy_to_user((unsigned int *)arg, &status,
+				sizeof(status));
+
+		break;
+#ifdef AIRPLAIN_MODE_TEST
+	case IOCTL_LTE_MODEM_AIRPLAIN_ON:
+		lte_airplain_mode = 1;
+		mif_info("IOCTL_LTE_MODEM LPM_ON\n");
+		break;
+	case IOCTL_LTE_MODEM_AIRPLAIN_OFF:
+		mif_info("IOCTL_LTE_MODEM LPM_OFF\n");
+		lte_airplain_mode = 0;
+		break;
+#endif
+	default:
+		mif_err("ioctl cmd error\n");
+		ret = -ENOIOCTLCMD;
+
+		break;
+	}
+	mutex_unlock(&loader->lock);
+
+exit_err:
+	return ret;
+}
+
+static const struct file_operations lte_modem_bootloader_fops = {
+	.owner = THIS_MODULE,
+	.open = bootloader_open,
+	.unlocked_ioctl = bootloader_ioctl,
+};
+
+static
+int bootloader_gpio_setup(struct lte_modem_bootloader *loader)
+{
+	if (!loader->gpio_lte2ap_status)
+		return -EINVAL;
+
+	gpio_request(loader->gpio_lte2ap_status, "GPIO_LTE2AP_STATUS");
+	gpio_direction_input(loader->gpio_lte2ap_status);
+
+	return 0;
+}
+
+static
+int __devinit lte_modem_bootloader_probe(struct spi_device *spi)
+{
+	int ret;
+
+	struct lte_modem_bootloader *loader;
+	struct lte_modem_bootloader_platform_data *pdata;
+
+	loader = kzalloc(sizeof(*loader), GFP_KERNEL);
+	if (!loader) {
+		mif_err("failed to allocate for lte_modem_bootloader\n");
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+	mutex_init(&loader->lock);
+
+	spi->bits_per_word = 8;
+
+	if (spi_setup(spi)) {
+		mif_err("failed to setup spi for lte_modem_bootloader\n");
+		ret = -EINVAL;
+		goto err_setup;
+	}
+
+	loader->spi_dev = spi;
+
+	if (!spi->dev.platform_data) {
+		mif_err("failed to get platform data for lte_modem_bootloader\n");
+		ret = -EINVAL;
+		goto err_setup;
+	}
+	pdata = (struct lte_modem_bootloader_platform_data *)
+			spi->dev.platform_data;
+	loader->gpio_lte2ap_status = pdata->gpio_lte2ap_status;
+
+	ret = bootloader_gpio_setup(loader);
+	if (ret) {
+		mif_err("failed to set gpio for lte_modem_boot_loader\n");
+		goto err_setup;
+	}
+
+	loader->gpio_lte2ap_status = pdata->gpio_lte2ap_status;
+	loader->xmit_status = XMIT_BOOT_READY;
+
+	spi_set_drvdata(spi, loader);
+
+	loader->dev.minor = MISC_DYNAMIC_MINOR;
+	loader->dev.name = "lte_spi";
+	loader->dev.fops = &lte_modem_bootloader_fops;
+	ret = misc_register(&loader->dev);
+	if (ret) {
+		mif_err("failed to register misc dev for lte_modem_bootloader\n");
+		goto err_setup;
+	}
+	mif_info("lte_modem_bootloader successfully probed\n");
+#ifdef AIRPLAIN_MODE_TEST
+	lte_airplain_mode = 0;
+#endif
+	return 0;
+
+err_setup:
+	mutex_destroy(&loader->lock);
+	kfree(loader);
+
+err_alloc:
+
+	return ret;
+}
+
+static
+int __devexit lte_modem_bootloader_remove(struct spi_device *spi)
+{
+	struct lte_modem_bootloader *loader = spi_get_drvdata(spi);
+
+	misc_deregister(&loader->dev);
+	mutex_destroy(&loader->lock);
+	kfree(loader);
+
+	return 0;
+}
+
+static
+struct spi_driver lte_modem_bootloader_driver = {
+	.driver = {
+		.name = "lte_modem_spi",
+		.bus = &spi_bus_type,
+		.owner = THIS_MODULE,
+	},
+	.probe = lte_modem_bootloader_probe,
+	.remove = __devexit_p(lte_modem_bootloader_remove),
+};
+
+static
+int __init lte_modem_bootloader_init(void)
+{
+	return spi_register_driver(&lte_modem_bootloader_driver);
+}
+
+static
+void __exit lte_modem_bootloader_exit(void)
+{
+	spi_unregister_driver(&lte_modem_bootloader_driver);
+}
+
+module_init(lte_modem_bootloader_init);
+module_exit(lte_modem_bootloader_exit);
+
+MODULE_DESCRIPTION("LTE Modem Bootloader driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/modem_if/modem_debug.c b/drivers/misc/modem_if/modem_debug.c
new file mode 100644
index 0000000..1ad3073
--- /dev/null
+++ b/drivers/misc/modem_if/modem_debug.c
@@ -0,0 +1,429 @@
+/* linux/drivers/misc/modem_if/modem_debug.c
+ *
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+#include <linux/if_arp.h>
+
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+#include <linux/wait.h>
+#include <linux/time.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/wakelock.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_variation.h"
+#include "modem_utils.h"
+
+static const char *hex = "0123456789abcdef";
+
+static inline void mif_irq2str(struct mif_event_buff *evtb, char *buff)
+{
+	int i;
+	char tb[32];
+
+	if (evtb->link_type == LINKDEV_DPRAM) {
+		struct dpram_irq_buff *irqb = &evtb->dpram_irqb;
+
+		sprintf(tb, "{0x%04X %d 0x%04X}",
+			irqb->magic, irqb->access, irqb->int2ap);
+		strcat(buff, tb);
+
+		for (i = 0; i < IPC_RFS; i++) {
+			snprintf(tb, 32, " {%d: %u %u %u %u}", i,
+				irqb->qsp[i].txq.in, irqb->qsp[i].txq.out,
+				irqb->qsp[i].rxq.in, irqb->qsp[i].rxq.out);
+			strcat(buff, tb);
+		}
+	} else {
+		sprintf(tb, "link unspeicified");
+		strcat(buff, tb);
+	}
+}
+
+static inline void mif_dump2hex(const char *data, size_t len, char *buff)
+{
+	char *src = (char *)data;
+	int i;
+	char tb[4];
+
+	tb[3] = 0;
+	for (i = 0; i < len; i++) {
+		tb[0] = hex[(*src >> 4) & 0xf];
+		tb[1] = hex[*src & 0xf];
+		tb[2] = ' ';
+		strcat(buff, tb);
+		src++;
+	}
+}
+
+static inline void mif_fin_str(char *buff)
+{
+	char tb[4];
+	sprintf(tb, "\n");
+	strcat(buff, tb);
+}
+
+static void mif_log2str(struct mif_event_buff *evtb, char *buff)
+{
+	struct timeval *tv;
+	struct tm date;
+
+	tv = &evtb->tv;
+
+	time_to_tm((tv->tv_sec - sys_tz.tz_minuteswest * 60), 0, &date);
+	sprintf(evtb->time, "%02d:%02d:%02d.%03ld",
+		date.tm_hour, date.tm_min, date.tm_sec, (tv->tv_usec / 1000));
+
+	if (evtb->evt == MIF_IRQ_EVT) {
+		sprintf(buff, "%s IRQ <%s> ", evtb->time, evtb->ld);
+		mif_irq2str(evtb, buff);
+		mif_fin_str(buff);
+	} else {
+		size_t len = evtb->len < (MAX_MIF_LOG_LEN >> 3) ?
+				evtb->len : (MAX_MIF_LOG_LEN >> 3);
+		sprintf(buff, "%s [%d] <%s:%s> ",
+			evtb->time, evtb->evt, evtb->iod, evtb->ld);
+		mif_dump2hex(evtb->data, len, buff);
+		mif_fin_str(buff);
+	}
+}
+
+static void mif_print_logs(struct modem_ctl *mc)
+{
+	struct sk_buff *skb;
+	struct mif_event_buff *evtb;
+	u8 *buff;
+
+	buff = kmalloc(2048, GFP_ATOMIC);
+	if (!buff)
+		return;
+
+	while (1) {
+		skb = skb_dequeue(&mc->evtq);
+		if (!skb)
+			break;
+
+		evtb = (struct mif_event_buff *)skb->data;
+		memset(buff, 0, 2048);
+
+		mif_log2str(evtb, buff);
+		pr_info("mif: %s", buff);
+
+		dev_kfree_skb_any(skb);
+	}
+
+	kfree(buff);
+}
+
+static void mif_save_logs(struct modem_ctl *mc)
+{
+	struct file *fp = mc->log_fp;
+	struct sk_buff *skb;
+	struct mif_event_buff *evtb;
+	int qlen = mc->evtq.qlen;
+	int i;
+	int ret;
+	mm_segment_t old_fs;
+
+	old_fs = get_fs();
+	set_fs(get_ds());
+
+	for (i = 0; i < qlen; i++) {
+		skb = skb_dequeue(&mc->evtq);
+
+#if 0
+		if (evtb->evt < mc->log_level) {
+			ret = fp->f_op->write(fp, skb->data,
+					MAX_MIF_EVT_BUFF_SIZE, &fp->f_pos);
+			if (ret < 0) {
+				mif_log2str((struct mif_event_buff *)skb->data,
+						mc->buff);
+				printk(KERN_ERR "%s", mc->buff);
+			}
+		}
+#else
+		evtb = (struct mif_event_buff *)skb->data;
+		if (evtb->evt < mc->log_level) {
+			mif_log2str(evtb, mc->buff);
+			ret = fp->f_op->write(fp, mc->buff, strlen(mc->buff),
+						&fp->f_pos);
+			if (ret < 0)
+				printk(KERN_ERR "%s", mc->buff);
+		}
+#endif
+
+		dev_kfree_skb_any(skb);
+	}
+
+	set_fs(old_fs);
+}
+
+static void mif_evt_work(struct work_struct *work)
+{
+	struct modem_ctl *mc = container_of(work, struct modem_ctl, evt_work);
+	struct file *fp = mc->log_fp;
+	loff_t size;
+	mm_segment_t old_fs;
+
+	/* use_mif_log */
+
+	if (!mc->log_level || mc->fs_failed) {
+		mif_print_logs(mc);
+		return;
+	}
+
+	/* use_mif_log && log_level && !fs_failed */
+
+	if (!mc->fs_ready) {
+		if (mc->evtq.qlen > 1000)
+			mif_print_logs(mc);
+		return;
+	}
+
+	/* use_mif_log && log_level && !fs_failed && fs_ready */
+
+	if (fp) {
+		mif_save_logs(mc);
+
+		old_fs = get_fs();
+		set_fs(get_ds());
+		size = fp->f_pos;
+		set_fs(old_fs);
+		if (size > MAX_MIF_LOG_FILE_SIZE) {
+			mif_err("%s size %lld > %d\n", mc->log_path, size,
+				MAX_MIF_LOG_FILE_SIZE);
+			mif_close_log_file(mc);
+			mif_open_log_file(mc);
+		}
+	}
+}
+
+void mif_irq_log(struct modem_ctl *mc, struct sk_buff *skb)
+{
+	if (!mc || !mc->use_mif_log)
+		return;
+	skb_queue_tail(&mc->evtq, skb);
+}
+
+void mif_ipc_log(struct modem_ctl *mc, enum mif_event_id evt,
+		struct io_device *iod, struct link_device *ld,
+		u8 *data, unsigned size)
+{
+	struct sk_buff *skb;
+	struct mif_event_buff *evtb;
+	unsigned len;
+
+	if (!mc || !mc->use_mif_log)
+		return;
+
+	skb = alloc_skb(MAX_MIF_EVT_BUFF_SIZE, GFP_ATOMIC);
+	if (!skb)
+		return;
+
+	evtb = (struct mif_event_buff *)skb_put(skb, MAX_MIF_EVT_BUFF_SIZE);
+	memset(evtb, 0, MAX_MIF_EVT_BUFF_SIZE);
+
+	do_gettimeofday(&evtb->tv);
+	evtb->evt = evt;
+
+	strncpy(evtb->mc, mc->name, MAX_MIF_NAME_LEN);
+
+	if (iod)
+		strncpy(evtb->iod, iod->name, MAX_MIF_NAME_LEN);
+
+	if (ld) {
+		strncpy(evtb->ld, ld->name, MAX_MIF_NAME_LEN);
+		evtb->link_type = ld->link_type;
+	}
+
+	len = min_t(unsigned, MAX_MIF_LOG_LEN, size);
+	memcpy(evtb->data, data, len);
+
+	evtb->rcvd = size;
+	evtb->len = len;
+
+	skb_queue_tail(&mc->evtq, skb);
+}
+
+void mif_flush_logs(struct modem_ctl *mc)
+{
+	if (!mc || !mc->use_mif_log)
+		return;
+
+	if (atomic_read(&mc->log_open))
+		queue_work(mc->evt_wq, &mc->evt_work);
+}
+
+int mif_init_log(struct modem_ctl *mc)
+{
+	char wq_name[32];
+	char wq_suffix[32];
+
+	mc->log_level = 0;
+
+	atomic_set(&mc->log_open, 0);
+
+	memset(wq_name, 0, sizeof(wq_name));
+	memset(wq_suffix, 0, sizeof(wq_suffix));
+	strncpy(wq_name, mc->name, sizeof(wq_name));
+	snprintf(wq_suffix, sizeof(wq_suffix), "%s", "_evt_wq");
+	strncat(wq_name, wq_suffix, sizeof(wq_suffix));
+	mc->evt_wq = create_singlethread_workqueue(wq_name);
+	if (!mc->evt_wq) {
+		printk(KERN_ERR "<%s:%s> fail to create %s\n",
+			__func__, mc->name, wq_name);
+		return -EFAULT;
+	}
+	printk(KERN_ERR "<%s:%s> %s created\n",
+		__func__, mc->name, wq_name);
+
+	INIT_WORK(&mc->evt_work, mif_evt_work);
+
+	skb_queue_head_init(&mc->evtq);
+
+	mc->buff = kzalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!mc->buff) {
+		printk(KERN_ERR "<%s> kzalloc fail\n", __func__);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+void mif_set_log_level(struct modem_ctl *mc)
+{
+	struct file *fp;
+	int ret;
+	mm_segment_t old_fs;
+
+	mc->log_level = 0;
+
+	old_fs = get_fs();
+	set_fs(get_ds());
+	fp = filp_open(MIF_LOG_LV_FILE, O_RDONLY, 0);
+	if (IS_ERR(fp)) {
+		printk(KERN_ERR "<%s:%s> %s open fail\n",
+			__func__, mc->name, MIF_LOG_LV_FILE);
+	} else {
+		char tb;
+
+		ret = fp->f_op->read(fp, &tb, 1, &fp->f_pos);
+		if (ret > 0) {
+			mc->log_level = tb & 0xF;
+		} else {
+			printk(KERN_ERR "<%s:%s> read fail (err %d)\n",
+				__func__, mc->name, ret);
+		}
+	}
+	set_fs(old_fs);
+
+	if (mc->use_mif_log && !mc->log_level)
+		atomic_set(&mc->log_open, 1);
+
+	printk(KERN_ERR "<%s:%s> log level = %d\n",
+		__func__, mc->name, mc->log_level);
+}
+
+int mif_open_log_file(struct modem_ctl *mc)
+{
+	struct timeval now;
+	struct tm date;
+	mm_segment_t old_fs;
+
+	if (!mc || !mc->use_mif_log)
+		return -EINVAL;
+
+	if (!mc->log_level) {
+		printk(KERN_ERR "<%s:%s> IPC logger is disabled.\n",
+			__func__, mc->name);
+		return -EINVAL;
+	}
+
+	if (!mc->fs_ready) {
+		printk(KERN_ERR "<%s:%s> File system is not ready.\n",
+			__func__, mc->name);
+		return -EINVAL;
+	}
+
+	if (mc->fs_failed) {
+		printk(KERN_ERR "<%s:%s> Log file cannot be created.\n",
+			__func__, mc->name);
+		return -EINVAL;
+	}
+
+	do_gettimeofday(&now);
+	time_to_tm((now.tv_sec - sys_tz.tz_minuteswest * 60), 0, &date);
+
+	snprintf(mc->log_path, MAX_MIF_LOG_PATH_LEN,
+		"%s/%s_mif_log.%ld%02d%02d.%02d%02d%02d.txt",
+		MIF_LOG_DIR, mc->name,
+		(1900 + date.tm_year), (1 + date.tm_mon), date.tm_mday,
+		date.tm_hour, date.tm_min, date.tm_sec);
+
+	old_fs = get_fs();
+	set_fs(get_ds());
+	mc->log_fp = filp_open(mc->log_path, O_RDWR|O_CREAT, 0666);
+	set_fs(old_fs);
+	if (IS_ERR(mc->log_fp)) {
+		printk(KERN_ERR "<%s:%s> %s open fail\n",
+			__func__, mc->name, mc->log_path);
+		mc->log_fp = NULL;
+		mc->fs_failed = true;
+		return -ENOENT;
+	}
+
+	atomic_set(&mc->log_open, 1);
+
+	mif_err("open %s\n", mc->log_path);
+
+	return 0;
+}
+
+void mif_close_log_file(struct modem_ctl *mc)
+{
+	mm_segment_t old_fs;
+
+	if (!mc || !mc->use_mif_log || !mc->log_level || mc->fs_failed ||
+	    !mc->fs_ready || !mc->log_fp)
+		return;
+
+	atomic_set(&mc->log_open, 0);
+
+	flush_work_sync(&mc->evt_work);
+
+	mif_err("close %s\n", mc->log_path);
+
+	mif_save_logs(mc);
+
+	old_fs = get_fs();
+	set_fs(get_ds());
+	filp_close(mc->log_fp, NULL);
+	set_fs(old_fs);
+
+	mc->log_fp = NULL;
+}
+
diff --git a/drivers/misc/modem_if/modem_link_device_c2c.c b/drivers/misc/modem_if/modem_link_device_c2c.c
new file mode 100644
index 0000000..acbaadf
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_c2c.c
@@ -0,0 +1,61 @@
+/* /linux/drivers/new_modem_if/link_dev_c2c.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/timer.h>
+#include <linux/wakelock.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+#include <linux/proc_fs.h>
+#include <linux/if_arp.h>
+#include <linux/platform_device.h>
+
+#include <linux/platform_data/modem.h>
+#include <linux/platform_data/c2c.h>
+#include "modem_prj.h"
+#include "modem_link_device_c2c.h"
+
+struct link_device *c2c_create_link_device(struct platform_device *pdev)
+{
+	struct c2c_link_device *dpld;
+	struct link_device *ld;
+	struct modem_data *pdata;
+
+	pdata = pdev->dev.platform_data;
+
+	dpld = kzalloc(sizeof(struct c2c_link_device), GFP_KERNEL);
+	if (!dpld) {
+		mif_err("dpld == NULL\n");
+		return NULL;
+	}
+
+	wake_lock_init(&dpld->c2c_wake_lock, WAKE_LOCK_SUSPEND, "c2c_wakelock");
+	wake_lock(&dpld->c2c_wake_lock);
+
+	ld = &dpld->ld;
+	dpld->pdata = pdata;
+
+	ld->name = "c2c";
+
+	mif_info("%s is created!!!\n", dpld->ld.name);
+
+	return ld;
+}
diff --git a/drivers/misc/modem_if/modem_link_device_c2c.h b/drivers/misc/modem_if/modem_link_device_c2c.h
new file mode 100644
index 0000000..7ec9aa6
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_c2c.h
@@ -0,0 +1,216 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/wakelock.h>
+
+#ifndef __MODEM_LINK_DEVICE_C2C_H__
+#define __MODEM_LINK_DEVICE_C2C_H__
+
+#define DPRAM_ERR_MSG_LEN		128
+#define DPRAM_ERR_DEVICE		"c2cerr"
+
+#define MAX_IDX				2
+
+#define DPRAM_BASE_PTR			0x4000000
+
+#define DPRAM_START_ADDRESS		0
+#define DPRAM_MAGIC_CODE_ADDRESS	DPRAM_START_ADDRESS
+#define DPRAM_GOTA_MAGIC_CODE_SIZE		0x4
+#define DPRAM_PDA2PHONE_FORMATTED_START_ADDRESS	\
+	(DPRAM_START_ADDRESS + DPRAM_GOTA_MAGIC_CODE_SIZE)
+#define BSP_DPRAM_BASE_SIZE		0x1ff8
+#define DPRAM_END_OF_ADDRESS		(BSP_DPRAM_BASE_SIZE - 1)
+#define DPRAM_INTERRUPT_SIZE		0x2
+#define DPRAM_PDA2PHONE_INTERRUPT_ADDRESS	\
+	(DPRAM_START_ADDRESS + BSP_DPRAM_BASE_SIZE -  DPRAM_INTERRUPT_SIZE*2)
+#define DPRAM_PHONE2PDA_INTERRUPT_ADDRESS	\
+	(DPRAM_START_ADDRESS + BSP_DPRAM_BASE_SIZE)
+#define DPRAM_BUFFER_SIZE			\
+	(DPRAM_PHONE2PDA_INTERRUPT_ADDRESS -\
+	DPRAM_PDA2PHONE_FORMATTED_START_ADDRESS)
+#define DPRAM_INDEX_SIZE		0x2
+
+#define MAGIC_DMDL			0x4445444C
+#define MAGIC_UMDL			0x4445444D
+
+#define DPRAM_PACKET_DATA_SIZE		0x3f00
+#define DPRAM_PACKET_HEADER_SIZE	0x7
+
+#define INT_GOTA_MASK_VALID		0xA000
+#define INT_DPRAM_DUMP_MASK_VALID		0xA000
+#define MASK_CMD_RECEIVE_READY_NOTIFICATION	0xA100
+#define MASK_CMD_DOWNLOAD_START_REQUEST		0xA200
+#define MASK_CMD_DOWNLOAD_START_RESPONSE	0xA301
+#define MASK_CMD_IMAGE_SEND_REQUEST		0xA400
+#define MASK_CMD_IMAGE_SEND_RESPONSE		0xA500
+#define MASK_CMD_SEND_DONE_REQUEST		0xA600
+#define MASK_CMD_SEND_DONE_RESPONSE		0xA701
+#define MASK_CMD_STATUS_UPDATE_NOTIFICATION	0xA800
+#define MASK_CMD_UPDATE_DONE_NOTIFICATION	0xA900
+#define MASK_CMD_EFS_CLEAR_RESPONSE		0xAB00
+#define MASK_CMD_ALARM_BOOT_OK			0xAC00
+#define MASK_CMD_ALARM_BOOT_FAIL		0xAD00
+
+#define WRITEIMG_HEADER_SIZE			8
+#define WRITEIMG_TAIL_SIZE			4
+#define WRITEIMG_BODY_SIZE			\
+	(DPRAM_BUFFER_SIZE - WRITEIMG_HEADER_SIZE - WRITEIMG_TAIL_SIZE)
+
+#define DPDN_DEFAULT_WRITE_LEN			WRITEIMG_BODY_SIZE
+#define CMD_DL_START_REQ			0x9200
+#define CMD_IMG_SEND_REQ			0x9400
+#define CMD_DL_SEND_DONE_REQ			0x9600
+
+#define CMD_UL_START_REQ		0x9200
+#define CMD_UL_START_READY		0x9400
+#define CMD_UL_SEND_RESP		0x9601
+#define CMD_UL_SEND_DONE_RESP		0x9801
+#define CMD_UL_SEND_REQ			0xA500
+#define CMD_UL_START_RESPONSE		0xA301
+#define CMD_UL_SEND_DONE_REQ		0xA700
+#define CMD_RECEIVE_READY_NOTIFICATION	0xA100
+
+#define MASK_CMD_RESULT_FAIL		0x0002
+#define MASK_CMD_RESULT_SUCCESS		0x0001
+
+#define START_INDEX			0x007F
+#define END_INDEX			0x007E
+
+#define CMD_IMG_SEND_REQ		0x9400
+
+#define CRC_TAB_SIZE			256
+#define CRC_16_L_SEED			0xFFFF
+
+struct c2c_device {
+	/* DPRAM memory addresses */
+	u16		*in_head_addr;
+	u16		*in_tail_addr;
+	u8		*in_buff_addr;
+	unsigned long	in_buff_size;
+
+	u16		*out_head_addr;
+	u16		*out_tail_addr;
+	u8		*out_buff_addr;
+	unsigned long	out_buff_size;
+
+	unsigned long	in_head_saved;
+	unsigned long	in_tail_saved;
+	unsigned long	out_head_saved;
+	unsigned long	out_tail_saved;
+
+	u16		mask_req_ack;
+	u16		mask_res_ack;
+	u16		mask_send;
+};
+
+struct memory_region {
+	u8 *control;
+	u8 *fmt_out;
+	u8 *raw_out;
+	u8 *fmt_in;
+	u8 *raw_in;
+	u8 *mbx;
+};
+
+struct UldDataHeader {
+	u8 bop;
+	u16 total_frame;
+	u16 curr_frame;
+	u16 len;
+};
+
+struct c2c_link_device {
+	struct link_device ld;
+
+	struct modem_data *pdata;
+
+	/*only c2c*/
+	struct wake_lock c2c_wake_lock;
+	atomic_t raw_txq_req_ack_rcvd;
+	atomic_t fmt_txq_req_ack_rcvd;
+	u8 net_stop_flag;
+	int phone_sync;
+	u8 phone_status;
+
+	 struct work_struct xmit_work_struct;
+
+	struct workqueue_struct *gota_wq;
+	struct work_struct gota_cmd_work;
+
+	struct c2c_device dev_map[MAX_IDX];
+
+	struct wake_lock dumplock;
+
+	u8 c2c_read_data[131072];
+
+	int c2c_init_cmd_wait_condition;
+	wait_queue_head_t c2c_init_cmd_wait_q;
+
+	int modem_pif_init_wait_condition;
+	wait_queue_head_t modem_pif_init_done_wait_q;
+
+	struct completion gota_download_start_complete;
+
+	int gota_send_done_cmd_wait_condition;
+	wait_queue_head_t gota_send_done_cmd_wait_q;
+
+	int gota_update_done_cmd_wait_condition;
+	wait_queue_head_t gota_update_done_cmd_wait_q;
+
+	int upload_send_req_wait_condition;
+	wait_queue_head_t upload_send_req_wait_q;
+
+	int upload_send_done_wait_condition;
+	wait_queue_head_t upload_send_done_wait_q;
+
+	int upload_start_req_wait_condition;
+	wait_queue_head_t upload_start_req_wait_q;
+
+	int upload_packet_start_condition;
+	wait_queue_head_t upload_packet_start_wait_q;
+
+	u16 gota_irq_handler_cmd;
+
+	u16 c2c_dump_handler_cmd;
+
+	int dump_region_number;
+
+	unsigned int is_c2c_err ;
+
+	int c2c_dump_start;
+	int gota_start;
+
+	char c2c_err_buf[DPRAM_ERR_MSG_LEN];
+
+	struct fasync_struct *c2c_err_async_q;
+
+	void (*clear_interrupt)(struct c2c_link_device *);
+
+	struct memory_region m_region;
+
+	unsigned long  fmt_out_buff_size;
+	unsigned long  raw_out_buff_size;
+	unsigned long  fmt_in_buff_size;
+	unsigned long  raw_in_buff_size;
+
+	struct delayed_work delayed_tx;
+	struct sk_buff *delayed_skb;
+	u8 delayed_count;
+};
+
+/* converts from struct link_device* to struct xxx_link_device* */
+#define to_c2c_link_device(linkdev) \
+			container_of(linkdev, struct c2c_link_device, ld)
+
+#endif
diff --git a/drivers/misc/modem_if/modem_link_device_dpram.c b/drivers/misc/modem_if/modem_link_device_dpram.c
new file mode 100644
index 0000000..862de30
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_dpram.c
@@ -0,0 +1,1931 @@
+/*
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/irq.h>
+#include <linux/gpio.h>
+#include <linux/time.h>
+#include <linux/interrupt.h>
+#include <linux/timer.h>
+#include <linux/wakelock.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+#include <linux/if_arp.h>
+#include <linux/platform_device.h>
+#include <linux/kallsyms.h>
+#include <linux/platform_data/modem.h>
+
+#include "modem_prj.h"
+#include "modem_link_device_dpram.h"
+#include "modem_utils.h"
+
+const inline char *get_dev_name(int dev)
+{
+	if (dev == IPC_FMT)
+		return "FMT";
+	else if (dev == IPC_RAW)
+		return "RAW";
+	else if (dev == IPC_RFS)
+		return "RFS";
+	else
+		return "NONE";
+}
+
+static void log_dpram_irq(struct dpram_link_device *dpld, u16 int2ap)
+{
+	struct sk_buff *skb;
+	struct mif_event_buff *evtb;
+	struct dpram_irq_buff *irqb;
+	struct link_device *ld = &dpld->ld;
+
+	skb = alloc_skb(MAX_MIF_EVT_BUFF_SIZE, GFP_ATOMIC);
+	if (!skb)
+		return;
+
+	evtb = (struct mif_event_buff *)skb_put(skb, MAX_MIF_EVT_BUFF_SIZE);
+	memset(evtb, 0, MAX_MIF_EVT_BUFF_SIZE);
+
+	do_gettimeofday(&evtb->tv);
+	evtb->evt = MIF_IRQ_EVT;
+
+	strncpy(evtb->mc, ld->mc->name, MAX_MIF_NAME_LEN);
+	strncpy(evtb->ld, ld->name, MAX_MIF_NAME_LEN);
+	evtb->link_type = ld->link_type;
+
+	irqb = &evtb->dpram_irqb;
+
+	irqb->magic = dpld->dpctl->get_magic();
+	irqb->access = dpld->dpctl->get_access();
+
+	irqb->qsp[IPC_FMT].txq.in = dpld->dpctl->get_tx_head(IPC_FMT);
+	irqb->qsp[IPC_FMT].txq.out = dpld->dpctl->get_tx_tail(IPC_FMT);
+	irqb->qsp[IPC_FMT].rxq.in = dpld->dpctl->get_rx_head(IPC_FMT);
+	irqb->qsp[IPC_FMT].rxq.out = dpld->dpctl->get_rx_tail(IPC_FMT);
+
+	irqb->qsp[IPC_RAW].txq.in = dpld->dpctl->get_tx_head(IPC_RAW);
+	irqb->qsp[IPC_RAW].txq.out = dpld->dpctl->get_tx_tail(IPC_RAW);
+	irqb->qsp[IPC_RAW].rxq.in = dpld->dpctl->get_rx_head(IPC_RAW);
+	irqb->qsp[IPC_RAW].rxq.out = dpld->dpctl->get_rx_tail(IPC_RAW);
+
+	irqb->int2ap = int2ap;
+
+	evtb->rcvd = sizeof(struct dpram_irq_buff);
+	evtb->len = sizeof(struct dpram_irq_buff);
+
+	mif_irq_log(ld->mc, skb);
+	mif_flush_logs(ld->mc);
+}
+
+static int memcmp16_to_io(const void __iomem *to, void *from, int size)
+{
+	u16 *d = (u16 *)to;
+	u16 *s = (u16 *)from;
+	int count = size >> 1;
+	int diff = 0;
+	int i;
+	u16 d1;
+	u16 s1;
+
+	for (i = 0; i < count; i++) {
+		d1 = ioread16(d);
+		s1 = *s;
+		if (d1 != s1) {
+			diff++;
+			mif_info("ERR! [%d] d:0x%04X != s:0x%04X\n", i, d1, s1);
+		}
+		d++;
+		s++;
+	}
+
+	return diff;
+}
+
+static int test_dpram(char *dp_name, u8 __iomem *start, u32 size)
+{
+	u8 __iomem *dst;
+	int i;
+	u16 val;
+
+	mif_info("%s: start = 0x%p, size = %d\n", dp_name, start, size);
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		iowrite16((i & 0xFFFF), dst);
+		dst += 2;
+	}
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		val = ioread16(dst);
+		if (val != (i & 0xFFFF)) {
+			mif_info("%s: ERR! dst[%d] 0x%04X != 0x%04X\n",
+				dp_name, i, val, (i & 0xFFFF));
+			return -EINVAL;
+		}
+		dst += 2;
+	}
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		iowrite16(0x00FF, dst);
+		dst += 2;
+	}
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		val = ioread16(dst);
+		if (val != 0x00FF) {
+			mif_info("%s: ERR! dst[%d] 0x%04X != 0x00FF\n",
+				dp_name, i, val);
+			return -EINVAL;
+		}
+		dst += 2;
+	}
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		iowrite16(0x0FF0, dst);
+		dst += 2;
+	}
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		val = ioread16(dst);
+		if (val != 0x0FF0) {
+			mif_info("%s: ERR! dst[%d] 0x%04X != 0x0FF0\n",
+				dp_name, i, val);
+			return -EINVAL;
+		}
+		dst += 2;
+	}
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		iowrite16(0xFF00, dst);
+		dst += 2;
+	}
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		val = ioread16(dst);
+		if (val != 0xFF00) {
+			mif_info("%s: ERR! dst[%d] 0x%04X != 0xFF00\n",
+				dp_name, i, val);
+			return -EINVAL;
+		}
+		dst += 2;
+	}
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		iowrite16(0, dst);
+		dst += 2;
+	}
+
+	dst = start;
+	for (i = 0; i < (size >> 1); i++) {
+		val = ioread16(dst);
+		if (val != 0) {
+			mif_info("%s: ERR! dst[%d] 0x%04X != 0\n",
+				dp_name, i, val);
+			return -EINVAL;
+		}
+		dst += 2;
+	}
+
+	mif_info("%s: PASS!!!\n", dp_name);
+	return 0;
+}
+
+static struct dpram_rxb *rxbq_create_pool(unsigned size, int count)
+{
+	struct dpram_rxb *rxb;
+	u8 *buff;
+	int i;
+
+	rxb = kzalloc(sizeof(struct dpram_rxb) * count, GFP_KERNEL);
+	if (!rxb) {
+		mif_info("ERR! kzalloc rxb fail\n");
+		return NULL;
+	}
+
+	buff = kzalloc((size * count), GFP_KERNEL|GFP_DMA);
+	if (!buff) {
+		mif_info("ERR! kzalloc buff fail\n");
+		kfree(rxb);
+		return NULL;
+	}
+
+	for (i = 0; i < count; i++) {
+		rxb[i].buff = buff;
+		rxb[i].size = size;
+		buff += size;
+	}
+
+	return rxb;
+}
+
+static inline unsigned rxbq_get_page_size(unsigned len)
+{
+	return ((len + PAGE_SIZE - 1) >> PAGE_SHIFT) << PAGE_SHIFT;
+}
+
+static inline bool rxbq_empty(struct dpram_rxb_queue *rxbq)
+{
+	return (rxbq->in == rxbq->out) ? true : false;
+}
+
+static inline int rxbq_free_size(struct dpram_rxb_queue *rxbq)
+{
+	int in = rxbq->in;
+	int out = rxbq->out;
+	int qsize = rxbq->size;
+	return (in < out) ? (out - in - 1) : (qsize + out - in - 1);
+}
+
+static inline struct dpram_rxb *rxbq_get_free_rxb(struct dpram_rxb_queue *rxbq)
+{
+	struct dpram_rxb *rxb = NULL;
+
+	if (likely(rxbq_free_size(rxbq) > 0)) {
+		rxb = &rxbq->rxb[rxbq->in];
+		rxbq->in++;
+		if (rxbq->in >= rxbq->size)
+			rxbq->in -= rxbq->size;
+		rxb->data = rxb->buff;
+	}
+
+	return rxb;
+}
+
+static inline int rxbq_size(struct dpram_rxb_queue *rxbq)
+{
+	int in = rxbq->in;
+	int out = rxbq->out;
+	int qsize = rxbq->size;
+	return (in >= out) ? (in - out) : (qsize - out + in);
+}
+
+static inline struct dpram_rxb *rxbq_get_data_rxb(struct dpram_rxb_queue *rxbq)
+{
+	struct dpram_rxb *rxb = NULL;
+
+	if (likely(!rxbq_empty(rxbq))) {
+		rxb = &rxbq->rxb[rxbq->out];
+		rxbq->out++;
+		if (rxbq->out >= rxbq->size)
+			rxbq->out -= rxbq->size;
+	}
+
+	return rxb;
+}
+
+static inline u8 *rxb_put(struct dpram_rxb *rxb, unsigned len)
+{
+	rxb->len = len;
+	return rxb->data;
+}
+
+static inline void rxb_clear(struct dpram_rxb *rxb)
+{
+	rxb->data = NULL;
+	rxb->len = 0;
+}
+
+static int dpram_register_isr(unsigned irq, irqreturn_t (*isr)(int, void*),
+		unsigned long flag, const char *name, struct link_device *ld)
+{
+	int ret = 0;
+
+	ret = request_irq(irq, isr, flag, name, ld);
+	if (ret) {
+		mif_info("%s: ERR! request_irq fail (err %d)\n", name, ret);
+		return ret;
+	}
+
+	ret = enable_irq_wake(irq);
+	if (ret)
+		mif_info("%s: ERR! enable_irq_wake fail (err %d)\n", name, ret);
+
+	mif_info("%s: IRQ#%d handler registered\n", name, irq);
+
+	return 0;
+}
+
+/*
+** CAUTION : dpram_allow_sleep() MUST be invoked after dpram_wake_up() success
+*/
+static int dpram_wake_up(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+
+	if (!dpld->dpctl->wakeup)
+		return 0;
+
+	if (dpld->dpctl->wakeup() < 0) {
+		mif_info("%s: ERR! <%pF> DPRAM wakeup fail\n",
+			ld->name, __builtin_return_address(0));
+		return -EACCES;
+	}
+	atomic_inc(&dpld->accessing);
+	return 0;
+}
+
+static void dpram_allow_sleep(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+
+	if (!dpld->dpctl->sleep)
+		return;
+
+	if (atomic_dec_return(&dpld->accessing) <= 0) {
+		dpld->dpctl->sleep();
+		atomic_set(&dpld->accessing, 0);
+		mif_debug("%s: DPRAM sleep possible\n", ld->name);
+	}
+}
+
+static int dpram_check_access(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+	int i;
+	u16 magic = dpld->dpctl->get_magic();
+	u16 access = dpld->dpctl->get_access();
+
+	if (likely(magic == DPRAM_MAGIC_CODE && access == 1))
+		return 0;
+
+	for (i = 1; i <= 10; i++) {
+		mif_info("%s: ERR! magic:%X access:%X -> retry:%d\n",
+			ld->name, magic, access, i);
+		mdelay(1);
+
+		magic = dpld->dpctl->get_magic();
+		access = dpld->dpctl->get_access();
+		if (likely(magic == DPRAM_MAGIC_CODE && access == 1))
+			return 0;
+	}
+
+	mif_info("%s: !CRISIS! magic:%X access:%X\n", ld->name, magic, access);
+	return -EACCES;
+}
+
+static bool dpram_ipc_active(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+
+	/* Check DPRAM mode */
+	if (ld->mode != LINK_MODE_IPC) {
+		mif_info("%s: ERR! <%pF> ld->mode != LINK_MODE_IPC\n",
+			ld->name, __builtin_return_address(0));
+		return false;
+	}
+
+	if (dpram_check_access(dpld) < 0) {
+		mif_info("%s: ERR! <%pF> dpram_check_access fail\n",
+			ld->name, __builtin_return_address(0));
+		return false;
+	}
+
+	return true;
+}
+
+static inline bool dpram_circ_valid(u32 size, u32 in, u32 out)
+{
+	if (in >= size)
+		return false;
+
+	if (out >= size)
+		return false;
+
+	return true;
+}
+
+/* get the size of the TXQ */
+static inline int dpram_get_txq_size(struct dpram_link_device *dpld, int dev)
+{
+	return dpld->dpctl->get_tx_buff_size(dev);
+}
+
+/* get in & out pointers of the TXQ */
+static inline void dpram_get_txq_ptrs(struct dpram_link_device *dpld, int dev,
+		u32 *in, u32 *out)
+{
+	*in = dpld->dpctl->get_tx_head(dev);
+	*out = dpld->dpctl->get_tx_tail(dev);
+}
+
+/* get free space in the TXQ as well as in & out pointers */
+static inline int dpram_get_txq_space(struct dpram_link_device *dpld, int dev,
+		u32 qsize, u32 *in, u32 *out)
+{
+	struct link_device *ld = &dpld->ld;
+
+	*in = dpld->dpctl->get_tx_head(dev);
+	*out = dpld->dpctl->get_tx_tail(dev);
+
+	if (!dpram_circ_valid(qsize, *in, *out)) {
+		mif_info("%s: ERR! <%pF> "
+			"%s_TXQ invalid (size:%d in:%d out:%d)\n",
+			ld->name, __builtin_return_address(0),
+			get_dev_name(dev), qsize, *in, *out);
+		dpld->dpctl->set_tx_head(dev, 0);
+		dpld->dpctl->set_tx_tail(dev, 0);
+		*in = 0;
+		*out = 0;
+		return -EINVAL;
+	}
+
+	return (*in < *out) ? (*out - *in - 1) : (qsize + *out - *in - 1);
+}
+
+static void dpram_ipc_write(struct dpram_link_device *dpld, int dev,
+		u32 qsize, u32 in, u32 out, struct sk_buff *skb)
+{
+	struct link_device *ld = &dpld->ld;
+	struct modemlink_dpram_control *dpctl = dpld->dpctl;
+	struct io_device *iod = skbpriv(skb)->iod;
+	u8 __iomem *dst = dpctl->get_tx_buff(dev);
+	u8 *src = skb->data;
+	u32 len = skb->len;
+
+	/* check queue status */
+	mif_debug("%s: {FMT %u %u %u %u} {RAW %u %u %u %u} ...\n", ld->name,
+		dpctl->get_tx_head(IPC_FMT), dpctl->get_tx_tail(IPC_FMT),
+		dpctl->get_rx_head(IPC_FMT), dpctl->get_rx_tail(IPC_FMT),
+		dpctl->get_tx_head(IPC_RAW), dpctl->get_tx_tail(IPC_RAW),
+		dpctl->get_rx_head(IPC_RAW), dpctl->get_rx_tail(IPC_RAW));
+
+	if (dev == IPC_FMT) {
+		mif_ipc_log(ld->mc, MIF_LNK_TX_EVT, iod, ld, src, len);
+		mif_flush_logs(ld->mc);
+	}
+
+	if (in < out) {
+		/* +++++++++ in ---------- out ++++++++++ */
+		memcpy((dst + in), src, len);
+	} else {
+		/* ------ out +++++++++++ in ------------ */
+		u32 space = qsize - in;
+
+		/* 1) in -> buffer end */
+		memcpy((dst + in), src, ((len > space) ? space : len));
+
+		/* 2) buffer start -> out */
+		if (len > space)
+			memcpy(dst, (src + space), (len - space));
+	}
+
+	/* update new in pointer */
+	in += len;
+	if (in >= qsize)
+		in -= qsize;
+	dpctl->set_tx_head(dev, in);
+}
+
+static int dpram_try_ipc_tx(struct dpram_link_device *dpld, int dev)
+{
+	struct link_device *ld = &dpld->ld;
+	struct modemlink_dpram_control *dpctl = dpld->dpctl;
+	struct sk_buff_head *txq = ld->skb_txq[dev];
+	struct sk_buff *skb;
+	u32 qsize = dpram_get_txq_size(dpld, dev);
+	u32 in;
+	u32 out;
+	int space;
+	int copied = 0;
+	u16 mask = 0;
+	unsigned long int flags;
+
+	while (1) {
+		skb = skb_dequeue(txq);
+		if (unlikely(!skb))
+			break;
+
+		space = dpram_get_txq_space(dpld, dev, qsize, &in, &out);
+		if (unlikely(space < 0)) {
+			skb_queue_head(txq, skb);
+			return -ENOSPC;
+		}
+
+		if (unlikely(space < skb->len)) {
+			atomic_set(&dpld->res_required[dev], 1);
+			skb_queue_head(txq, skb);
+			mask = dpctl->get_mask_req_ack(dev);
+			mif_info("%s: %s "
+				"qsize[%u] in[%u] out[%u] free[%u] < len[%u]\n",
+				ld->name, get_dev_name(dev),
+				qsize, in, out, space, skb->len);
+			break;
+		}
+
+		/* TX if there is enough room in the queue
+		*/
+		mif_debug("%s: %s "
+			"qsize[%u] in[%u] out[%u] free[%u] >= len[%u]\n",
+			ld->name, get_dev_name(dev),
+			qsize, in, out, space, skb->len);
+
+		spin_lock_irqsave(&dpld->tx_lock, flags);
+		dpram_ipc_write(dpld, dev, qsize, in, out, skb);
+		spin_unlock_irqrestore(&dpld->tx_lock, flags);
+
+		copied += skb->len;
+
+		dev_kfree_skb_any(skb);
+	}
+
+	if (mask)
+		return -ENOSPC;
+	else
+		return copied;
+}
+
+static void dpram_trigger_crash(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+	struct io_device *iod;
+	int i;
+
+	for (i = 0; i < dpld->max_ipc_dev; i++) {
+		mif_info("%s: purging %s_skb_txq\b", ld->name, get_dev_name(i));
+		skb_queue_purge(ld->skb_txq[i]);
+	}
+
+	ld->mode = LINK_MODE_ULOAD;
+
+	iod = link_get_iod_with_format(ld, IPC_FMT);
+	iod->modem_state_changed(iod, STATE_CRASH_EXIT);
+
+	iod = link_get_iod_with_format(ld, IPC_BOOT);
+	iod->modem_state_changed(iod, STATE_CRASH_EXIT);
+
+	iod = link_get_iod_with_channel(ld, PS_DATA_CH_0);
+	if (iod)
+		iodevs_for_each(&iod->mc->commons, iodev_netif_stop, 0);
+}
+
+static int dpram_trigger_force_cp_crash(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+	int ret;
+	int cnt = 5000;
+
+	mif_info("%s\n", ld->name);
+
+	dpld->dpctl->send_intr(INT_CMD(INT_CMD_CRASH_EXIT));
+
+	while (cnt--) {
+		ret = try_wait_for_completion(&dpld->crash_start_complete);
+		if (ret)
+			break;
+		udelay(1000);
+	}
+
+	if (!ret) {
+		mif_info("%s: ERR! No CRASH_EXIT ACK from CP\n", ld->name);
+		dpram_trigger_crash(dpld);
+	}
+
+	return 0;
+}
+
+static void dpram_ipc_rx_task(unsigned long data)
+{
+	struct link_device *ld;
+	struct dpram_link_device *dpld;
+	struct dpram_rxb *rxb;
+	struct io_device *iod;
+	u32 qlen;
+	int i;
+
+	dpld = (struct dpram_link_device *)data;
+	ld = &dpld->ld;
+
+	for (i = 0; i < dpld->max_ipc_dev; i++) {
+		if (i == IPC_RAW)
+			iod = link_get_iod_with_format(ld, IPC_MULTI_RAW);
+		else
+			iod = link_get_iod_with_format(ld, i);
+
+		qlen = rxbq_size(&dpld->rxbq[i]);
+		while (qlen > 0) {
+			rxb = rxbq_get_data_rxb(&dpld->rxbq[i]);
+			iod->recv(iod, ld, rxb->data, rxb->len);
+			rxb_clear(rxb);
+			qlen--;
+		}
+	}
+}
+
+static void dpram_ipc_read(struct dpram_link_device *dpld, int dev, u8 *dst,
+	u8 __iomem *src, u32 out, u32 len, u32 qsize)
+{
+	if ((out + len) <= qsize) {
+		/* ----- (out)         (in) ----- */
+		/* -----   7f 00 00 7e      ----- */
+		memcpy(dst, (src + out), len);
+	} else {
+		/*       (in) ----------- (out)   */
+		/* 00 7e      -----------   7f 00 */
+		unsigned len1 = qsize - out;
+
+		/* 1) out -> buffer end */
+		memcpy(dst, (src + out), len1);
+
+		/* 2) buffer start -> in */
+		dst += len1;
+		memcpy(dst, src, (len - len1));
+	}
+}
+
+/*
+  ret < 0  : error
+  ret == 0 : no data
+  ret > 0  : valid data
+*/
+static int dpram_ipc_recv_data(struct dpram_link_device *dpld, int dev,
+				u16 non_cmd)
+{
+	struct modemlink_dpram_control *dpctl = dpld->dpctl;
+	struct link_device *ld = &dpld->ld;
+	struct dpram_rxb *rxb;
+	u8 __iomem *src = dpctl->get_rx_buff(dev);
+	u32 in = dpctl->get_rx_head(dev);
+	u32 out = dpctl->get_rx_tail(dev);
+	u32 qsize = dpctl->get_rx_buff_size(dev);
+	u32 rcvd = 0;
+
+	if (in == out)
+		return 0;
+
+	if (dev == IPC_FMT)
+		log_dpram_irq(dpld, non_cmd);
+
+	/* Get data length in DPRAM*/
+	rcvd = (in > out) ? (in - out) : (qsize - out + in);
+
+	mif_debug("%s: %s qsize[%u] in[%u] out[%u] rcvd[%u]\n",
+		ld->name, get_dev_name(dev), qsize, in, out, rcvd);
+
+	/* Check each queue */
+	if (!dpram_circ_valid(qsize, in, out)) {
+		mif_info("%s: ERR! %s_RXQ invalid (size:%d in:%d out:%d)\n",
+			ld->name, get_dev_name(dev), qsize, in, out);
+		dpctl->set_rx_head(dev, 0);
+		dpctl->set_rx_tail(dev, 0);
+		return -EINVAL;
+	}
+
+	/* Allocate an rxb */
+	rxb = rxbq_get_free_rxb(&dpld->rxbq[dev]);
+	if (!rxb) {
+		mif_info("%s: ERR! %s rxbq_get_free_rxb fail\n",
+			ld->name, get_dev_name(dev));
+		return -ENOMEM;
+	}
+
+	/* Read data from each DPRAM buffer */
+	dpram_ipc_read(dpld, dev, rxb_put(rxb, rcvd), src, out, rcvd, qsize);
+
+	/* Calculate and set new out */
+	out += rcvd;
+	if (out >= qsize)
+		out -= qsize;
+	dpctl->set_rx_tail(dev, out);
+
+	return rcvd;
+}
+
+static void dpram_purge_rx_circ(struct dpram_link_device *dpld, int dev)
+{
+	u32 in = dpld->dpctl->get_rx_head(dev);
+	dpld->dpctl->set_rx_tail(dev, in);
+}
+
+static void non_command_handler(struct dpram_link_device *dpld, u16 non_cmd)
+{
+	struct modemlink_dpram_control *dpctl = dpld->dpctl;
+	struct link_device *ld = &dpld->ld;
+	struct sk_buff_head *txq;
+	struct sk_buff *skb;
+	int i;
+	int ret = 0;
+	int copied = 0;
+	u32 in;
+	u32 out;
+	u16 mask = 0;
+	u16 req_mask = 0;
+	u16 tx_mask = 0;
+
+	if (!dpram_ipc_active(dpld))
+		return;
+
+	/* Read data from DPRAM */
+	for (i = 0; i < dpld->max_ipc_dev; i++) {
+		ret = dpram_ipc_recv_data(dpld, i, non_cmd);
+		if (ret < 0)
+			dpram_purge_rx_circ(dpld, i);
+
+		/* Check and process REQ_ACK (at this time, in == out) */
+		if (non_cmd & dpctl->get_mask_req_ack(i)) {
+			mif_debug("%s: send %s_RES_ACK\n",
+				ld->name, get_dev_name(i));
+			mask = dpctl->get_mask_res_ack(i);
+			dpctl->send_intr(INT_NON_CMD(mask));
+		}
+	}
+
+	/* Schedule soft IRQ for RX */
+	tasklet_hi_schedule(&dpld->rx_tsk);
+
+	/* Try TX via DPRAM */
+	for (i = 0; i < dpld->max_ipc_dev; i++) {
+		if (atomic_read(&dpld->res_required[i]) > 0) {
+			dpram_get_txq_ptrs(dpld, i, &in, &out);
+			if (likely(in == out)) {
+				ret = dpram_try_ipc_tx(dpld, i);
+				if (ret > 0) {
+					atomic_set(&dpld->res_required[i], 0);
+					tx_mask |= dpctl->get_mask_send(i);
+				} else {
+					req_mask |= dpctl->get_mask_req_ack(i);
+				}
+			} else {
+				req_mask |= dpctl->get_mask_req_ack(i);
+			}
+		}
+	}
+
+	if (req_mask || tx_mask) {
+		tx_mask |= req_mask;
+		dpctl->send_intr(INT_NON_CMD(tx_mask));
+		mif_debug("%s: send intr 0x%04X\n", ld->name, tx_mask);
+	}
+}
+
+static int dpram_init_ipc(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+	struct modemlink_dpram_control *dpctl = dpld->dpctl;
+	int i;
+
+	if (ld->mode == LINK_MODE_IPC &&
+	    dpctl->get_magic() == DPRAM_MAGIC_CODE &&
+	    dpctl->get_access() == 1)
+		mif_info("%s: IPC already initialized\n", ld->name);
+
+	/* Clear pointers in every circular queue */
+	for (i = 0; i < dpld->max_ipc_dev; i++) {
+		dpctl->set_tx_head(i, 0);
+		dpctl->set_tx_tail(i, 0);
+		dpctl->set_rx_head(i, 0);
+		dpctl->set_rx_tail(i, 0);
+	}
+
+	/* Enable IPC */
+	dpctl->set_magic(DPRAM_MAGIC_CODE);
+	dpctl->set_access(1);
+	if (dpctl->get_magic() != DPRAM_MAGIC_CODE || dpctl->get_access() != 1)
+		return -EACCES;
+
+	ld->mode = LINK_MODE_IPC;
+
+	for (i = 0; i < dpld->max_ipc_dev; i++)
+		atomic_set(&dpld->res_required[i], 0);
+
+	atomic_set(&dpld->accessing, 0);
+
+	return 0;
+}
+
+static void cmd_req_active_handler(struct dpram_link_device *dpld)
+{
+	dpld->dpctl->send_intr(INT_CMD(INT_CMD_RES_ACTIVE));
+}
+
+static void cmd_crash_reset_handler(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+	struct io_device *iod = NULL;
+
+	mif_info("%s: Recv 0xC7 (CRASH_RESET)\n", ld->name);
+
+	ld->mode = LINK_MODE_ULOAD;
+
+	iod = link_get_iod_with_format(ld, IPC_FMT);
+	iod->modem_state_changed(iod, STATE_CRASH_RESET);
+
+	iod = link_get_iod_with_format(ld, IPC_BOOT);
+	iod->modem_state_changed(iod, STATE_CRASH_RESET);
+}
+
+static void cmd_crash_exit_handler(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+
+	mif_info("%s: Recv 0xC9 (CRASH_EXIT)\n", ld->name);
+
+	ld->mode = LINK_MODE_ULOAD;
+
+	complete_all(&dpld->crash_start_complete);
+
+	if (ld->mdm_data->modem_type == QC_MDM6600) {
+		if (dpld->dpctl->log_disp)
+			dpld->dpctl->log_disp(dpld->dpctl);
+	}
+
+	dpram_trigger_crash(dpld);
+}
+
+static void cmd_phone_start_handler(struct dpram_link_device *dpld)
+{
+	struct link_device *ld = &dpld->ld;
+	struct io_device *iod = NULL;
+
+	mif_info("%s: Recv 0xC8 (CP_START)\n", ld->name);
+
+	dpram_init_ipc(dpld);
+
+	iod = link_get_iod_with_format(ld, IPC_FMT);
+	if (!iod) {
+		mif_info("%s: ERR! no iod\n", ld->name);
+		return;
+	}
+
+	if (ld->mdm_data->modem_type == SEC_CMC221) {
+		if (ld->mc->phone_state != STATE_ONLINE) {
+			mif_info("%s: phone_state: %d -> ONLINE\n",
+				ld->name, ld->mc->phone_state);
+			iod->modem_state_changed(iod, STATE_ONLINE);
+		}
+	} else if (ld->mdm_data->modem_type == QC_MDM6600) {
+		if (dpld->dpctl->phone_boot_start_handler)
+			dpld->dpctl->phone_boot_start_handler(dpld->dpctl);
+	}
+
+	mif_info("%s: Send 0xC2 (INIT_END)\n", ld->name);
+	dpld->dpctl->send_intr(INT_CMD(INT_CMD_INIT_END));
+}
+
+static void command_handler(struct dpram_link_device *dpld, u16 cmd)
+{
+	struct link_device *ld = &dpld->ld;
+
+	switch (INT_CMD_MASK(cmd)) {
+	case INT_CMD_REQ_ACTIVE:
+		cmd_req_active_handler(dpld);
+		break;
+
+	case INT_CMD_CRASH_RESET:
+		dpld->dpram_init_status = DPRAM_INIT_STATE_NONE;
+		cmd_crash_reset_handler(dpld);
+		break;
+
+	case INT_CMD_CRASH_EXIT:
+		dpld->dpram_init_status = DPRAM_INIT_STATE_NONE;
+		cmd_crash_exit_handler(dpld);
+		break;
+
+	case INT_CMD_PHONE_START:
+		dpld->dpram_init_status = DPRAM_INIT_STATE_READY;
+		cmd_phone_start_handler(dpld);
+		complete_all(&dpld->dpram_init_cmd);
+		break;
+
+	case INT_CMD_NV_REBUILDING:
+		mif_info("%s: NV_REBUILDING\n", ld->name);
+		break;
+
+	case INT_CMD_PIF_INIT_DONE:
+		complete_all(&dpld->modem_pif_init_done);
+		break;
+
+	case INT_CMD_SILENT_NV_REBUILDING:
+		mif_info("%s: SILENT_NV_REBUILDING\n", ld->name);
+		break;
+
+	case INT_CMD_NORMAL_PWR_OFF:
+		/*ToDo:*/
+		/*kernel_sec_set_cp_ack()*/;
+		break;
+
+	case INT_CMD_REQ_TIME_SYNC:
+	case INT_CMD_CP_DEEP_SLEEP:
+	case INT_CMD_EMER_DOWN:
+		break;
+
+	default:
+		mif_info("%s: unknown command 0x%04X\n", ld->name, cmd);
+	}
+}
+
+static void ext_command_handler(struct dpram_link_device *dpld, u16 cmd)
+{
+	struct link_device *ld = &dpld->ld;
+	u16 resp;
+
+	switch (EXT_CMD_MASK(cmd)) {
+	case EXT_CMD_SET_SPEED_LOW:
+		if (dpld->dpctl->setup_speed) {
+			dpld->dpctl->setup_speed(DPRAM_SPEED_LOW);
+			resp = INT_EXT_CMD(EXT_CMD_SET_SPEED_LOW);
+			dpld->dpctl->send_intr(resp);
+		}
+		break;
+
+	case EXT_CMD_SET_SPEED_MID:
+		if (dpld->dpctl->setup_speed) {
+			dpld->dpctl->setup_speed(DPRAM_SPEED_MID);
+			resp = INT_EXT_CMD(EXT_CMD_SET_SPEED_MID);
+			dpld->dpctl->send_intr(resp);
+		}
+		break;
+
+	case EXT_CMD_SET_SPEED_HIGH:
+		if (dpld->dpctl->setup_speed) {
+			dpld->dpctl->setup_speed(DPRAM_SPEED_HIGH);
+			resp = INT_EXT_CMD(EXT_CMD_SET_SPEED_HIGH);
+			dpld->dpctl->send_intr(resp);
+		}
+		break;
+
+	default:
+		mif_info("%s: unknown command 0x%04X\n", ld->name, cmd);
+		break;
+	}
+}
+
+static void cmc22x_idpram_enable_ipc(struct dpram_link_device *dpld)
+{
+	dpram_init_ipc(dpld);
+}
+
+static int cmc22x_idpram_wait_response(struct dpram_link_device *dpld, u32 resp)
+{
+	struct link_device *ld = &dpld->ld;
+	int count = 50000;
+	u32 rcvd = 0;
+
+	if (resp == CMC22x_CP_REQ_NV_DATA) {
+		while (1) {
+			rcvd = ioread32(dpld->bt_map.resp);
+			if (rcvd == resp)
+				break;
+
+			rcvd = dpld->dpctl->recv_msg();
+			if (rcvd == 0x9999) {
+				mif_info("%s: Invalid resp 0x%04X\n",
+					ld->name, rcvd);
+				panic("CP Crash ... BAD CRC in CP");
+			}
+
+			if (count-- < 0) {
+				mif_info("%s: Invalid resp 0x%08X\n",
+					ld->name, rcvd);
+				return -EAGAIN;
+			}
+
+			udelay(100);
+		}
+	} else {
+		while (1) {
+			rcvd = dpld->dpctl->recv_msg();
+
+			if (rcvd == resp)
+				break;
+
+			if (resp == CMC22x_CP_RECV_NV_END &&
+			    rcvd == CMC22x_CP_CAL_BAD) {
+				mif_info("%s: CMC22x_CP_CAL_BAD\n", ld->name);
+				break;
+			}
+
+			if (count-- < 0) {
+				mif_info("%s: Invalid resp 0x%04X\n",
+					ld->name, rcvd);
+				return -EAGAIN;
+			}
+
+			udelay(100);
+		}
+	}
+
+	return rcvd;
+}
+
+static int cmc22x_idpram_send_boot(struct link_device *ld, unsigned long arg)
+{
+	int err = 0;
+	int cnt = 0;
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+	u8 __iomem *bt_buff = dpld->bt_map.buff;
+	struct dpram_boot_img cp_img;
+	u8 *img_buff = NULL;
+
+	ld->mode = LINK_MODE_BOOT;
+
+	dpld->dpctl->setup_speed(DPRAM_SPEED_LOW);
+
+	/* Test memory... After testing, memory is cleared. */
+	if (test_dpram(ld->name, bt_buff, dpld->bt_map.size) < 0) {
+		mif_info("%s: ERR! test_dpram fail!\n", ld->name);
+		ld->mode = LINK_MODE_INVALID;
+		return -EIO;
+	}
+
+	/* Get information about the boot image */
+	err = copy_from_user((struct dpram_boot_img *)&cp_img, (void *)arg,
+			     sizeof(struct dpram_boot_img));
+	mif_info("%s: CP image addr = 0x%08X, size = %d\n",
+		ld->name, (int)cp_img.addr, cp_img.size);
+
+	/* Alloc a buffer for the boot image */
+	img_buff = kzalloc(dpld->bt_map.size, GFP_KERNEL);
+	if (!img_buff) {
+		mif_info("%s: ERR! kzalloc fail\n", ld->name);
+		ld->mode = LINK_MODE_INVALID;
+		return -ENOMEM;
+	}
+
+	/* Copy boot image from the user space to the image buffer */
+	err = copy_from_user(img_buff, cp_img.addr, cp_img.size);
+
+	/* Copy boot image to DPRAM and verify it */
+	memcpy(bt_buff, img_buff, cp_img.size);
+	if (memcmp16_to_io(bt_buff, img_buff, cp_img.size)) {
+		mif_info("%s: ERR! Boot may be broken!!!\n", ld->name);
+		goto err;
+	}
+
+	dpld->dpctl->reset();
+	udelay(1000);
+
+	if (cp_img.mode == CMC22x_BOOT_MODE_NORMAL) {
+		mif_info("%s: CMC22x_BOOT_MODE_NORMAL\n", ld->name);
+		mif_info("%s: Send req 0x%08X\n", ld->name, cp_img.req);
+		iowrite32(cp_img.req, dpld->bt_map.req);
+
+		/* Wait for cp_img.resp for 1 second */
+		mif_info("%s: Wait resp 0x%08X\n", ld->name, cp_img.resp);
+		while (ioread32(dpld->bt_map.resp) != cp_img.resp) {
+			cnt++;
+			msleep_interruptible(10);
+			if (cnt > 100) {
+				mif_info("%s: ERR! Invalid resp 0x%08X\n",
+					ld->name, ioread32(dpld->bt_map.resp));
+				goto err;
+			}
+		}
+	} else {
+		mif_info("%s: CMC22x_BOOT_MODE_DUMP\n", ld->name);
+	}
+
+	kfree(img_buff);
+
+	mif_info("%s: Send BOOT done\n", ld->name);
+
+	if (dpld->dpctl->setup_speed)
+		dpld->dpctl->setup_speed(DPRAM_SPEED_HIGH);
+
+	return 0;
+
+err:
+	ld->mode = LINK_MODE_INVALID;
+	kfree(img_buff);
+
+	mif_info("%s: ERR! Boot send fail!!!\n", ld->name);
+	return -EIO;
+}
+
+static int cmc22x_idpram_send_main(struct link_device *ld, struct sk_buff *skb)
+{
+	int err = 0;
+	int ret = 0;
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+	struct dpram_boot_frame *bf = (struct dpram_boot_frame *)skb->data;
+	u8 __iomem *buff = (dpld->bt_map.buff + bf->offset);
+
+	if ((bf->offset + bf->len) > dpld->bt_map.size) {
+		mif_info("%s: ERR! Out of DPRAM boundary\n", ld->name);
+		err = -EINVAL;
+		goto exit;
+	}
+
+	if (bf->len)
+		memcpy(buff, bf->data, bf->len);
+
+	if (bf->request)
+		dpld->dpctl->send_msg((u16)bf->request);
+
+	if (bf->response) {
+		err = cmc22x_idpram_wait_response(dpld, bf->response);
+		if (err < 0)
+			mif_info("%s: ERR! wait_response fail (err %d)\n",
+				ld->name, err);
+	}
+
+	if (bf->request == CMC22x_CAL_NV_DOWN_END) {
+		mif_info("%s: CMC22x_CAL_NV_DOWN_END\n", ld->name);
+		cmc22x_idpram_enable_ipc(dpld);
+	}
+
+exit:
+	if (err < 0)
+		ret = err;
+	else
+		ret = skb->len;
+
+	dev_kfree_skb_any(skb);
+
+	return ret;
+}
+
+static void cmc22x_idpram_wait_dump(unsigned long arg)
+{
+	struct dpram_link_device *dpld = (struct dpram_link_device *)arg;
+	u16 msg;
+
+	if (!dpld) {
+		mif_info("ERR! dpld == NULL\n");
+		return;
+	}
+
+	msg = dpld->dpctl->recv_msg();
+
+	if (msg == CMC22x_CP_DUMP_END) {
+		complete_all(&dpld->dump_recv_done);
+		return;
+	}
+
+	if (((dpld->dump_rcvd & 0x1) == 0) && (msg == CMC22x_1ST_BUFF_FULL)) {
+		complete_all(&dpld->dump_recv_done);
+		return;
+	}
+
+	if (((dpld->dump_rcvd & 0x1) == 1) && (msg == CMC22x_2ND_BUFF_FULL)) {
+		complete_all(&dpld->dump_recv_done);
+		return;
+	}
+
+	mif_add_timer(&dpld->dump_timer, CMC22x_DUMP_WAIT_TIMEOVER,
+		cmc22x_idpram_wait_dump, (unsigned long)dpld);
+}
+
+static int cmc22x_idpram_upload(struct dpram_link_device *dpld,
+		struct dpram_dump_arg *dumparg)
+{
+	struct link_device *ld = &dpld->ld;
+	int ret;
+	u8 __iomem *src;
+	int buff_size = CMC22x_DUMP_BUFF_SIZE;
+
+	if ((dpld->dump_rcvd & 0x1) == 0)
+		dpld->dpctl->send_msg(CMC22x_1ST_BUFF_READY);
+	else
+		dpld->dpctl->send_msg(CMC22x_2ND_BUFF_READY);
+
+	init_completion(&dpld->dump_recv_done);
+
+	mif_add_timer(&dpld->dump_timer, CMC22x_DUMP_WAIT_TIMEOVER,
+		cmc22x_idpram_wait_dump, (unsigned long)dpld);
+
+	ret = wait_for_completion_interruptible_timeout(
+			&dpld->dump_recv_done, DUMP_TIMEOUT);
+	if (!ret) {
+		mif_info("%s: ERR! CP didn't send dump data!!!\n", ld->name);
+		goto err_out;
+	}
+
+	if (dpld->dpctl->recv_msg() == CMC22x_CP_DUMP_END) {
+		mif_info("%s: CMC22x_CP_DUMP_END\n", ld->name);
+		wake_unlock(&dpld->dpram_wake_lock);
+		return 0;
+	}
+
+	if ((dpld->dump_rcvd & 0x1) == 0)
+		src = dpld->ul_map.buff;
+	else
+		src = dpld->ul_map.buff + CMC22x_DUMP_BUFF_SIZE;
+
+	memcpy(dpld->buff, src, buff_size);
+
+	ret = copy_to_user(dumparg->buff, dpld->buff, buff_size);
+	if (ret < 0) {
+		mif_info("%s: ERR! copy_to_user fail\n", ld->name);
+		goto err_out;
+	}
+
+	dpld->dump_rcvd++;
+	return buff_size;
+
+err_out:
+	wake_unlock(&dpld->dpram_wake_lock);
+	return -EIO;
+}
+
+static int cbp72_edpram_wait_response(struct dpram_link_device *dpld, u32 resp)
+{
+	struct link_device *ld = &dpld->ld;
+	int ret;
+	int int2cp;
+
+	ret = wait_for_completion_interruptible_timeout(
+			&dpld->udl_cmd_complete, UDL_TIMEOUT);
+	if (!ret) {
+		mif_info("%s: ERR! No UDL_CMD_RESP!!!\n", ld->name);
+		return -ENXIO;
+	}
+
+	int2cp = dpld->dpctl->recv_intr();
+	mif_debug("%s: int2cp = 0x%x\n", ld->name, int2cp);
+	if (resp == int2cp || int2cp == 0xA700)
+		return int2cp;
+	else
+		return -EINVAL;
+}
+
+static int cbp72_edpram_send_bin(struct link_device *ld, struct sk_buff *skb)
+{
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+	struct dpram_boot_frame *bf = (struct dpram_boot_frame *)skb->data;
+	u8 __iomem *buff = dpld->bt_map.buff;
+	int err = 0;
+
+	if (bf->len > dpld->bt_map.size) {
+		mif_info("%s: ERR! Out of DPRAM boundary\n", ld->name);
+		err = -EINVAL;
+		goto exit;
+	}
+
+	if (bf->len)
+		memcpy(buff, bf->data, bf->len);
+
+	init_completion(&dpld->udl_cmd_complete);
+
+	if (bf->request)
+		dpld->dpctl->send_intr((u16)bf->request);
+
+	if (bf->response) {
+		err = cbp72_edpram_wait_response(dpld, bf->response);
+		if (err < 0) {
+			mif_info("%s: ERR! wait_response fail (%d)\n",
+				ld->name, err);
+			goto exit;
+		} else if (err == bf->response) {
+			err = skb->len;
+		}
+	}
+
+exit:
+	dev_kfree_skb_any(skb);
+	return err;
+}
+
+static int dpram_upload(struct dpram_link_device *dpld,
+		struct dpram_dump_arg *dump, unsigned char __user *target)
+{
+	struct link_device *ld = &dpld->ld;
+	struct ul_header header;
+	u8 *dest;
+	u8 *buff = vmalloc(DP_DEFAULT_DUMP_LEN);
+	u16 plen = 0;
+	int err = 0;
+	int ret = 0;
+	int buff_size = 0;
+
+	mif_info("\n");
+
+	wake_lock(&dpld->dpram_wake_lock);
+	init_completion(&dpld->udl_cmd_complete);
+
+	mif_info("%s: req = %x, resp =%x", ld->name, dump->req, dump->resp);
+
+	if (dump->req)
+		dpld->dpctl->send_intr((u16)dump->req);
+
+	if (dump->resp) {
+		err = cbp72_edpram_wait_response(dpld, dump->resp);
+		if (err < 0) {
+			mif_info("%s: ERR! wait_response fail (%d)\n",
+				ld->name, err);
+			goto exit;
+		}
+	}
+
+	if (dump->cmd)
+		return err;
+
+	dest = (u8 *)dpld->ul_map.buff;
+
+	header.bop = *(u8 *)(dest);
+	header.total_frame = *(u16 *)(dest + 1);
+	header.curr_frame = *(u16 *)(dest + 3);
+	header.len = *(u16 *)(dest + 5);
+
+	mif_info("%s: total frame:%d, current frame:%d, data len:%d\n",
+		ld->name, header.total_frame, header.curr_frame, header.len);
+
+	plen = min_t(u16, header.len, DP_DEFAULT_DUMP_LEN);
+
+	memcpy(buff, dest + sizeof(struct ul_header), plen);
+	ret = copy_to_user(dump->buff, buff, plen);
+	if (ret < 0) {
+		mif_info("%s: copy_to_user fail\n", ld->name);
+		goto exit;
+	}
+	buff_size = plen;
+
+	ret = copy_to_user(target + 4, &buff_size, sizeof(int));
+	if (ret < 0) {
+		mif_info("%s: copy_to_user fail\n", ld->name);
+		goto exit;
+	}
+
+	wake_unlock(&dpld->dpram_wake_lock);
+
+	return err;
+
+exit:
+	vfree(buff);
+	iowrite32(0, dpld->ul_map.magic);
+	wake_unlock(&dpld->dpram_wake_lock);
+	return -EIO;
+}
+
+static void udl_cmd_handler(struct dpram_link_device *dpld, u16 cmd)
+{
+	struct link_device *ld = &dpld->ld;
+
+	if (cmd & UDL_RESULT_FAIL) {
+		mif_info("%s: ERR! Command failed: %04x\n", ld->name, cmd);
+		return;
+	}
+
+	switch (UDL_CMD_MASK(cmd)) {
+	case UDL_CMD_RECEIVE_READY:
+		mif_debug("%s: Send CP-->AP RECEIVE_READY\n", ld->name);
+		dpld->dpctl->send_intr(CMD_IMG_START_REQ);
+		break;
+	default:
+		complete_all(&dpld->udl_cmd_complete);
+	}
+}
+
+static irqreturn_t dpram_irq_handler(int irq, void *data)
+{
+	struct link_device *ld = (struct link_device *)data;
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+	u16 int2ap = 0;
+
+	if (!ld->mc || ld->mc->phone_state == STATE_OFFLINE)
+		return IRQ_HANDLED;
+
+	if (dpram_wake_up(dpld) < 0)
+		return IRQ_HANDLED;
+
+	int2ap = dpld->dpctl->recv_intr();
+
+	if (dpld->dpctl->clear_intr)
+		dpld->dpctl->clear_intr();
+
+	if (int2ap == INT_POWERSAFE_FAIL) {
+		mif_info("%s: int2ap == INT_POWERSAFE_FAIL\n", ld->name);
+		goto exit_isr;
+	}
+
+	if (ld->mdm_data->modem_type == QC_MDM6600) {
+		if ((int2ap == 0x1234)|(int2ap == 0xDBAB)|(int2ap == 0xABCD)) {
+			if (dpld->dpctl->dload_cmd_hdlr)
+				dpld->dpctl->dload_cmd_hdlr(dpld->dpctl,
+									int2ap);
+			goto exit_isr;
+		}
+	}
+
+	if (UDL_CMD_VALID(int2ap))
+		udl_cmd_handler(dpld, int2ap);
+	else if (EXT_INT_VALID(int2ap) && EXT_CMD_VALID(int2ap))
+		ext_command_handler(dpld, int2ap);
+	else if (INT_CMD_VALID(int2ap))
+		command_handler(dpld, int2ap);
+	else if (INT_VALID(int2ap))
+		non_command_handler(dpld, int2ap);
+	else
+		mif_info("%s: ERR! invalid intr 0x%04X\n", ld->name, int2ap);
+
+exit_isr:
+	dpram_allow_sleep(dpld);
+	return IRQ_HANDLED;
+}
+
+static void dpram_send_ipc(struct link_device *ld, int dev, struct sk_buff *skb)
+{
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+	struct sk_buff_head *txq;
+	int ret;
+	u16 mask;
+
+	if (unlikely(dev >= dpld->max_ipc_dev)) {
+		mif_info("%s: ERR! dev %d >= max_ipc_dev(%s)\n",
+			ld->name, dev, get_dev_name(dpld->max_ipc_dev));
+		return;
+	}
+
+	if (dpram_wake_up(dpld) < 0)
+		return;
+
+	if (!dpram_ipc_active(dpld))
+		goto exit;
+
+	txq = ld->skb_txq[dev];
+	if (txq->qlen > 1024)
+		mif_info("%s: txq->qlen %d > 1024\n", ld->name, txq->qlen);
+
+	skb_queue_tail(txq, skb);
+
+	if (atomic_read(&dpld->res_required[dev]) > 0) {
+		mif_debug("%s: %s_TXQ is full\n", ld->name, get_dev_name(dev));
+		goto exit;
+	}
+
+	ret = dpram_try_ipc_tx(dpld, dev);
+	if (ret > 0) {
+		mask = dpld->dpctl->get_mask_send(dev);
+		dpld->dpctl->send_intr(INT_NON_CMD(mask));
+	} else {
+		mask = dpld->dpctl->get_mask_req_ack(dev);
+		dpld->dpctl->send_intr(INT_NON_CMD(mask));
+		mif_info("%s: Send REQ_ACK 0x%04X\n", ld->name, mask);
+	}
+
+exit:
+	dpram_allow_sleep(dpld);
+}
+
+static int dpram_send_binary(struct link_device *ld, struct sk_buff *skb)
+{
+	int err = 0;
+
+	if (ld->mdm_data->modem_type == SEC_CMC221)
+		err = cmc22x_idpram_send_main(ld, skb);
+	else if (ld->mdm_data->modem_type == VIA_CBP72)
+		err = cbp72_edpram_send_bin(ld, skb);
+
+	return err;
+}
+
+static int dpram_send(struct link_device *ld, struct io_device *iod,
+		struct sk_buff *skb)
+{
+	enum dev_format fmt = iod->format;
+	int len = skb->len;
+
+	switch (fmt) {
+	case IPC_FMT:
+	case IPC_RAW:
+	case IPC_RFS:
+		if (likely(ld->mc->phone_state == STATE_ONLINE))
+			dpram_send_ipc(ld, fmt, skb);
+		return len;
+
+	case IPC_BOOT:
+		return dpram_send_binary(ld, skb);
+
+	default:
+		mif_info("%s: ERR! no TXQ for %s\n", ld->name, iod->name);
+		dev_kfree_skb_any(skb);
+		return -ENODEV;
+	}
+}
+
+static int dpram_set_dl_magic(struct link_device *ld, struct io_device *iod)
+{
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+
+	ld->mode = LINK_MODE_DLOAD;
+
+	iowrite32(DP_MAGIC_DMDL, dpld->dl_map.magic);
+
+	return 0;
+}
+
+static int dpram_force_dump(struct link_device *ld, struct io_device *iod)
+{
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+
+	mif_info("%s\n", ld->name);
+
+	if (dpram_wake_up(dpld) < 0)
+		mif_info("%s: WARNING! dpram_wake_up fail\n", ld->name);
+
+	dpram_trigger_force_cp_crash(dpld);
+
+	dpram_allow_sleep(dpld);
+
+	return 0;
+}
+
+static int dpram_set_ul_magic(struct link_device *ld, struct io_device *iod)
+{
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+	u8 *dest = dpld->ul_map.buff;
+
+	ld->mode = LINK_MODE_ULOAD;
+
+	if (ld->mdm_data->modem_type == SEC_CMC221) {
+		wake_lock(&dpld->dpram_wake_lock);
+		dpld->dump_rcvd = 0;
+		iowrite32(CMC22x_CP_DUMP_MAGIC, dpld->ul_map.magic);
+	} else {
+		iowrite32(DP_MAGIC_UMDL, dpld->ul_map.magic);
+
+		iowrite8((u8)START_INDEX, dest + 0);
+		iowrite8((u8)0x1, dest + 1);
+		iowrite8((u8)0x1, dest + 2);
+		iowrite8((u8)0x0, dest + 3);
+		iowrite8((u8)END_INDEX, dest + 4);
+	}
+
+	init_completion(&dpld->dump_start_complete);
+
+	return 0;
+}
+
+static int dpram_dump_update(struct link_device *ld, struct io_device *iod,
+		unsigned long arg)
+{
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+	struct dpram_dump_arg dump;
+	int ret;
+
+	ret = copy_from_user(&dump, (void __user *)arg, sizeof(dump));
+	if (ret  < 0) {
+		mif_info("%s: ERR! copy_from_user fail\n", ld->name);
+		return ret;
+	}
+
+	if (ld->mdm_data->modem_type == SEC_CMC221)
+		return cmc22x_idpram_upload(dpld, &dump);
+	else
+		return dpram_upload(dpld, &dump, (unsigned char __user *)arg);
+}
+
+static int dpram_link_ioctl(struct link_device *ld, struct io_device *iod,
+		unsigned int cmd, unsigned long arg)
+{
+	int err = -EFAULT;
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+
+	mif_info("%s: cmd 0x%08X\n", ld->name, cmd);
+
+	switch (cmd) {
+	case IOCTL_DPRAM_SEND_BOOT:
+		err = cmc22x_idpram_send_boot(ld, arg);
+		if (err < 0) {
+			mif_info("%s: ERR! dpram_send_boot fail\n", ld->name);
+			goto exit;
+		}
+		break;
+
+	case IOCTL_DPRAM_PHONE_POWON:
+		if (dpld->dpctl->cpimage_load_prepare) {
+			err = dpld->dpctl->cpimage_load_prepare(dpld->dpctl);
+			if (err < 0) {
+				mif_info("%s: ERR! cpimage_load_prepare fail\n",
+					ld->name);
+				goto exit;
+			}
+		}
+		break;
+
+	case IOCTL_DPRAM_PHONEIMG_LOAD:
+		if (dpld->dpctl->cpimage_load) {
+			err = dpld->dpctl->cpimage_load(
+					(void *)arg, dpld->dpctl);
+			if (err < 0) {
+				mif_info("%s: ERR! cpimage_load fail\n",
+					ld->name);
+				goto exit;
+			}
+		}
+		break;
+
+	case IOCTL_DPRAM_NVDATA_LOAD:
+		if (dpld->dpctl->nvdata_load) {
+			err = dpld->dpctl->nvdata_load(
+					(void *)arg, dpld->dpctl);
+			if (err < 0) {
+				mif_info("%s: ERR! nvdata_load fail\n",
+					ld->name);
+				goto exit;
+			}
+		}
+		break;
+
+	case IOCTL_DPRAM_PHONE_BOOTSTART:
+		if (dpld->dpctl->phone_boot_start) {
+			err = dpld->dpctl->phone_boot_start(dpld->dpctl);
+			if (err < 0) {
+				mif_info("%s: ERR! phone_boot_start fail\n",
+					ld->name);
+				goto exit;
+			}
+		}
+		if (dpld->dpctl->phone_boot_start_post_process) {
+			err = dpld->dpctl->phone_boot_start_post_process();
+			if (err < 0) {
+				mif_info("%s: ERR! "
+					"phone_boot_start_post_process fail\n",
+					ld->name);
+				goto exit;
+			}
+		}
+		break;
+
+	case IOCTL_DPRAM_PHONE_UPLOAD_STEP1:
+		disable_irq_nosync(dpld->irq);
+
+		if (dpld->dpctl->cpupload_step1) {
+			err = dpld->dpctl->cpupload_step1(dpld->dpctl);
+			if (err < 0) {
+				dpld->dpctl->clear_intr();
+				enable_irq(dpld->irq);
+				mif_info("%s: ERR! cpupload_step1 fail\n",
+					ld->name);
+				goto exit;
+			}
+		}
+		break;
+
+	case IOCTL_DPRAM_PHONE_UPLOAD_STEP2:
+		if (dpld->dpctl->cpupload_step2) {
+			err = dpld->dpctl->cpupload_step2(
+					(void *)arg, dpld->dpctl);
+			if (err < 0) {
+				dpld->dpctl->clear_intr();
+				enable_irq(dpld->irq);
+				mif_info("%s: ERR! cpupload_step2 fail\n",
+					ld->name);
+				goto exit;
+			}
+		}
+		break;
+
+	case IOCTL_DPRAM_INIT_STATUS:
+		mif_debug("%s: get dpram init status\n", ld->name);
+		return dpld->dpram_init_status;
+
+	case IOCTL_MODEM_DL_START:
+		err = dpram_set_dl_magic(ld, iod);
+		if (err < 0) {
+			mif_info("%s: ERR! dpram_set_dl_magic fail\n",
+				ld->name);
+			goto exit;
+		}
+
+	default:
+		break;
+	}
+
+	return 0;
+
+exit:
+	return err;
+}
+
+static void dpram_table_init(struct dpram_link_device *dpld)
+{
+	struct link_device *ld;
+	u8 __iomem *dp_base;
+
+	if (!dpld) {
+		mif_info("ERR! dpld == NULL\n");
+		return;
+	}
+	ld = &dpld->ld;
+
+	if (!dpld->dp_base) {
+		mif_info("%s: ERR! dpld->dp_base == NULL\n", ld->name);
+		return;
+	}
+
+	dp_base = dpld->dp_base;
+
+	/* Map for booting */
+	if (ld->mdm_data->modem_type == SEC_CMC221) {
+		dpld->bt_map.buff = (u8 *)(dp_base);
+		dpld->bt_map.req  = (u32 *)(dp_base + DP_BOOT_REQ_OFFSET);
+		dpld->bt_map.resp = (u32 *)(dp_base + DP_BOOT_RESP_OFFSET);
+		dpld->bt_map.size = dpld->dp_size;
+	} else if (ld->mdm_data->modem_type == QC_MDM6600) {
+		if (dpld->dpctl->bt_map_init)
+			dpld->dpctl->bt_map_init(dpld->dpctl);
+	} else if (ld->mdm_data->modem_type == VIA_CBP72) {
+		dpld->bt_map.magic = (u32 *)(dp_base);
+		dpld->bt_map.buff = (u8 *)(dp_base + DP_BOOT_BUFF_OFFSET);
+		dpld->bt_map.size = dpld->dp_size - 4;
+	} else {
+		dpld->bt_map.buff = (u8 *)(dp_base);
+		dpld->bt_map.req  = (u32 *)(dp_base + DP_BOOT_REQ_OFFSET);
+		dpld->bt_map.resp = (u32 *)(dp_base + DP_BOOT_RESP_OFFSET);
+		dpld->bt_map.size = dpld->dp_size - 4;
+	}
+
+	/* Map for download (FOTA, UDL, etc.) */
+	if (ld->mdm_data->modem_type == SEC_CMC221 ||
+			ld->mdm_data->modem_type == VIA_CBP72) {
+		dpld->dl_map.magic = (u32 *)(dp_base);
+		dpld->dl_map.buff  = (u8 *)(dp_base + DP_DLOAD_BUFF_OFFSET);
+	}
+
+	/* Map for upload mode */
+	dpld->ul_map.magic = (u32 *)(dp_base);
+	if (ld->mdm_data->modem_type == SEC_CMC221)
+		dpld->ul_map.buff = (u8 *)(dp_base);
+	else
+		dpld->ul_map.buff = (u8 *)(dp_base + DP_ULOAD_BUFF_OFFSET);
+}
+
+#if defined(CONFIG_MACH_M0_CTC)
+static void dpram_link_terminate(struct link_device *ld, struct io_device *iod)
+{
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+
+	mif_info("dpram_link_terminate\n");
+
+	if (dpld->dpctl->terminate_link)
+			dpld->dpctl->terminate_link(dpld->dpctl);
+}
+#endif
+
+struct link_device *dpram_create_link_device(struct platform_device *pdev)
+{
+	struct dpram_link_device *dpld = NULL;
+	struct link_device *ld = NULL;
+	struct modem_data *pdata = NULL;
+	struct modemlink_dpram_control *dpctl = NULL;
+	int ret = 0;
+	int i = 0;
+	int bsize;
+	int qsize;
+	char wq_name[32];
+	char wq_suffix[32];
+
+	/* Get the platform data */
+	pdata = (struct modem_data *)pdev->dev.platform_data;
+	if (!pdata) {
+		mif_info("ERR! pdata == NULL\n");
+		goto err;
+	}
+	if (!pdata->dpram_ctl) {
+		mif_info("ERR! pdata->dpram_ctl == NULL\n");
+		goto err;
+	}
+	mif_info("link device = %s\n", pdata->link_name);
+	mif_info("modem = %s\n", pdata->name);
+
+	/* Alloc DPRAM link device structure */
+	dpld = kzalloc(sizeof(struct dpram_link_device), GFP_KERNEL);
+	if (!dpld) {
+		mif_info("ERR! kzalloc dpld fail\n");
+		goto err;
+	}
+	ld = &dpld->ld;
+
+	/* Extract modem data and DPRAM control data from the platform data */
+	ld->mdm_data = pdata;
+	ld->name = pdata->link_name;
+	ld->ipc_version = pdata->ipc_version;
+
+	/* Set attributes as a link device */
+	ld->aligned = pdata->dpram_ctl->aligned;
+	if (ld->aligned)
+		mif_info("%s: ld->aligned == TRUE\n", ld->name);
+
+	ld->send = dpram_send;
+	ld->force_dump = dpram_force_dump;
+	ld->dump_start = dpram_set_ul_magic;
+	ld->dump_update = dpram_dump_update;
+	ld->ioctl = dpram_link_ioctl;
+
+#if defined(CONFIG_MACH_M0_CTC)
+	ld->terminate_comm = dpram_link_terminate;
+#endif
+	INIT_LIST_HEAD(&ld->list);
+
+	skb_queue_head_init(&ld->sk_fmt_tx_q);
+	skb_queue_head_init(&ld->sk_raw_tx_q);
+	skb_queue_head_init(&ld->sk_rfs_tx_q);
+	ld->skb_txq[IPC_FMT] = &ld->sk_fmt_tx_q;
+	ld->skb_txq[IPC_RAW] = &ld->sk_raw_tx_q;
+	ld->skb_txq[IPC_RFS] = &ld->sk_rfs_tx_q;
+
+	/* Set attributes as a dpram link device */
+	dpctl = pdata->dpram_ctl;
+	dpld->dpctl = dpctl;
+
+	dpld->dp_base = dpctl->dp_base;
+	dpld->dp_size = dpctl->dp_size;
+	dpld->dp_type = dpctl->dp_type;
+
+	dpld->max_ipc_dev = dpctl->max_ipc_dev;
+
+	dpld->irq = dpctl->dpram_irq;
+	if (dpld->irq < 0) {
+		mif_info("%s: ERR! failed to get IRQ#\n", ld->name);
+		goto err;
+	}
+	mif_info("%s: DPRAM IRQ# = %d\n", ld->name, dpld->irq);
+
+	wake_lock_init(&dpld->dpram_wake_lock, WAKE_LOCK_SUSPEND,
+			dpctl->dpram_wlock_name);
+
+	init_completion(&dpld->dpram_init_cmd);
+	init_completion(&dpld->modem_pif_init_done);
+	init_completion(&dpld->udl_start_complete);
+	init_completion(&dpld->udl_cmd_complete);
+	init_completion(&dpld->crash_start_complete);
+	init_completion(&dpld->dump_start_complete);
+	init_completion(&dpld->dump_recv_done);
+
+	spin_lock_init(&dpld->tx_lock);
+
+	tasklet_init(&dpld->rx_tsk, dpram_ipc_rx_task, (unsigned long)dpld);
+
+	/* Initialize DPRAM map (physical map -> logical map) */
+	dpram_table_init(dpld);
+
+#if 0
+	dpld->magic = dpctl->ipc_map->magic;
+	dpld->access = dpctl->ipc_map->access;
+	for (i = 0; i < dpld->max_ipc_dev; i++)
+		dpld->dev[i] = &dpctl->ipc_map->dev[i];
+	dpld->mbx2ap = dpctl->ipc_map->mbx_cp2ap;
+	dpld->mbx2cp = dpctl->ipc_map->mbx_ap2cp;
+#endif
+
+	/* Prepare rxb queue */
+	qsize = DPRAM_MAX_RXBQ_SIZE;
+	for (i = 0; i < dpld->max_ipc_dev; i++) {
+		bsize = rxbq_get_page_size(dpctl->get_rx_buff_size(i));
+		dpld->rxbq[i].size = qsize;
+		dpld->rxbq[i].in = 0;
+		dpld->rxbq[i].out = 0;
+		dpld->rxbq[i].rxb = rxbq_create_pool(bsize, qsize);
+		if (!dpld->rxbq[i].rxb) {
+			mif_info("%s: ERR! %s rxbq_create_pool fail\n",
+				ld->name, get_dev_name(i));
+			goto err;
+		}
+		mif_info("%s: %s rxbq_pool created (bsize:%d, qsize:%d)\n",
+			ld->name, get_dev_name(i), bsize, qsize);
+	}
+
+	/* Prepare a clean buffer */
+	dpld->buff = kzalloc(dpld->dp_size, GFP_KERNEL);
+	if (!dpld->buff) {
+		mif_info("%s: ERR! kzalloc dpld->buff fail\n", ld->name);
+		goto err;
+	}
+
+	if (ld->mdm_data->modem_type == QC_MDM6600) {
+		if (dpctl->load_init)
+			dpctl->load_init(dpctl);
+	}
+
+	/* Disable IPC */
+	dpctl->set_magic(0);
+	dpctl->set_access(0);
+	dpld->dpram_init_status = DPRAM_INIT_STATE_NONE;
+
+	/* Register DPRAM interrupt handler */
+	ret = dpram_register_isr(dpld->irq, dpram_irq_handler,
+			 dpctl->dpram_irq_flags, dpctl->dpram_irq_name, ld);
+	if (ret)
+		goto err;
+
+	return ld;
+
+err:
+	if (dpld) {
+		if (dpld->buff)
+			kfree(dpld->buff);
+		kfree(dpld);
+	}
+
+	return NULL;
+}
+
diff --git a/drivers/misc/modem_if/modem_link_device_dpram.h b/drivers/misc/modem_if/modem_link_device_dpram.h
new file mode 100644
index 0000000..d61da83
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_dpram.h
@@ -0,0 +1,379 @@
+/*
+ * Copyright (C) 2011 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+#ifndef __MODEM_LINK_DEVICE_DPRAM_H__
+#define __MODEM_LINK_DEVICE_DPRAM_H__
+
+#include <linux/spinlock.h>
+#include <linux/wakelock.h>
+#include <linux/workqueue.h>
+#include <linux/timer.h>
+#include <linux/platform_data/modem.h>
+
+#include "modem_prj.h"
+
+/* for DPRAM hostboot */
+#define CMC22x_AP_BOOT_DOWN_DONE	0x54329876
+#define CMC22x_CP_REQ_MAIN_BIN		0xA5A5A5A5
+#define CMC22x_CP_REQ_NV_DATA		0x5A5A5A5A
+#define CMC22x_CP_DUMP_MAGIC		0xDEADDEAD
+
+#define CMC22x_HOST_DOWN_START		0x1234
+#define CMC22x_HOST_DOWN_END		0x4321
+#define CMC22x_REG_NV_DOWN_END		0xABCD
+#define CMC22x_CAL_NV_DOWN_END		0xDCBA
+
+#define CMC22x_1ST_BUFF_READY		0xAAAA
+#define CMC22x_2ND_BUFF_READY		0xBBBB
+#define CMC22x_1ST_BUFF_FULL		0x1111
+#define CMC22x_2ND_BUFF_FULL		0x2222
+
+#define CMC22x_CP_RECV_NV_END		0x8888
+#define CMC22x_CP_CAL_OK		0x4F4B
+#define CMC22x_CP_CAL_BAD		0x4552
+#define CMC22x_CP_DUMP_END		0xFADE
+
+#define CMC22x_DUMP_BUFF_SIZE		8192	/* 8 KB */
+#define CMC22x_DUMP_WAIT_TIMEOVER	1	/* 1 ms */
+
+/* interrupt masks.*/
+#define INT_MASK_VALID	0x0080
+#define INT_MASK_CMD	0x0040
+#define INT_VALID(x)	((x) & INT_MASK_VALID)
+#define INT_CMD_VALID(x)	((x) & INT_MASK_CMD)
+#define INT_NON_CMD(x)	(INT_MASK_VALID | (x))
+#define INT_CMD(x)	(INT_MASK_VALID | INT_MASK_CMD | (x))
+
+#define EXT_INT_VALID_MASK	0x8000
+#define EXT_CMD_VALID_MASK	0x4000
+#define EXT_INT_VALID(x)	((x) & EXT_INT_VALID_MASK)
+#define EXT_CMD_VALID(x)	((x) & EXT_CMD_VALID_MASK)
+#define INT_EXT_CMD(x)		(EXT_INT_VALID_MASK | EXT_CMD_VALID_MASK | (x))
+
+#define INT_MASK_REQ_ACK_F	0x0020
+#define INT_MASK_REQ_ACK_R	0x0010
+#define INT_MASK_RES_ACK_F	0x0008
+#define INT_MASK_RES_ACK_R	0x0004
+#define INT_MASK_SEND_F		0x0002
+#define INT_MASK_SEND_R		0x0001
+
+#define INT_MASK_REQ_ACK_RFS	0x0400 /* Request RES_ACK_RFS		*/
+#define INT_MASK_RES_ACK_RFS	0x0200 /* Response of REQ_ACK_RFS	*/
+#define INT_MASK_SEND_RFS	0x0100 /* Indicate sending RFS data	*/
+
+#define INT_MASK_RES_ACK_SET \
+	(INT_MASK_RES_ACK_F | INT_MASK_RES_ACK_R | INT_MASK_RES_ACK_RFS)
+
+#define INT_MASK_SEND_SET \
+	(INT_MASK_SEND_F | INT_MASK_SEND_R | INT_MASK_SEND_RFS)
+
+#define INT_CMD_MASK(x)		((x) & 0xF)
+#define INT_CMD_INIT_START	0x1
+#define INT_CMD_INIT_END	0x2
+#define INT_CMD_REQ_ACTIVE	0x3
+#define INT_CMD_RES_ACTIVE	0x4
+#define INT_CMD_REQ_TIME_SYNC	0x5
+#define INT_CMD_CRASH_RESET	0x7
+#define INT_CMD_PHONE_START	0x8
+#define INT_CMD_ERR_DISPLAY	0x9
+#define INT_CMD_CRASH_EXIT	0x9
+#define INT_CMD_CP_DEEP_SLEEP	0xA
+#define INT_CMD_NV_REBUILDING	0xB
+#define INT_CMD_EMER_DOWN	0xC
+#define INT_CMD_PIF_INIT_DONE	0xD
+#define INT_CMD_SILENT_NV_REBUILDING	0xE
+#define INT_CMD_NORMAL_PWR_OFF	0xF
+
+#define EXT_CMD_MASK(x)		((x) & 0x3FFF)
+#define EXT_CMD_SET_SPEED_LOW	0x0011
+#define EXT_CMD_SET_SPEED_MID	0x0012
+#define EXT_CMD_SET_SPEED_HIGH	0x0013
+
+/* special interrupt cmd indicating modem boot failure. */
+#define INT_POWERSAFE_FAIL	0xDEAD
+
+#define UDL_CMD_VALID(x)		(((x) & 0xA000) == 0xA000)
+#define UDL_RESULT_FAIL		0x2
+#define UDL_RESULT_SUCCESS		0x1
+#define UDL_CMD_MASK(x)		(((x) >> 8) & 0xF)
+#define UDL_CMD_RECEIVE_READY		0x1
+#define UDL_CMD_DOWNLOAD_START_REQ	0x2
+#define UDL_CMD_DOWNLOAD_START_RESP	0x3
+#define UDL_CMD_IMAGE_SEND_REQ		0x4
+/* change dpram download flow */
+#define UDL_CMD_IMAGE_SEND_RESP		0x9
+
+#define UDL_CMD_SEND_DONE_RESP		0x5
+#define UDL_CMD_SEND_DONE_REQ		0x6
+#define UDL_CMD_UPDATE_DONE		0x7
+
+#define UDL_CMD_STATUS_UPDATE		0x8
+#define UDL_CMD_EFS_CLEAR_RESP		0xB
+#define UDL_CMD_ALARM_BOOT_OK		0xC
+#define UDL_CMD_ALARM_BOOT_FAIL		0xD
+
+#define CMD_IMG_START_REQ		0x9200
+#define CMD_IMG_SEND_REQ		0x9400
+#define CMD_DL_SEND_DONE_REQ		0x9600
+#define CMD_UL_RECEIVE_RESP		0x9601
+#define CMD_UL_RECEIVE_DONE_RESP	0x9801
+
+#define START_INDEX			0x7F
+#define END_INDEX			0x7E
+
+#define DP_MAGIC_DMDL			0x4445444C
+#define DP_MAGIC_UMDL			0x4445444D
+#define DP_DPRAM_SIZE			0x4000
+#define DP_DEFAULT_WRITE_LEN		8168
+#define DP_DEFAULT_DUMP_LEN		16128
+#define DP_DUMP_HEADER_SIZE		7
+
+#define UDL_TIMEOUT			(50 * HZ)
+#define UDL_SEND_TIMEOUT		(200 * HZ)
+#define FORCE_CRASH_TIMEOUT		(3 * HZ)
+#define DUMP_TIMEOUT			(30 * HZ)
+#define DUMP_START_TIMEOUT		(100 * HZ)
+
+enum cmc22x_boot_mode {
+	CMC22x_BOOT_MODE_NORMAL,
+	CMC22x_BOOT_MODE_DUMP,
+};
+
+enum dpram_init_status {
+	DPRAM_INIT_STATE_NONE,
+	DPRAM_INIT_STATE_READY,
+};
+
+struct dpram_boot_img {
+	char *addr;
+	int size;
+	enum cmc22x_boot_mode mode;
+	unsigned req;
+	unsigned resp;
+};
+
+#define MAX_PAYLOAD_SIZE 0x2000
+struct dpram_boot_frame {
+	unsigned request;	/* AP to CP Message */
+	unsigned response;	/* CP to AP Response */
+	ssize_t len;		/* request size*/
+	unsigned offset;	/* offset to write */
+	char data[MAX_PAYLOAD_SIZE];
+};
+
+/* buffer type for modem image */
+struct dpram_dump_arg {
+	char *buff;
+	int buff_size;/* AP->CP: Buffer size */
+	unsigned req; /* AP->CP request */
+	unsigned resp; /* CP->AP response */
+	bool cmd; /* AP->CP command */
+};
+
+struct dpram_firmware {
+	char *firmware;
+	int size;
+	int is_delta;
+};
+enum dpram_link_mode {
+	DPRAM_LINK_MODE_INVALID = 0,
+	DPRAM_LINK_MODE_IPC,
+	DPRAM_LINK_MODE_BOOT,
+	DPRAM_LINK_MODE_DLOAD,
+	DPRAM_LINK_MODE_ULOAD,
+};
+
+struct dpram_boot_map {
+	u32 __iomem *magic;
+	u8  __iomem *buff;
+	u32 __iomem *req;
+	u32 __iomem *resp;
+	u32          size;
+};
+
+struct dpram_dload_map {
+	u32 __iomem *magic;
+	u8  __iomem *buff;
+};
+
+struct dpram_uload_map {
+	u32 __iomem *magic;
+	u8  __iomem *buff;
+};
+
+struct dpram_ota_header {
+	u8 start_index;
+	u16 nframes;
+	u16 curframe;
+	u16 len;
+
+} __packed;
+
+struct ul_header {
+	u8  bop;
+	u16 total_frame;
+	u16 curr_frame;
+	u16 len;
+} __packed;
+/*
+	magic_code +
+	access_enable +
+	fmt_tx_head + fmt_tx_tail + fmt_tx_buff +
+	raw_tx_head + raw_tx_tail + raw_tx_buff +
+	fmt_rx_head + fmt_rx_tail + fmt_rx_buff +
+	raw_rx_head + raw_rx_tail + raw_rx_buff +
+	mbx_cp2ap +
+	mbx_ap2cp
+ =	2 +
+	2 +
+	2 + 2 + 1336 +
+	2 + 2 + 4564 +
+	2 + 2 + 1336 +
+	2 + 2 + 9124 +
+	2 +
+	2
+ =	16384
+*/
+
+#define SIPC5_DP_FMT_TX_BUFF_SZ	1336
+#define SIPC5_DP_RAW_TX_BUFF_SZ	4564
+#define SIPC5_DP_FMT_RX_BUFF_SZ	1336
+#define SIPC5_DP_RAW_RX_BUFF_SZ	9124
+
+struct sipc5_dpram_ipc_cfg {
+	u16 magic;
+	u16 access;
+
+	u16 fmt_tx_head;
+	u16 fmt_tx_tail;
+	u8  fmt_tx_buff[SIPC5_DP_FMT_TX_BUFF_SZ];
+
+	u16 raw_tx_head;
+	u16 raw_tx_tail;
+	u8  raw_tx_buff[SIPC5_DP_RAW_TX_BUFF_SZ];
+
+	u16 fmt_rx_head;
+	u16 fmt_rx_tail;
+	u8  fmt_rx_buff[SIPC5_DP_FMT_RX_BUFF_SZ];
+
+	u16 raw_rx_head;
+	u16 raw_rx_tail;
+	u8  raw_rx_buff[SIPC5_DP_RAW_RX_BUFF_SZ];
+
+	u16 mbx_cp2ap;
+	u16 mbx_ap2cp;
+};
+
+#define DPRAM_MAX_SKB_SIZE	3072	/* 3 KB */
+
+#define DP_BOOT_REQ_OFFSET	0
+#define DP_BOOT_BUFF_OFFSET	4
+#define DP_BOOT_RESP_OFFSET	8
+#define DP_DLOAD_BUFF_OFFSET	4
+#define DP_ULOAD_BUFF_OFFSET	4
+
+#define MAX_WQ_NAME_LENGTH	64
+
+#define DPRAM_MAX_RXBQ_SIZE	256
+
+struct dpram_rxb {
+	u8 *buff;
+	unsigned size;
+
+	u8 *data;
+	unsigned len;
+};
+
+struct dpram_rxb_queue {
+	int size;
+	int in;
+	int out;
+	struct dpram_rxb *rxb;
+};
+
+struct dpram_link_device {
+	struct link_device ld;
+
+	/* The mode of this DPRAM link device */
+	enum dpram_link_mode mode;
+
+	/* DPRAM address and size */
+	u32 dp_size;			/* DPRAM size			*/
+	u8 __iomem *dp_base;		/* DPRAM base virtual address	*/
+	enum dpram_type dp_type;	/* DPRAM type			*/
+
+	/* DPRAM IRQ from CP */
+	int irq;
+
+	/* Link to DPRAM control functions dependent on each platform */
+	int max_ipc_dev;
+	struct modemlink_dpram_control *dpctl;
+
+	/* Physical configuration -> logical configuration */
+	struct dpram_boot_map bt_map;
+	struct dpram_dload_map dl_map;
+	struct dpram_uload_map ul_map;
+
+	/* IPC device map */
+	struct dpram_ipc_map *ipc_map;
+
+	u16 __iomem *magic;
+	u16 __iomem *access;
+	struct dpram_ipc_device *dev[MAX_IPC_DEV];
+	u16 __iomem *mbx2ap;
+	u16 __iomem *mbx2cp;
+
+	/* Wakelock for DPRAM device */
+	struct wake_lock dpram_wake_lock;
+
+	/* For booting */
+	struct completion dpram_init_cmd;
+	struct completion modem_pif_init_done;
+
+	/* For UDL */
+	struct completion udl_start_complete;
+	struct completion udl_cmd_complete;
+
+	/* For CP RAM dump */
+	struct completion crash_start_complete;
+	struct completion dump_start_complete;
+	struct completion dump_recv_done;
+	struct timer_list dump_timer;
+	int dump_rcvd;		/* Count of dump packets received */
+
+	/* For locking Tx process */
+	spinlock_t tx_lock;
+
+	/* For efficient RX process */
+	struct tasklet_struct rx_tsk;
+	struct dpram_rxb_queue rxbq[MAX_IPC_DEV];
+
+	/* For wake-up/sleep control */
+	atomic_t accessing;
+
+	/* For retransmission after buffer full state */
+	atomic_t res_required[MAX_IPC_DEV];
+
+	/* Multi-purpose miscellaneous buffer */
+	u8 *buff;
+
+	/* DPRAM IPC initialization status */
+	int dpram_init_status;
+};
+
+/* converts from struct link_device* to struct xxx_link_device* */
+#define to_dpram_link_device(linkdev) \
+		container_of(linkdev, struct dpram_link_device, ld)
+
+#endif
diff --git a/drivers/misc/modem_if/modem_link_device_hsic.c b/drivers/misc/modem_if/modem_link_device_hsic.c
new file mode 100755
index 0000000..af058f4
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_hsic.c
@@ -0,0 +1,1584 @@
+/* /linux/drivers/new_modem_if/link_dev_usb.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+#include <linux/irq.h>
+#include <linux/poll.h>
+#include <linux/gpio.h>
+#include <linux/if_arp.h>
+#include <linux/usb.h>
+#include <linux/usb/cdc.h>
+#include <linux/pm_runtime.h>
+#include <linux/cdev.h>
+#include <linux/platform_device.h>
+#include <linux/wakelock.h>
+#include <linux/suspend.h>
+#include <linux/version.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_link_device_hsic.h"
+#include "modem_utils.h"
+
+static struct modem_ctl *if_usb_get_modemctl(struct link_pm_data *pm_data);
+static int link_pm_runtime_get_active(struct link_pm_data *pm_data);
+static int usb_tx_urb_with_skb(struct usb_device *usbdev, struct sk_buff *skb,
+					struct if_usb_devdata *pipe_data);
+#ifdef FOR_TEGRA
+#define ehci_vendor_txfilltuning tegra_ehci_txfilltuning
+#else
+#define ehci_vendor_txfilltuning()
+#endif
+static void usb_rx_complete(struct urb *urb);
+
+#if 1
+static void usb_set_autosuspend_delay(struct usb_device *usbdev, int delay)
+{
+	pm_runtime_set_autosuspend_delay(&usbdev->dev, delay);
+}
+#else
+static void usb_set_autosuspend_delay(struct usb_device *usbdev, int delay)
+{
+	usbdev->autosuspend_delay = msecs_to_jiffies(delay);
+}
+#endif
+
+static int start_ipc(struct link_device *ld, struct io_device *iod)
+{
+	struct sk_buff *skb;
+	char data[1] = {'a'};
+	int err;
+	struct usb_link_device *usb_ld = to_usb_link_device(ld);
+	struct link_pm_data *pm_data = usb_ld->link_pm_data;
+	struct device *dev = &usb_ld->usbdev->dev;
+	struct if_usb_devdata *pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
+
+	if (!usb_ld->if_usb_connected) {
+		mif_err("HSIC not connected, skip start ipc\n");
+		err = -ENODEV;
+		goto exit;
+	}
+
+retry:
+	if (ld->mc->phone_state != STATE_ONLINE) {
+		mif_err("MODEM is not online, skip start ipc\n");
+		err = -ENODEV;
+		goto exit;
+	}
+
+	/* check usb runtime pm first */
+	if (dev->power.runtime_status != RPM_ACTIVE) {
+		if (!pm_data->resume_requested) {
+			mif_debug("QW PM\n");
+			INIT_COMPLETION(pm_data->active_done);
+			queue_delayed_work(pm_data->wq,
+					&pm_data->link_pm_work, 0);
+		}
+		mif_debug("Wait pm\n");
+		err = wait_for_completion_timeout(&pm_data->active_done,
+							msecs_to_jiffies(500));
+		/* timeout or -ERESTARTSYS */
+		if (err <= 0)
+			goto retry;
+	}
+
+	pm_runtime_get_sync(dev);
+
+	mif_err("send 'a'\n");
+
+	skb = alloc_skb(16, GFP_ATOMIC);
+	if (unlikely(!skb)) {
+		pm_runtime_put(dev);
+		return -ENOMEM;
+	}
+	memcpy(skb_put(skb, 1), data, 1);
+	skbpriv(skb)->iod = iod;
+	skbpriv(skb)->ld = ld;
+
+	if (!usb_ld->if_usb_connected || !usb_ld->usbdev)
+		return -ENODEV;
+
+	usb_mark_last_busy(usb_ld->usbdev);
+	err = usb_tx_urb_with_skb(usb_ld->usbdev, skb, pipe_data);
+	if (err < 0) {
+		mif_err("usb_tx_urb fail\n");
+		dev_kfree_skb_any(skb);
+	}
+
+	pm_runtime_put(dev);
+exit:
+	return err;
+}
+
+static void stop_ipc(struct link_device *ld)
+{
+	ld->com_state = COM_NONE;
+}
+
+static int usb_init_communication(struct link_device *ld,
+			struct io_device *iod)
+{
+	struct task_struct *task = get_current();
+	char str[TASK_COMM_LEN];
+
+	mif_info("%d:%s\n", task->pid, get_task_comm(str, task));
+
+	/* Send IPC Start ASCII 'a' */
+	if (iod->id == 0x1)
+		return start_ipc(ld, iod);
+
+	return 0;
+}
+
+static void usb_terminate_communication(struct link_device *ld,
+			struct io_device *iod)
+{
+	if (iod->id != 0x1 || iod->format != IPC_FMT)
+		return;
+
+	if (iod->mc->phone_state == STATE_CRASH_RESET ||
+			iod->mc->phone_state == STATE_CRASH_EXIT)
+		stop_ipc(ld);
+}
+
+static int usb_rx_submit(struct usb_link_device *usb_ld,
+					struct if_usb_devdata *pipe_data,
+					gfp_t gfp_flags)
+{
+	int ret;
+	struct urb *urb;
+
+	if (pipe_data->disconnected)
+		return -ENOENT;
+
+	ehci_vendor_txfilltuning();
+
+	urb = pipe_data->urb;
+
+	urb->transfer_flags = 0;
+	usb_fill_bulk_urb(urb, pipe_data->usbdev,
+				pipe_data->rx_pipe, pipe_data->rx_buf,
+				pipe_data->rx_buf_size, usb_rx_complete,
+				(void *)pipe_data);
+
+	if (!usb_ld->if_usb_connected || !usb_ld->usbdev)
+		return -ENOENT;
+
+	usb_mark_last_busy(usb_ld->usbdev);
+	ret = usb_submit_urb(urb, gfp_flags);
+	if (ret)
+		mif_err("submit urb fail with ret (%d)\n", ret);
+
+	return ret;
+}
+
+static void usb_rx_retry_work(struct work_struct *work)
+{
+	int ret = 0;
+	struct usb_link_device *usb_ld =
+		container_of(work, struct usb_link_device, rx_retry_work.work);
+	struct urb *urb = usb_ld->retry_urb;
+	struct if_usb_devdata *pipe_data = urb->context;
+	struct io_device *iod;
+	int iod_format;
+
+	if (!usb_ld->if_usb_connected || !usb_ld->usbdev)
+		return;
+
+	if (usb_ld->usbdev)
+		usb_mark_last_busy(usb_ld->usbdev);
+	switch (pipe_data->format) {
+	case IF_USB_FMT_EP:
+		if (usb_ld->if_usb_is_main) {
+			pr_urb("IPC-RX, retry", urb);
+			iod_format = IPC_FMT;
+		} else {
+			iod_format = IPC_BOOT;
+		}
+		break;
+	case IF_USB_RAW_EP:
+		iod_format = IPC_MULTI_RAW;
+		break;
+	case IF_USB_RFS_EP:
+		iod_format = IPC_RFS;
+		pr_urb("RFS-RX, retry", urb);
+		break;
+	case IF_USB_CMD_EP:
+		iod_format = IPC_CMD;
+		break;
+	default:
+		iod_format = -1;
+		break;
+	}
+
+	iod = link_get_iod_with_format(&usb_ld->ld, iod_format);
+	if (iod) {
+		ret = iod->recv(iod, &usb_ld->ld, (char *)urb->transfer_buffer,
+			urb->actual_length);
+		if (ret == -ENOMEM) {
+			/* TODO: check the retry count */
+			/* retry the delay work after 20ms and resubit*/
+			mif_err("ENOMEM, +retry 20ms\n");
+			if (usb_ld->usbdev)
+				usb_mark_last_busy(usb_ld->usbdev);
+			usb_ld->retry_urb = urb;
+			if (usb_ld->rx_retry_cnt++ < 10)
+				queue_delayed_work(usb_ld->ld.tx_wq,
+					&usb_ld->rx_retry_work,	10);
+			return;
+		}
+		if (ret < 0)
+			mif_err("io device recv error (%d)\n", ret);
+		usb_ld->rx_retry_cnt = 0;
+	}
+
+	if (usb_ld->usbdev)
+		usb_mark_last_busy(usb_ld->usbdev);
+	usb_rx_submit(usb_ld, pipe_data, GFP_ATOMIC);
+}
+
+
+static void usb_rx_complete(struct urb *urb)
+{
+	struct if_usb_devdata *pipe_data = urb->context;
+	struct usb_link_device *usb_ld = pipe_data->usb_ld;
+	struct io_device *iod;
+	int iod_format;
+	int ret;
+
+	if (usb_ld->usbdev)
+		usb_mark_last_busy(usb_ld->usbdev);
+
+	switch (urb->status) {
+	case -ENOENT:
+		/* case for 'link pm suspended but rx data had remained' */
+		mif_debug("urb->status = -ENOENT\n");
+	case 0:
+		if (!urb->actual_length) {
+			mif_debug("urb has zero length!\n");
+			goto rx_submit;
+		}
+
+		usb_ld->link_pm_data->rx_cnt++;
+		/* call iod recv */
+		/* how we can distinguish boot ch with fmt ch ?? */
+		switch (pipe_data->format) {
+		case IF_USB_FMT_EP:
+			if (usb_ld->if_usb_is_main) {
+				pr_urb("IPC-RX", urb);
+				iod_format = IPC_FMT;
+			} else {
+				iod_format = IPC_BOOT;
+			}
+			break;
+		case IF_USB_RAW_EP:
+			iod_format = IPC_MULTI_RAW;
+			break;
+		case IF_USB_RFS_EP:
+			iod_format = IPC_RFS;
+			break;
+		case IF_USB_CMD_EP:
+			iod_format = IPC_CMD;
+			break;
+		default:
+			iod_format = -1;
+			break;
+		}
+
+		/* flow control CMD by CP, not use io device */
+		if (unlikely(iod_format == IPC_CMD)) {
+			ret = link_rx_flowctl_cmd(&usb_ld->ld,
+					(char *)urb->transfer_buffer,
+					urb->actual_length);
+			if (ret < 0)
+				mif_err("no multi raw device (%d)\n", ret);
+			goto rx_submit;
+		}
+
+		iod = link_get_iod_with_format(&usb_ld->ld, iod_format);
+		if (iod) {
+			ret = iod->recv(iod,
+					&usb_ld->ld,
+					(char *)urb->transfer_buffer,
+					urb->actual_length);
+			if (ret == -ENOMEM) {
+				/* retry the delay work and resubit*/
+				mif_err("ENOMEM, retry\n");
+				if (usb_ld->usbdev)
+					usb_mark_last_busy(usb_ld->usbdev);
+				usb_ld->retry_urb = urb;
+				queue_delayed_work(usb_ld->ld.tx_wq,
+					&usb_ld->rx_retry_work, 0);
+				return;
+			}
+			if (ret < 0)
+				mif_err("io device recv error (%d)\n", ret);
+		}
+rx_submit:
+		if (urb->status == 0) {
+			if (usb_ld->usbdev)
+				usb_mark_last_busy(usb_ld->usbdev);
+			usb_rx_submit(usb_ld, pipe_data, GFP_ATOMIC);
+		}
+		break;
+	default:
+		mif_err("urb err status = %d\n", urb->status);
+		break;
+	}
+}
+
+static int usb_send(struct link_device *ld, struct io_device *iod,
+			struct sk_buff *skb)
+{
+	struct sk_buff_head *txq;
+	size_t tx_size;
+	struct usb_link_device *usb_ld = to_usb_link_device(ld);
+	struct link_pm_data *pm_data = usb_ld->link_pm_data;
+
+	switch (iod->format) {
+	case IPC_RAW:
+		txq = &ld->sk_raw_tx_q;
+
+		if (unlikely(ld->raw_tx_suspended)) {
+			/* Unlike misc_write, vnet_xmit is in interrupt.
+			 * Despite call netif_stop_queue on CMD_SUSPEND,
+			 * packets can be reached here.
+			 */
+			if (in_irq()) {
+				mif_err("raw tx is suspended, "
+						"drop packet. size=%d",
+						skb->len);
+				return -EBUSY;
+			}
+
+			mif_err("wait RESUME CMD...\n");
+			INIT_COMPLETION(ld->raw_tx_resumed_by_cp);
+			wait_for_completion(&ld->raw_tx_resumed_by_cp);
+			mif_err("resumed done.\n");
+		}
+		break;
+	case IPC_BOOT:
+	case IPC_FMT:
+	case IPC_RFS:
+	default:
+		txq = &ld->sk_fmt_tx_q;
+		break;
+	}
+	/* store the tx size before run the tx_delayed_work*/
+	tx_size = skb->len;
+
+	/* drop packet, when link is not online */
+	if (ld->com_state == COM_BOOT && iod->format != IPC_BOOT) {
+		mif_err("%s: drop packet, size=%d, com_state=%d\n",
+				iod->name, skb->len, ld->com_state);
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	/* en queue skb data */
+	skb_queue_tail(txq, skb);
+	/* Hold wake_lock for getting schedule the tx_work */
+	wake_lock(&pm_data->tx_async_wake);
+
+	if (!work_pending(&ld->tx_delayed_work.work))
+		queue_delayed_work(ld->tx_wq, &ld->tx_delayed_work, 0);
+
+	return tx_size;
+}
+
+static void usb_tx_complete(struct urb *urb)
+{
+	struct sk_buff *skb = urb->context;
+	struct io_device *iod = skbpriv(skb)->iod;
+
+	switch (urb->status) {
+	case 0:
+		break;
+	case -ENOENT:
+	case -ECONNRESET:
+	case -ESHUTDOWN:
+	default:
+		if (iod->format != IPC_BOOT)
+			mif_info("TX error (%d)\n", urb->status);
+	}
+
+	dev_kfree_skb_any(skb);
+	if (urb->dev)
+		usb_mark_last_busy(urb->dev);
+	usb_free_urb(urb);
+}
+
+/* Even if usb_tx_urb_with_skb is failed, does not release the skb to retry */
+static int usb_tx_urb_with_skb(struct usb_device *usbdev, struct sk_buff *skb,
+					struct if_usb_devdata *pipe_data)
+{
+	int ret;
+	struct urb *urb;
+
+	if (pipe_data->disconnected)
+		return -ENOENT;
+
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		mif_err("alloc urb error\n");
+		return -ENOMEM;
+	}
+#if 0
+	int i;
+	for (i = 0; i < skb->len; i++) {
+		if (i > 16)
+			break;
+		mif_err("[0x%02x]", *(skb->data + i));
+	}
+#endif
+	urb->transfer_flags = URB_ZERO_PACKET;
+	usb_fill_bulk_urb(urb, pipe_data->usbdev, pipe_data->tx_pipe, skb->data,
+			skb->len, usb_tx_complete, (void *)skb);
+
+	usb_mark_last_busy(usbdev);
+	ret = usb_submit_urb(urb, GFP_KERNEL);
+	if (ret < 0) {
+		mif_err("usb_submit_urb with ret(%d)\n", ret);
+		usb_free_urb(urb);
+		return ret;
+	}
+	return 0;
+}
+
+
+static int _usb_tx_work(struct sk_buff *skb)
+{
+	struct sk_buff_head *txq;
+	struct io_device *iod = skbpriv(skb)->iod;
+	struct link_device *ld = skbpriv(skb)->ld;
+	struct usb_link_device *usb_ld = to_usb_link_device(ld);
+	struct if_usb_devdata *pipe_data;
+
+	switch (iod->format) {
+	case IPC_BOOT:
+	case IPC_FMT:
+		/* boot device uses same intf with fmt*/
+		pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
+		txq = &ld->sk_fmt_tx_q;
+		break;
+	case IPC_RAW:
+		pipe_data = &usb_ld->devdata[IF_USB_RAW_EP];
+		txq = &ld->sk_raw_tx_q;
+		break;
+	case IPC_RFS:
+		pipe_data = &usb_ld->devdata[IF_USB_RFS_EP];
+		txq = &ld->sk_fmt_tx_q;
+		break;
+	default:
+		/* wrong packet, drop it */
+		pipe_data =  NULL;
+		txq = NULL;
+		break;
+	}
+
+	if (!pipe_data)
+		return -ENOENT;
+
+	if (iod->format == IPC_FMT && usb_ld->if_usb_is_main)
+		pr_skb("IPC-TX", skb);
+
+	if (iod->format == IPC_RAW)
+		mif_debug("TX[RAW]\n");
+
+	return usb_tx_urb_with_skb(usb_ld->usbdev, skb,	pipe_data);
+}
+
+
+static void usb_tx_work(struct work_struct *work)
+{
+	int ret = 0;
+	struct link_device *ld =
+		container_of(work, struct link_device, tx_delayed_work.work);
+	struct usb_link_device *usb_ld = to_usb_link_device(ld);
+	struct sk_buff *skb;
+	struct link_pm_data *pm_data = usb_ld->link_pm_data;
+
+	if (!usb_ld->usbdev) {
+		mif_info("usbdev is invalid\n");
+		return;
+	}
+
+	pm_data->tx_cnt++;
+
+	while (ld->sk_fmt_tx_q.qlen || ld->sk_raw_tx_q.qlen) {
+		/* request and check usb runtime pm first */
+		ret = link_pm_runtime_get_active(pm_data);
+		if (ret < 0) {
+			if (ret == -ENODEV) {
+				mif_err("link not avail, retry reconnect.\n");
+				goto exit;
+			}
+			goto retry_tx_work;
+		}
+
+		/* If AP try to tx when interface disconnect->reconnect probe,
+		 * usbdev was created but one of interface channel device are
+		 * probing, _usb_tx_work return to -ENOENT then runtime usage
+		 * count allways positive and never enter to L2
+		 */
+		if (!usb_ld->if_usb_connected_last) {
+			mif_info("link is available, but if  was not readey\n");
+			goto retry_tx_work;
+		}
+		pm_runtime_get_sync(&usb_ld->usbdev->dev);
+
+		ret = 0;
+		/* send skb from fmt_txq and raw_txq,*/
+		/* one by one for fair flow control */
+		skb = skb_dequeue(&ld->sk_fmt_tx_q);
+		if (skb)
+			ret = _usb_tx_work(skb);
+
+		if (ret) {
+			mif_err("usb_tx_urb_with_skb for fmt_q %d\n", ret);
+			skb_queue_head(&ld->sk_fmt_tx_q, skb);
+
+			if (ret == -ENODEV || ret == -ENOENT)
+				goto exit;
+
+			/* tx fail and usbdev alived, retry tx work */
+			pm_runtime_put(&usb_ld->usbdev->dev);
+			goto retry_tx_work;
+		}
+
+		skb = skb_dequeue(&ld->sk_raw_tx_q);
+		if (skb)
+			ret = _usb_tx_work(skb);
+
+		if (ret) {
+			mif_err("usb_tx_urb_with_skb for raw_q %d\n", ret);
+			skb_queue_head(&ld->sk_raw_tx_q, skb);
+
+			if (ret == -ENODEV || ret == -ENOENT)
+				goto exit;
+
+			pm_runtime_put(&usb_ld->usbdev->dev);
+			goto retry_tx_work;
+		}
+
+		pm_runtime_put(&usb_ld->usbdev->dev);
+	}
+	wake_unlock(&pm_data->tx_async_wake);
+exit:
+	return;
+
+retry_tx_work:
+	queue_delayed_work(ld->tx_wq, &ld->tx_delayed_work,
+		msecs_to_jiffies(20));
+	return;
+}
+
+/*
+#ifdef CONFIG_LINK_PM
+*/
+
+static int link_pm_runtime_get_active(struct link_pm_data *pm_data)
+{
+	int ret;
+	struct usb_link_device *usb_ld = pm_data->usb_ld;
+	struct device *dev = &usb_ld->usbdev->dev;
+
+	if (!usb_ld->if_usb_connected || usb_ld->ld.com_state == COM_NONE)
+		return -ENODEV;
+
+	if (pm_data->dpm_suspending) {
+		mif_err("Kernel in suspending try get_active later\n");
+		/* during dpm_suspending..
+		 * if AP get tx data, wake up. */
+		wake_lock(&pm_data->l2_wake);
+		return -EAGAIN;
+	}
+
+	if (dev->power.runtime_status == RPM_ACTIVE) {
+		pm_data->resume_retry_cnt = 0;
+		return 0;
+	}
+
+	if (!pm_data->resume_requested) {
+		mif_debug("QW PM\n");
+		queue_delayed_work(pm_data->wq, &pm_data->link_pm_work, 0);
+	}
+	mif_debug("Wait pm\n");
+	INIT_COMPLETION(pm_data->active_done);
+	ret = wait_for_completion_timeout(&pm_data->active_done,
+						msecs_to_jiffies(500));
+
+	/* If usb link was disconnected while waiting ACTIVE State, usb device
+	 * was removed, usb_ld->usbdev->dev is invalid and below
+	 * dev->power.runtime_status is also invalid address.
+	 * It will be occured LPA L3 -> AP iniated L0 -> disconnect -> link
+	 * timeout
+	 */
+	if (!usb_ld->if_usb_connected || usb_ld->ld.com_state == COM_NONE) {
+		mif_info("link disconnected after timed-out\n");
+		return -ENODEV;
+	}
+
+	if (dev->power.runtime_status != RPM_ACTIVE) {
+		mif_info("link_active (%d) retry\n",
+						dev->power.runtime_status);
+		return -EAGAIN;
+	}
+	mif_debug("link_active success(%d)\n", ret);
+	return 0;
+}
+
+static void link_pm_runtime_start(struct work_struct *work)
+{
+	struct link_pm_data *pm_data =
+		container_of(work, struct link_pm_data, link_pm_start.work);
+	struct usb_device *usbdev = pm_data->usb_ld->usbdev;
+	struct device *dev, *ppdev;
+	struct link_device *ld = &pm_data->usb_ld->ld;
+
+	if (!pm_data->usb_ld->if_usb_connected
+		|| pm_data->usb_ld->ld.com_state == COM_NONE) {
+		mif_debug("disconnect status, ignore\n");
+		return;
+	}
+
+	dev = &pm_data->usb_ld->usbdev->dev;
+
+	/* wait interface driver resumming */
+	if (dev->power.runtime_status == RPM_SUSPENDED) {
+		mif_info("suspended yet, delayed work\n");
+		queue_delayed_work(pm_data->wq, &pm_data->link_pm_start,
+			msecs_to_jiffies(20));
+		return;
+	}
+
+	if (pm_data->usb_ld->usbdev && dev->parent) {
+		mif_info("rpm_status: %d\n",
+			dev->power.runtime_status);
+		usb_set_autosuspend_delay(usbdev, 200);
+		ppdev = dev->parent->parent;
+		pm_runtime_allow(dev);
+		pm_runtime_allow(ppdev);/*ehci*/
+		pm_data->link_pm_active = true;
+		pm_data->resume_requested = false;
+		pm_data->link_reconnect_cnt = 2;
+		pm_data->resume_retry_cnt = 0;
+
+		/* retry prvious link tx q */
+		queue_delayed_work(ld->tx_wq, &ld->tx_delayed_work, 0);
+	}
+}
+
+static void link_pm_force_cp_dump(struct link_pm_data *pm_data)
+{
+	struct modem_ctl *mc = if_usb_get_modemctl(pm_data);
+
+	mif_err("Set modem crash ap_dump_int by %pF\n",
+		__builtin_return_address(0));
+
+	if (mc->gpio_ap_dump_int) {
+		if (gpio_get_value(mc->gpio_ap_dump_int)) {
+			gpio_set_value(mc->gpio_ap_dump_int, 0);
+			msleep(20);
+		}
+		gpio_set_value(mc->gpio_ap_dump_int, 1);
+		msleep(20);
+		mif_err("AP_DUMP_INT(%d)\n",
+			gpio_get_value(mc->gpio_ap_dump_int));
+		gpio_set_value(mc->gpio_ap_dump_int, 0);
+	}
+}
+
+static void link_pm_change_modem_state(struct link_pm_data *pm_data,
+						enum modem_state state)
+{
+	struct modem_ctl *mc = if_usb_get_modemctl(pm_data);
+
+	if (!mc->iod || pm_data->usb_ld->ld.com_state != COM_ONLINE)
+		return;
+
+	mif_err("set modem state %d by %pF\n", state,
+		__builtin_return_address(0));
+	mc->iod->modem_state_changed(mc->iod, state);
+	mc->bootd->modem_state_changed(mc->bootd, state);
+}
+
+static void link_pm_reconnect_work(struct work_struct *work)
+{
+	struct link_pm_data *pm_data =
+		container_of(work, struct link_pm_data,
+					link_reconnect_work.work);
+	struct modem_ctl *mc = if_usb_get_modemctl(pm_data);
+
+	if (!mc || pm_data->usb_ld->if_usb_connected)
+		return;
+
+	if (pm_data->usb_ld->ld.com_state != COM_ONLINE)
+		return;
+
+	if (pm_data->link_reconnect_cnt--) {
+		if (mc->phone_state == STATE_ONLINE &&
+						!pm_data->link_reconnect())
+			/* try reconnect and check */
+			schedule_delayed_work(&pm_data->link_reconnect_work,
+							msecs_to_jiffies(500));
+		else	/* under cp crash or reset, just return */
+			return;
+	} else {
+		/* try to recover cp */
+		mif_err("recover connection: silent reset\n");
+		link_pm_change_modem_state(pm_data, STATE_CRASH_RESET);
+	}
+}
+
+static inline int link_pm_slave_wake(struct link_pm_data *pm_data)
+{
+	int spin = 20;
+
+	/* when slave device is in sleep, wake up slave cpu first */
+	if (gpio_get_value(pm_data->gpio_link_hostwake)
+				!= HOSTWAKE_TRIGLEVEL) {
+		if (gpio_get_value(pm_data->gpio_link_slavewake)) {
+			gpio_set_value(pm_data->gpio_link_slavewake, 0);
+			mif_info("gpio [SWK] set [0]\n");
+			mdelay(5);
+		}
+		gpio_set_value(pm_data->gpio_link_slavewake, 1);
+		mif_info("gpio [SWK] set [1]\n");
+		mdelay(5);
+
+		/* wait host wake signal*/
+		while (spin-- && gpio_get_value(pm_data->gpio_link_hostwake) !=
+							HOSTWAKE_TRIGLEVEL)
+			mdelay(5);
+	}
+	/* runtime pm goes to active */
+	if (!gpio_get_value(pm_data->gpio_link_active)) {
+		mif_debug("gpio [H ACTV : %d] set 1\n",
+				gpio_get_value(pm_data->gpio_link_active));
+		gpio_set_value(pm_data->gpio_link_active, 1);
+	}
+	return spin;
+}
+
+static void link_pm_runtime_work(struct work_struct *work)
+{
+	int ret;
+	struct link_pm_data *pm_data =
+		container_of(work, struct link_pm_data, link_pm_work.work);
+	struct device *dev = &pm_data->usb_ld->usbdev->dev;
+
+	if (!pm_data->usb_ld->if_usb_connected || pm_data->dpm_suspending)
+		return;
+
+	if (pm_data->usb_ld->ld.com_state == COM_NONE)
+		return;
+
+	mif_debug("for dev 0x%p : current %d\n", dev,
+					dev->power.runtime_status);
+
+	switch (dev->power.runtime_status) {
+	case RPM_ACTIVE:
+		pm_data->resume_retry_cnt = 0;
+		pm_data->resume_requested = false;
+		complete(&pm_data->active_done);
+
+		return;
+	case RPM_SUSPENDED:
+		if (pm_data->resume_requested)
+			break;
+		pm_data->resume_requested = true;
+		wake_lock(&pm_data->rpm_wake);
+		ret = link_pm_slave_wake(pm_data);
+		if (ret < 0) {
+			mif_err("slave wake fail\n");
+			wake_unlock(&pm_data->rpm_wake);
+			break;
+		}
+
+		if (!pm_data->usb_ld->if_usb_connected) {
+			wake_unlock(&pm_data->rpm_wake);
+			return;
+		}
+
+		ret = pm_runtime_resume(dev);
+		if (ret < 0) {
+			mif_err("resume error(%d)\n", ret);
+			if (!pm_data->usb_ld->if_usb_connected) {
+				wake_unlock(&pm_data->rpm_wake);
+				return;
+			}
+			/* force to go runtime idle before retry resume */
+			if (dev->power.timer_expires == 0 &&
+						!dev->power.request_pending) {
+				mif_debug("run time idle\n");
+				pm_runtime_idle(dev);
+			}
+		}
+		wake_unlock(&pm_data->rpm_wake);
+		break;
+	default:
+		break;
+	}
+	pm_data->resume_requested = false;
+
+	/* check until runtime_status goes to active */
+	/* attemp 10 times, or re-establish modem-link */
+	/* if pm_runtime_resume run properly, rpm status must be in ACTIVE */
+	if (dev->power.runtime_status == RPM_ACTIVE) {
+		pm_data->resume_retry_cnt = 0;
+		complete(&pm_data->active_done);
+	} else if (pm_data->resume_retry_cnt++ > 10) {
+		mif_err("runtime_status(%d), retry_cnt(%d)\n",
+			dev->power.runtime_status, pm_data->resume_retry_cnt);
+		link_pm_change_modem_state(pm_data, STATE_CRASH_RESET);
+	} else
+		queue_delayed_work(pm_data->wq, &pm_data->link_pm_work,
+							msecs_to_jiffies(20));
+}
+
+static irqreturn_t link_pm_irq_handler(int irq, void *data)
+{
+	int value;
+	struct link_pm_data *pm_data = data;
+
+#if defined(CONFIG_SLP)
+	pm_wakeup_event(pm_data->miscdev.this_device, 0);
+#endif
+
+	if (!pm_data->link_pm_active)
+		return IRQ_HANDLED;
+
+	/* host wake up HIGH */
+	/*
+		resume usb runtime pm start
+	*/
+	/* host wake up LOW */
+	/*
+		slave usb enumeration end,
+		host can send usb packet after
+		runtime pm status changes to ACTIVE
+	*/
+	value = gpio_get_value(pm_data->gpio_link_hostwake);
+	mif_info("gpio [HWK] get [%d]\n", value);
+
+	/*
+	* igonore host wakeup interrupt at suspending kernel
+	*/
+	if (pm_data->dpm_suspending) {
+		mif_info("ignore request by suspending\n");
+		/* Ignore HWK but AP got to L2 by suspending fail */
+		wake_lock(&pm_data->l2_wake);
+		return IRQ_HANDLED;
+	}
+
+	if (value == HOSTWAKE_TRIGLEVEL) {
+		/* move to slave wake function */
+		/* runtime pm goes to active */
+		/*
+		if (gpio_get_value(pm_data->gpio_link_active)) {
+			mif_err("gpio [H ACTV : %d] set 1\n",
+				gpio_get_value(pm_data->gpio_link_active));
+			gpio_set_value(pm_data->gpio_link_active, 1);
+		}
+		*/
+		queue_delayed_work(pm_data->wq, &pm_data->link_pm_work, 0);
+	} else {
+		/* notification of enumeration process from slave device
+		 * But it does not mean whole connection is in resume, so do not
+		 * notify resume completion here.
+
+		if (pm_data->link_pm_active && !pm_data->active_done.done)
+			complete(&pm_data->active_done);
+		*/
+		/* clear slave cpu wake up pin */
+		gpio_set_value(pm_data->gpio_link_slavewake, 0);
+		mif_debug("gpio [SWK] set [0]\n");
+	}
+	return IRQ_HANDLED;
+}
+
+static long link_pm_ioctl(struct file *file, unsigned int cmd,
+						unsigned long arg)
+{
+	int value;
+	struct link_pm_data *pm_data = file->private_data;
+
+	mif_info("%x\n", cmd);
+
+	switch (cmd) {
+	case IOCTL_LINK_CONTROL_ENABLE:
+		if (copy_from_user(&value, (const void __user *)arg,
+							sizeof(int)))
+			return -EFAULT;
+		if (pm_data->link_ldo_enable)
+			pm_data->link_ldo_enable(!!value);
+		if (pm_data->gpio_link_enable)
+			gpio_set_value(pm_data->gpio_link_enable, value);
+		break;
+	case IOCTL_LINK_CONTROL_ACTIVE:
+		if (copy_from_user(&value, (const void __user *)arg,
+							sizeof(int)))
+			return -EFAULT;
+		gpio_set_value(pm_data->gpio_link_active, value);
+		break;
+	case IOCTL_LINK_GET_HOSTWAKE:
+		return !gpio_get_value(pm_data->gpio_link_hostwake);
+	case IOCTL_LINK_CONNECTED:
+		return pm_data->usb_ld->if_usb_connected;
+	case IOCTL_LINK_SET_BIAS_CLEAR:
+		if (copy_from_user(&value, (const void __user *)arg,
+							sizeof(int)))
+			return -EFAULT;
+		if (value) {
+			gpio_direction_output(pm_data->gpio_link_slavewake, 0);
+			gpio_direction_output(pm_data->gpio_link_hostwake, 0);
+		} else {
+			gpio_direction_output(pm_data->gpio_link_slavewake, 0);
+			gpio_direction_input(pm_data->gpio_link_hostwake);
+			irq_set_irq_type(pm_data->irq_link_hostwake,
+				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING);
+		}
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int link_pm_open(struct inode *inode, struct file *file)
+{
+	struct link_pm_data *pm_data =
+		(struct link_pm_data *)file->private_data;
+	file->private_data = (void *)pm_data;
+	return 0;
+}
+
+static int link_pm_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+
+static const struct file_operations link_pm_fops = {
+	.owner = THIS_MODULE,
+	.open = link_pm_open,
+	.release = link_pm_release,
+	.unlocked_ioctl = link_pm_ioctl,
+};
+
+static int link_pm_notifier_event(struct notifier_block *this,
+				unsigned long event, void *ptr)
+{
+	struct link_pm_data *pm_data =
+			container_of(this, struct link_pm_data,	pm_notifier);
+#ifdef CONFIG_UMTS_MODEM_XMM6262
+	struct modem_ctl *mc = if_usb_get_modemctl(pm_data);
+#endif
+
+	switch (event) {
+	case PM_SUSPEND_PREPARE:
+#ifdef CONFIG_HIBERNATION
+	case PM_HIBERNATION_PREPARE:
+	case PM_RESTORE_PREPARE:
+#endif
+		pm_data->dpm_suspending = true;
+#ifdef CONFIG_UMTS_MODEM_XMM6262
+		/* set PDA Active High if previous state was LPA */
+		gpio_set_value(mc->gpio_pda_active, 1);
+#endif
+		mif_debug("dpm suspending set to true\n");
+		return NOTIFY_OK;
+	case PM_POST_SUSPEND:
+#ifdef CONFIG_HIBERNATION
+	case PM_POST_HIBERNATION:
+	case PM_POST_RESTORE:
+#endif
+		pm_data->dpm_suspending = false;
+		if (gpio_get_value(pm_data->gpio_link_hostwake)
+			== HOSTWAKE_TRIGLEVEL) {
+			queue_delayed_work(pm_data->wq, &pm_data->link_pm_work,
+				0);
+			mif_info("post resume\n");
+		}
+		mif_debug("dpm suspending set to false\n");
+		return NOTIFY_OK;
+	}
+	return NOTIFY_DONE;
+}
+
+static struct modem_ctl *if_usb_get_modemctl(struct link_pm_data *pm_data)
+{
+	struct io_device *iod;
+
+	iod = link_get_iod_with_format(&pm_data->usb_ld->ld, IPC_FMT);
+	if (!iod) {
+		mif_err("no iodevice for modem control\n");
+		return NULL;
+	}
+
+	return iod->mc;
+}
+
+static int if_usb_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct if_usb_devdata *devdata = usb_get_intfdata(intf);
+	struct link_pm_data *pm_data = devdata->usb_ld->link_pm_data;
+	if (!devdata->disconnected && devdata->state == STATE_RESUMED) {
+		usb_kill_urb(devdata->urb);
+		devdata->state = STATE_SUSPENDED;
+	}
+
+	devdata->usb_ld->suspended++;
+
+	if (devdata->usb_ld->suspended == LINKPM_DEV_NUM) {
+		mif_debug("[if_usb_suspended]\n");
+		wake_lock_timeout(&pm_data->l2_wake, msecs_to_jiffies(50));
+#ifdef	CONFIG_SLP
+		pm_wakeup_event(pm_data->miscdev.this_device,
+				msecs_to_jiffies(20));
+#endif
+		/* XMM6262 Host wakeup toggle recovery */
+		if (!pm_data->rx_cnt && !pm_data->tx_cnt) {
+			if (pm_data->ipc_debug_cnt++ > 10) {
+				mif_err("No TX/RX after resume 10times\n");
+				link_pm_change_modem_state(pm_data,
+					STATE_CRASH_RESET);
+			}
+		} else {
+			pm_data->ipc_debug_cnt = 0;
+			pm_data->rx_cnt = 0;
+			pm_data->tx_cnt = 0;
+		}
+	}
+	return 0;
+}
+
+static int if_usb_resume(struct usb_interface *intf)
+{
+	int ret;
+	struct if_usb_devdata *devdata = usb_get_intfdata(intf);
+	struct link_pm_data *pm_data = devdata->usb_ld->link_pm_data;
+
+	if (!devdata->disconnected && devdata->state == STATE_SUSPENDED) {
+		ret = usb_rx_submit(devdata->usb_ld, devdata, GFP_ATOMIC);
+		if (ret < 0) {
+			mif_err("usb_rx_submit error with (%d)\n", ret);
+			return ret;
+		}
+		devdata->state = STATE_RESUMED;
+	}
+
+	/* For debugging -  nomal case, never reach below... */
+	if (pm_data->resume_retry_cnt > 5) {
+		mif_err("retry_cnt=%d, rpm_status=%d",
+			pm_data->resume_retry_cnt,
+			devdata->usb_ld->usbdev->dev.power.runtime_status);
+		pm_data->resume_retry_cnt = 0;
+	}
+
+	devdata->usb_ld->suspended--;
+	if (!devdata->usb_ld->suspended) {
+		mif_debug("[if_usb_resumed]\n");
+		wake_lock(&pm_data->l2_wake);
+	}
+
+	return 0;
+}
+
+static int if_usb_reset_resume(struct usb_interface *intf)
+{
+	int ret;
+	struct if_usb_devdata *devdata = usb_get_intfdata(intf);
+	struct link_pm_data *pm_data = devdata->usb_ld->link_pm_data;
+
+	ret = if_usb_resume(intf);
+	pm_data->ipc_debug_cnt = 0;
+	/*
+	 * for runtime suspend, kick runtime pm at L3 -> L0 reset resume
+	*/
+	if (!devdata->usb_ld->suspended)
+		queue_delayed_work(pm_data->wq, &pm_data->link_pm_start, 0);
+	return ret;
+}
+
+static void if_usb_disconnect(struct usb_interface *intf)
+{
+	struct if_usb_devdata *devdata = usb_get_intfdata(intf);
+	struct link_pm_data *pm_data = devdata->usb_ld->link_pm_data;
+	struct device *dev, *ppdev;
+	struct link_device *ld = &devdata->usb_ld->ld;
+
+	mif_info("\n");
+
+	if (devdata->disconnected)
+		return;
+
+	usb_driver_release_interface(to_usb_driver(intf->dev.driver), intf);
+
+	usb_kill_urb(devdata->urb);
+
+	dev = &devdata->usb_ld->usbdev->dev;
+	ppdev = dev->parent->parent;
+	pm_runtime_forbid(ppdev); /*ehci*/
+
+	mif_info("put dev 0x%p\n", devdata->usbdev);
+	usb_put_dev(devdata->usbdev);
+
+	devdata->data_intf = NULL;
+	devdata->usbdev = NULL;
+	/* if possible, merge below 2 variables */
+	devdata->disconnected = 1;
+	devdata->state = STATE_SUSPENDED;
+	pm_data->ipc_debug_cnt = 0;
+
+	devdata->usb_ld->if_usb_connected = 0;
+	devdata->usb_ld->if_usb_connected_last = 0;
+	devdata->usb_ld->suspended = 0;
+	wake_lock(&pm_data->boot_wake);
+
+	usb_set_intfdata(intf, NULL);
+
+	/* cancel runtime start delayed works */
+	cancel_delayed_work_sync(&pm_data->link_pm_start);
+	cancel_delayed_work_sync(&ld->tx_delayed_work);
+
+	/* if reconnect function exist , try reconnect without reset modem
+	 * reconnect function checks modem is under crash or not, so we don't
+	 * need check crash state here. reconnect work checks and determine
+	 * further works
+	 */
+	if (!pm_data->link_reconnect)
+		return;
+
+	if (devdata->usb_ld->ld.com_state != COM_ONLINE) {
+		cancel_delayed_work(&pm_data->link_reconnect_work);
+		return;
+	} else
+		schedule_delayed_work(&pm_data->link_reconnect_work,
+							msecs_to_jiffies(500));
+	return;
+}
+
+static int if_usb_set_pipe(struct usb_link_device *usb_ld,
+			const struct usb_host_interface *desc, int pipe)
+{
+	if (pipe < 0 || pipe >= IF_USB_DEVNUM_MAX) {
+		mif_err("undefined endpoint, exceed max\n");
+		return -EINVAL;
+	}
+
+	mif_info("set %d\n", pipe);
+
+	if ((usb_pipein(desc->endpoint[0].desc.bEndpointAddress)) &&
+	    (usb_pipeout(desc->endpoint[1].desc.bEndpointAddress))) {
+		usb_ld->devdata[pipe].rx_pipe = usb_rcvbulkpipe(usb_ld->usbdev,
+				desc->endpoint[0].desc.bEndpointAddress);
+		usb_ld->devdata[pipe].tx_pipe = usb_sndbulkpipe(usb_ld->usbdev,
+				desc->endpoint[1].desc.bEndpointAddress);
+	} else if ((usb_pipeout(desc->endpoint[0].desc.bEndpointAddress)) &&
+		   (usb_pipein(desc->endpoint[1].desc.bEndpointAddress))) {
+		usb_ld->devdata[pipe].rx_pipe = usb_rcvbulkpipe(usb_ld->usbdev,
+				desc->endpoint[1].desc.bEndpointAddress);
+		usb_ld->devdata[pipe].tx_pipe = usb_sndbulkpipe(usb_ld->usbdev,
+				desc->endpoint[0].desc.bEndpointAddress);
+	} else {
+		mif_err("undefined endpoint\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+
+static struct usb_id_info hsic_channel_info;
+
+static int __devinit if_usb_probe(struct usb_interface *intf,
+					const struct usb_device_id *id)
+{
+	int err;
+	int pipe;
+	const struct usb_cdc_union_desc *union_hdr;
+	const struct usb_host_interface *data_desc;
+	unsigned char *buf = intf->altsetting->extra;
+	int buflen = intf->altsetting->extralen;
+	struct usb_interface *data_intf;
+	struct usb_device *usbdev = interface_to_usbdev(intf);
+	struct usb_driver *usbdrv = to_usb_driver(intf->dev.driver);
+	struct usb_id_info *info = (struct usb_id_info *)id->driver_info;
+	struct usb_link_device *usb_ld = info->usb_ld;
+
+	mif_info("usbdev = 0x%p\n", usbdev);
+
+	usb_ld->usbdev = usbdev;
+	pm_runtime_forbid(&usbdev->dev);
+	usb_ld->link_pm_data->link_pm_active = false;
+	usb_ld->link_pm_data->dpm_suspending = false;
+	usb_ld->link_pm_data->ipc_debug_cnt = 0;
+	usb_ld->if_usb_is_main = (info == &hsic_channel_info);
+
+	union_hdr = NULL;
+	/* for WMC-ACM compatibility, WMC-ACM use an end-point for control msg*/
+	if (intf->altsetting->desc.bInterfaceSubClass != USB_CDC_SUBCLASS_ACM) {
+		mif_err("ignore Non ACM end-point\n");
+		return -EINVAL;
+	}
+
+	if (!buflen) {
+		if (intf->cur_altsetting->endpoint->extralen &&
+				    intf->cur_altsetting->endpoint->extra) {
+			buflen = intf->cur_altsetting->endpoint->extralen;
+			buf = intf->cur_altsetting->endpoint->extra;
+		} else {
+			mif_err("Zero len descriptor reference\n");
+			return -EINVAL;
+		}
+	}
+
+	while (buflen > 0) {
+		if (buf[1] == USB_DT_CS_INTERFACE) {
+			switch (buf[2]) {
+			case USB_CDC_UNION_TYPE:
+				if (union_hdr)
+					break;
+				union_hdr = (struct usb_cdc_union_desc *)buf;
+				break;
+			default:
+				break;
+			}
+		}
+		buf += buf[0];
+		buflen -= buf[0];
+	}
+
+	if (!union_hdr) {
+		mif_err("USB CDC is not union type\n");
+		return -EINVAL;
+	}
+
+	data_intf = usb_ifnum_to_if(usbdev, union_hdr->bSlaveInterface0);
+	if (!data_intf) {
+		mif_err("data_inferface is NULL\n");
+		return -ENODEV;
+	}
+
+	data_desc = data_intf->altsetting;
+	if (!data_desc) {
+		mif_err("data_desc is NULL\n");
+		return -ENODEV;
+	}
+
+	switch (info->intf_id) {
+	case BOOT_DOWN:
+		pipe = IF_USB_BOOT_EP;
+		usb_ld->ld.com_state = COM_BOOT;
+		/* purge previous boot fmt/raw tx q
+		 clear all tx q*/
+		skb_queue_purge(&usb_ld->ld.sk_fmt_tx_q);
+		skb_queue_purge(&usb_ld->ld.sk_raw_tx_q);
+		break;
+	case IPC_CHANNEL:
+		pipe = intf->altsetting->desc.bInterfaceNumber / 2;
+		usb_ld->ld.com_state = COM_ONLINE;
+		break;
+	default:
+		pipe = -1;
+		break;
+	}
+
+	if (if_usb_set_pipe(usb_ld, data_desc, pipe) < 0)
+		return -EINVAL;
+
+	usb_ld->devdata[pipe].usbdev = usb_get_dev(usbdev);
+	mif_info("devdata usbdev = 0x%p\n",
+		usb_ld->devdata[pipe].usbdev);
+	usb_ld->devdata[pipe].usb_ld = usb_ld;
+	usb_ld->devdata[pipe].data_intf = data_intf;
+	usb_ld->devdata[pipe].format = pipe;
+	usb_ld->devdata[pipe].disconnected = 0;
+	usb_ld->devdata[pipe].state = STATE_RESUMED;
+
+	usb_ld->if_usb_connected = 1;
+	usb_ld->suspended = 0;
+
+	err = usb_driver_claim_interface(usbdrv, data_intf,
+		(void *)&usb_ld->devdata[pipe]);
+	if (err < 0) {
+		mif_err("usb_driver_claim() failed\n");
+		return err;
+	}
+
+	pm_suspend_ignore_children(&usbdev->dev, true);
+
+	usb_set_intfdata(intf, (void *)&usb_ld->devdata[pipe]);
+
+	/* rx start for this endpoint */
+	usb_rx_submit(usb_ld, &usb_ld->devdata[pipe], GFP_KERNEL);
+
+	if (info->intf_id == IPC_CHANNEL &&
+		!work_pending(&usb_ld->link_pm_data->link_pm_start.work)) {
+			queue_delayed_work(usb_ld->link_pm_data->wq,
+					&usb_ld->link_pm_data->link_pm_start,
+					msecs_to_jiffies(500));
+			wake_lock(&usb_ld->link_pm_data->l2_wake);
+			wake_unlock(&usb_ld->link_pm_data->boot_wake);
+	}
+
+	/* HSIC main comm channel has been established */
+	if (pipe == IF_USB_CMD_EP)
+		link_pm_change_modem_state(usb_ld->link_pm_data, STATE_ONLINE);
+
+	if (pipe == IF_USB_CMD_EP || info->intf_id == BOOT_DOWN)
+		usb_ld->if_usb_connected_last = 1;
+
+	mif_info("successfully done\n");
+
+	return 0;
+}
+
+static void if_usb_free_pipe_data(struct usb_link_device *usb_ld)
+{
+	int i;
+	for (i = 0; i < IF_USB_DEVNUM_MAX; i++) {
+		kfree(usb_ld->devdata[i].rx_buf);
+		usb_kill_urb(usb_ld->devdata[i].urb);
+	}
+}
+
+static struct usb_id_info hsic_boot_down_info = {
+	.intf_id = BOOT_DOWN,
+};
+static struct usb_id_info hsic_channel_info = {
+	.intf_id = IPC_CHANNEL,
+};
+
+static struct usb_device_id if_usb_ids[] = {
+	{USB_DEVICE(IMC_BOOT_VID, IMC_BOOT_PID),
+	.driver_info = (unsigned long)&hsic_boot_down_info,},
+	{USB_DEVICE(IMC_MAIN_VID, IMC_MAIN_PID),
+	.driver_info = (unsigned long)&hsic_channel_info,},
+	{USB_DEVICE(STE_BOOT_VID, STE_BOOT_PID),
+	.driver_info = (unsigned long)&hsic_boot_down_info,},
+	{USB_DEVICE(STE_MAIN_VID, STE_MAIN_PID),
+	.driver_info = (unsigned long)&hsic_channel_info,},
+	{}
+};
+MODULE_DEVICE_TABLE(usb, if_usb_ids);
+
+static struct usb_driver if_usb_driver = {
+	.name =		"cdc_modem",
+	.probe =		if_usb_probe,
+	.disconnect =	if_usb_disconnect,
+	.id_table =	if_usb_ids,
+	.suspend =	if_usb_suspend,
+	.resume =	if_usb_resume,
+	.reset_resume =	if_usb_reset_resume,
+	.supports_autosuspend = 1,
+};
+
+static int if_usb_init(struct link_device *ld)
+{
+	int ret;
+	int i;
+	struct usb_link_device *usb_ld = to_usb_link_device(ld);
+	struct if_usb_devdata *pipe_data;
+	struct usb_id_info *id_info;
+
+	/* to connect usb link device with usb interface driver */
+	for (i = 0; i < ARRAY_SIZE(if_usb_ids); i++) {
+		id_info = (struct usb_id_info *)if_usb_ids[i].driver_info;
+		if (id_info)
+			id_info->usb_ld = usb_ld;
+	}
+
+	ret = usb_register(&if_usb_driver);
+	if (ret) {
+		mif_err("usb_register_driver() fail : %d\n", ret);
+		return ret;
+	}
+
+	/* allocate rx buffer for usb receive */
+	for (i = 0; i < IF_USB_DEVNUM_MAX; i++) {
+		pipe_data = &usb_ld->devdata[i];
+		pipe_data->format = i;
+		pipe_data->rx_buf_size = 16 * 1024;
+
+		pipe_data->rx_buf = kmalloc(pipe_data->rx_buf_size,
+						GFP_DMA | GFP_KERNEL);
+		if (!pipe_data->rx_buf) {
+			if_usb_free_pipe_data(usb_ld);
+			ret = -ENOMEM;
+			break;
+		}
+
+		pipe_data->urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!pipe_data->urb) {
+			mif_err("alloc urb fail\n");
+			if_usb_free_pipe_data(usb_ld);
+			return -ENOMEM;
+		}
+	}
+
+	mif_info("if_usb_init() done : %d, usb_ld (0x%p)\n",
+								ret, usb_ld);
+	return ret;
+}
+
+static int usb_link_pm_init(struct usb_link_device *usb_ld, void *data)
+{
+	int r;
+	struct platform_device *pdev = (struct platform_device *)data;
+	struct modem_data *pdata =
+			(struct modem_data *)pdev->dev.platform_data;
+	struct modemlink_pm_data *pm_pdata = pdata->link_pm_data;
+	struct link_pm_data *pm_data =
+			kzalloc(sizeof(struct link_pm_data), GFP_KERNEL);
+	if (!pm_data) {
+		mif_err("link_pm_data is NULL\n");
+		return -ENOMEM;
+	}
+	/* get link pm data from modemcontrol's platform data */
+	pm_data->gpio_link_active = pm_pdata->gpio_link_active;
+	pm_data->gpio_link_enable = pm_pdata->gpio_link_enable;
+	pm_data->gpio_link_hostwake = pm_pdata->gpio_link_hostwake;
+	pm_data->gpio_link_slavewake = pm_pdata->gpio_link_slavewake;
+	pm_data->irq_link_hostwake = gpio_to_irq(pm_data->gpio_link_hostwake);
+	pm_data->link_ldo_enable = pm_pdata->link_ldo_enable;
+	pm_data->link_reconnect = pm_pdata->link_reconnect;
+
+	pm_data->usb_ld = usb_ld;
+	pm_data->link_pm_active = false;
+	pm_data->ipc_debug_cnt = 0;
+	usb_ld->link_pm_data = pm_data;
+
+	pm_data->miscdev.minor = MISC_DYNAMIC_MINOR;
+	pm_data->miscdev.name = "link_pm";
+	pm_data->miscdev.fops = &link_pm_fops;
+
+	r = misc_register(&pm_data->miscdev);
+	if (r < 0) {
+		mif_err("fail to register pm device(%d)\n", r);
+		goto err_misc_register;
+	}
+
+	r = request_irq(pm_data->irq_link_hostwake, link_pm_irq_handler,
+		IRQF_NO_SUSPEND | IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+		"hostwake", (void *)pm_data);
+	if (r) {
+		mif_err("fail to request irq(%d)\n", r);
+		goto err_request_irq;
+	}
+
+	r = enable_irq_wake(pm_data->irq_link_hostwake);
+	if (r) {
+		mif_err("failed to enable_irq_wake:%d\n", r);
+		goto err_set_wake_irq;
+	}
+
+	/* create work queue & init work for runtime pm */
+	pm_data->wq = create_singlethread_workqueue("linkpmd");
+	if (!pm_data->wq) {
+		mif_err("fail to create wq\n");
+		goto err_create_wq;
+	}
+
+	pm_data->pm_notifier.notifier_call = link_pm_notifier_event;
+	register_pm_notifier(&pm_data->pm_notifier);
+
+	init_completion(&pm_data->active_done);
+	INIT_DELAYED_WORK(&pm_data->link_pm_work, link_pm_runtime_work);
+	INIT_DELAYED_WORK(&pm_data->link_pm_start, link_pm_runtime_start);
+	INIT_DELAYED_WORK(&pm_data->link_reconnect_work,
+						link_pm_reconnect_work);
+	wake_lock_init(&pm_data->l2_wake, WAKE_LOCK_SUSPEND, "l2_hsic");
+	wake_lock_init(&pm_data->boot_wake, WAKE_LOCK_SUSPEND, "boot_hsic");
+	wake_lock_init(&pm_data->rpm_wake, WAKE_LOCK_SUSPEND, "rpm_hsic");
+	wake_lock_init(&pm_data->tx_async_wake, WAKE_LOCK_SUSPEND, "tx_hsic");
+
+#if defined(CONFIG_SLP)
+	device_init_wakeup(pm_data->miscdev.this_device, true);
+#endif
+
+	return 0;
+
+err_create_wq:
+	disable_irq_wake(pm_data->irq_link_hostwake);
+err_set_wake_irq:
+	free_irq(pm_data->irq_link_hostwake, (void *)pm_data);
+err_request_irq:
+	misc_deregister(&pm_data->miscdev);
+err_misc_register:
+	kfree(pm_data);
+	return r;
+}
+
+struct link_device *hsic_create_link_device(void *data)
+{
+	int ret;
+	struct usb_link_device *usb_ld;
+	struct link_device *ld;
+
+	usb_ld = kzalloc(sizeof(struct usb_link_device), GFP_KERNEL);
+	if (!usb_ld)
+		return NULL;
+
+	INIT_LIST_HEAD(&usb_ld->ld.list);
+	skb_queue_head_init(&usb_ld->ld.sk_fmt_tx_q);
+	skb_queue_head_init(&usb_ld->ld.sk_raw_tx_q);
+
+	ld = &usb_ld->ld;
+
+	ld->name = "usb";
+	ld->init_comm = usb_init_communication;
+	ld->terminate_comm = usb_terminate_communication;
+	ld->send = usb_send;
+	ld->com_state = COM_NONE;
+	ld->raw_tx_suspended = false;
+	init_completion(&ld->raw_tx_resumed_by_cp);
+
+	ld->tx_wq = create_singlethread_workqueue("usb_tx_wq");
+	if (!ld->tx_wq) {
+		mif_err("fail to create work Q.\n");
+		goto err;
+	}
+
+	INIT_DELAYED_WORK(&ld->tx_delayed_work, usb_tx_work);
+	INIT_DELAYED_WORK(&usb_ld->rx_retry_work, usb_rx_retry_work);
+	usb_ld->rx_retry_cnt = 0;
+
+	/* create link pm device */
+	ret = usb_link_pm_init(usb_ld, data);
+	if (ret)
+		goto err;
+
+	ret = if_usb_init(ld);
+	if (ret)
+		goto err;
+
+	mif_info("%s : create_link_device DONE\n", usb_ld->ld.name);
+	return (void *)ld;
+err:
+	kfree(usb_ld);
+	return NULL;
+}
+
+static void __exit if_usb_exit(void)
+{
+	usb_deregister(&if_usb_driver);
+}
diff --git a/drivers/misc/modem_if/modem_link_device_hsic.h b/drivers/misc/modem_if/modem_link_device_hsic.h
new file mode 100755
index 0000000..8aec412
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_hsic.h
@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#ifndef __MODEM_LINK_DEVICE_USB_H__
+#define __MODEM_LINK_DEVICE_USB_H__
+
+
+enum {
+	IF_USB_BOOT_EP = 0,
+	IF_USB_FMT_EP = 0,
+	IF_USB_RAW_EP,
+	IF_USB_RFS_EP,
+	IF_USB_CMD_EP,
+	IF_USB_DEVNUM_MAX,
+};
+
+/* each pipe has 2 ep for in/out */
+#define LINKPM_DEV_NUM	(IF_USB_DEVNUM_MAX * 2)
+/******************/
+/* xmm6260 specific */
+
+#define IOCTL_LINK_CONTROL_ENABLE	_IO('o', 0x30)
+#define IOCTL_LINK_CONTROL_ACTIVE	_IO('o', 0x31)
+#define IOCTL_LINK_GET_HOSTWAKE		_IO('o', 0x32)
+#define IOCTL_LINK_CONNECTED		_IO('o', 0x33)
+#define IOCTL_LINK_SET_BIAS_CLEAR	_IO('o', 0x34)
+
+/* VID,PID for IMC - XMM6260, XMM6262*/
+#define IMC_BOOT_VID		0x058b
+#define IMC_BOOT_PID		0x0041
+#define IMC_MAIN_VID		0x1519
+#define IMC_MAIN_PID		0x0020
+/* VID,PID for STE - M7400 */
+#define STE_BOOT_VID		0x04cc
+#define STE_BOOT_PID		0x7400
+#define STE_MAIN_VID		0x04cc
+#define STE_MAIN_PID		0x2333
+
+enum {
+	BOOT_DOWN = 0,
+	IPC_CHANNEL
+};
+
+enum ch_state {
+	STATE_SUSPENDED,
+	STATE_RESUMED,
+};
+
+#define HOSTWAKE_TRIGLEVEL	0
+/******************/
+
+struct link_pm_info {
+	struct usb_link_device *usb_ld;
+};
+
+struct usb_id_info {
+	int intf_id;
+	struct usb_link_device *usb_ld;
+};
+
+struct link_pm_data {
+	struct miscdevice miscdev;
+	struct usb_link_device *usb_ld;
+	unsigned irq_link_hostwake;
+	int (*link_ldo_enable)(bool);
+	unsigned gpio_link_enable;
+	unsigned gpio_link_active;
+	unsigned gpio_link_hostwake;
+	unsigned gpio_link_slavewake;
+	int (*link_reconnect)(void);
+	int link_reconnect_cnt;
+
+	struct workqueue_struct *wq;
+	struct completion active_done;
+	struct delayed_work link_pm_work;
+	struct delayed_work link_pm_start;
+	struct delayed_work link_reconnect_work;
+	bool resume_requested;
+	bool link_pm_active;
+	int resume_retry_cnt;
+
+	struct wake_lock l2_wake;
+	struct wake_lock boot_wake;
+	struct wake_lock rpm_wake;
+	struct wake_lock tx_async_wake;
+	struct notifier_block pm_notifier;
+	bool dpm_suspending;
+
+	/* Host wakeup toggle debugging */
+	unsigned ipc_debug_cnt;
+	unsigned long tx_cnt;
+	unsigned long rx_cnt;
+};
+
+struct if_usb_devdata {
+	struct usb_interface *data_intf;
+	struct usb_link_device *usb_ld;
+	struct usb_device *usbdev;
+	unsigned int tx_pipe;
+	unsigned int rx_pipe;
+	u8 disconnected;
+
+	int format;
+	struct urb *urb;
+	void *rx_buf;
+	unsigned int rx_buf_size;
+	enum ch_state state;
+};
+
+struct usb_link_device {
+	/*COMMON LINK DEVICE*/
+	struct link_device ld;
+
+	/*USB SPECIFIC LINK DEVICE*/
+	struct usb_device	*usbdev;
+	struct if_usb_devdata	devdata[IF_USB_DEVNUM_MAX];
+	unsigned int		dev_count;
+	unsigned int		suspended;
+	int if_usb_connected;
+
+	/*It is same with if_usb_connected, but we need to check the side-effect
+	 * from timming changed, it will merge with if_usb_connect variable.*/
+	int if_usb_connected_last;
+
+	bool if_usb_is_main; /* boot,down(false) or main(true) */
+
+	/* LINK PM DEVICE DATA */
+	struct link_pm_data *link_pm_data;
+
+	/*RX retry work by -ENOMEM*/
+	struct delayed_work rx_retry_work;
+	struct urb *retry_urb;
+	unsigned rx_retry_cnt;
+};
+/* converts from struct link_device* to struct xxx_link_device* */
+#define to_usb_link_device(linkdev) \
+			container_of(linkdev, struct usb_link_device, ld)
+
+
+#ifdef FOR_TEGRA
+extern void tegra_ehci_txfilltuning(void);
+#endif
+
+#endif
diff --git a/drivers/misc/modem_if/modem_link_device_mipi.c b/drivers/misc/modem_if/modem_link_device_mipi.c
new file mode 100644
index 0000000..f2804e9
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_mipi.c
@@ -0,0 +1,1418 @@
+/* /linux/drivers/new_modem_if/link_dev_mipi.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+#include <linux/irq.h>
+#include <linux/poll.h>
+#include <linux/gpio.h>
+#include <linux/if_arp.h>
+#include <linux/wakelock.h>
+#include <linux/semaphore.h>
+#include <linux/hsi_driver_if.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_link_device_mipi.h"
+#include "modem_utils.h"
+
+static int mipi_hsi_init_communication(struct link_device *ld,
+			struct io_device *iod)
+{
+	struct mipi_link_device *mipi_ld = to_mipi_link_device(ld);
+
+	switch (iod->format) {
+	case IPC_FMT:
+		return hsi_init_handshake(mipi_ld, HSI_INIT_MODE_NORMAL);
+
+	case IPC_BOOT:
+		return hsi_init_handshake(mipi_ld,
+					HSI_INIT_MODE_FLASHLESS_BOOT);
+
+	case IPC_RAMDUMP:
+		return hsi_init_handshake(mipi_ld,
+					HSI_INIT_MODE_CP_RAMDUMP);
+
+	case IPC_RFS:
+	case IPC_RAW:
+	default:
+		return 0;
+	}
+}
+
+static void mipi_hsi_terminate_communication(
+			struct link_device *ld, struct io_device *iod)
+{
+	struct mipi_link_device *mipi_ld = to_mipi_link_device(ld);
+
+	switch (iod->format) {
+	case IPC_BOOT:
+		if (&mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL].opened)
+			if_hsi_close_channel(&mipi_ld->hsi_channles[
+					HSI_FLASHLESS_CHANNEL]);
+		break;
+
+	case IPC_RAMDUMP:
+		if (&mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL].opened)
+			if_hsi_close_channel(&mipi_ld->hsi_channles[
+					HSI_CP_RAMDUMP_CHANNEL]);
+		break;
+
+	case IPC_FMT:
+	case IPC_RFS:
+	case IPC_RAW:
+	default:
+		break;
+	}
+}
+
+static int mipi_hsi_send(struct link_device *ld, struct io_device *iod,
+			struct sk_buff *skb)
+{
+	int ret;
+	struct mipi_link_device *mipi_ld = to_mipi_link_device(ld);
+
+	struct sk_buff_head *txq;
+
+	switch (iod->format) {
+	case IPC_RAW:
+		txq = &ld->sk_raw_tx_q;
+		break;
+
+	case IPC_RAMDUMP:
+		ret = if_hsi_write(&mipi_ld->hsi_channles[
+					HSI_CP_RAMDUMP_CHANNEL],
+					(u32 *)skb->data, skb->len);
+		if (ret < 0) {
+			mif_err("[MIPI-HSI] write fail : %d\n", ret);
+			dev_kfree_skb_any(skb);
+			return ret;
+		} else
+			mif_debug("[MIPI-HSI] write Done\n");
+		dev_kfree_skb_any(skb);
+		return ret;
+
+	case IPC_BOOT:
+		ret = if_hsi_write(&mipi_ld->hsi_channles[
+					HSI_FLASHLESS_CHANNEL],
+					(u32 *)skb->data, skb->len);
+		if (ret < 0) {
+			mif_err("[MIPI-HSI] write fail : %d\n", ret);
+			dev_kfree_skb_any(skb);
+			return ret;
+		} else
+			mif_debug("[MIPI-HSI] write Done\n");
+		dev_kfree_skb_any(skb);
+		return ret;
+
+	case IPC_FMT:
+	case IPC_RFS:
+	default:
+		txq = &ld->sk_fmt_tx_q;
+		break;
+	}
+
+	/* save io device */
+	skbpriv(skb)->iod = iod;
+	/* en queue skb data */
+	skb_queue_tail(txq, skb);
+
+	queue_work(ld->tx_wq, &ld->tx_work);
+	return skb->len;
+}
+
+static void mipi_hsi_tx_work(struct work_struct *work)
+{
+	int ret;
+	struct link_device *ld = container_of(work, struct link_device,
+				tx_work);
+	struct mipi_link_device *mipi_ld = to_mipi_link_device(ld);
+	struct io_device *iod;
+	struct sk_buff *fmt_skb;
+	struct sk_buff *raw_skb;
+	int send_channel = 0;
+
+	while (ld->sk_fmt_tx_q.qlen || ld->sk_raw_tx_q.qlen) {
+		mif_debug("[MIPI-HSI] fmt qlen : %d, raw qlen:%d\n",
+				ld->sk_fmt_tx_q.qlen, ld->sk_raw_tx_q.qlen);
+
+		fmt_skb = skb_dequeue(&ld->sk_fmt_tx_q);
+		if (fmt_skb) {
+			iod = skbpriv(fmt_skb)->iod;
+
+			mif_debug("[MIPI-HSI] dequeue. fmt qlen : %d\n",
+						ld->sk_fmt_tx_q.qlen);
+
+			if (ld->com_state != COM_ONLINE) {
+				mif_err("[MIPI-HSI] CP not ready\n");
+				skb_queue_head(&ld->sk_fmt_tx_q, fmt_skb);
+				return;
+			}
+
+			switch (iod->format) {
+			case IPC_FMT:
+				send_channel = HSI_FMT_CHANNEL;
+				break;
+
+			case IPC_RFS:
+				send_channel = HSI_RFS_CHANNEL;
+				break;
+
+			case IPC_BOOT:
+				send_channel = HSI_FLASHLESS_CHANNEL;
+				break;
+
+			case IPC_RAMDUMP:
+				send_channel = HSI_CP_RAMDUMP_CHANNEL;
+				break;
+
+			default:
+				break;
+			}
+			ret = if_hsi_protocol_send(mipi_ld, send_channel,
+					(u32 *)fmt_skb->data, fmt_skb->len);
+			if (ret < 0) {
+				/* TODO: Re Enqueue */
+				mif_err("[MIPI-HSI] write fail : %d\n", ret);
+			}  else
+				mif_debug("[MIPI-HSI] write Done\n");
+
+			dev_kfree_skb_any(fmt_skb);
+		}
+
+		raw_skb = skb_dequeue(&ld->sk_raw_tx_q);
+		if (raw_skb) {
+			if (ld->com_state != COM_ONLINE) {
+				mif_err("[MIPI-HSI] RAW CP not ready\n");
+				skb_queue_head(&ld->sk_raw_tx_q, raw_skb);
+				return;
+			}
+
+			mif_debug("[MIPI-HSI] dequeue. raw qlen:%d\n",
+						ld->sk_raw_tx_q.qlen);
+
+			ret = if_hsi_protocol_send(mipi_ld, HSI_RAW_CHANNEL,
+					(u32 *)raw_skb->data, raw_skb->len);
+			if (ret < 0) {
+				/* TODO: Re Enqueue */
+				mif_err("[MIPI-HSI] write fail : %d\n", ret);
+			}  else
+				mif_debug("[MIPI-HSI] write Done\n");
+
+			dev_kfree_skb_any(raw_skb);
+		}
+	}
+}
+
+static int __devinit if_hsi_probe(struct hsi_device *dev);
+static struct hsi_device_driver if_hsi_driver = {
+	.ctrl_mask = ANY_HSI_CONTROLLER,
+	.probe = if_hsi_probe,
+	.driver = {
+		.name = "if_hsi_driver"
+	},
+};
+
+static int if_hsi_set_wakeline(struct if_hsi_channel *channel,
+			unsigned int state)
+{
+	int ret;
+
+	spin_lock_bh(&channel->acwake_lock);
+	if (channel->acwake == state) {
+		spin_unlock_bh(&channel->acwake_lock);
+		return 0;
+	}
+
+	ret = hsi_ioctl(channel->dev, state ?
+		HSI_IOCTL_ACWAKE_UP : HSI_IOCTL_ACWAKE_DOWN, NULL);
+	if (ret) {
+		mif_err("[MIPI-HSI] ACWAKE(%d) setting fail : %d\n", state,
+					ret);
+		/* duplicate operation */
+		if (ret == -EPERM)
+			channel->acwake = state;
+		spin_unlock_bh(&channel->acwake_lock);
+		return ret;
+	}
+
+	channel->acwake = state;
+	spin_unlock_bh(&channel->acwake_lock);
+
+	mif_debug("[MIPI-HSI] ACWAKE_%d(%d)\n", channel->channel_id, state);
+	return 0;
+}
+
+static void if_hsi_acwake_down_func(unsigned long data)
+{
+	int i;
+	struct if_hsi_channel *channel;
+	struct mipi_link_device *mipi_ld = (struct mipi_link_device *)data;
+
+	mif_debug("[MIPI-HSI]\n");
+
+	for (i = 0; i < HSI_NUM_OF_USE_CHANNELS; i++) {
+		channel = &mipi_ld->hsi_channles[i];
+
+		if ((channel->send_step == STEP_IDLE) &&
+			(channel->recv_step == STEP_IDLE)) {
+			if_hsi_set_wakeline(channel, 0);
+		} else {
+			mod_timer(&mipi_ld->hsi_acwake_down_timer, jiffies +
+					HSI_ACWAKE_DOWN_TIMEOUT);
+			mif_debug("[MIPI-HSI] mod_timer done(%d)\n",
+					HSI_ACWAKE_DOWN_TIMEOUT);
+			return;
+		}
+	}
+}
+
+static int if_hsi_open_channel(struct if_hsi_channel *channel)
+{
+	int ret;
+
+	if (channel->opened) {
+		mif_debug("[MIPI-HSI] channel %d is already opened\n",
+					channel->channel_id);
+		return 0;
+	}
+
+	ret = hsi_open(channel->dev);
+	if (ret) {
+		mif_err("[MIPI-HSI] hsi_open fail : %d\n", ret);
+		return ret;
+	}
+	channel->opened = 1;
+
+	channel->send_step = STEP_IDLE;
+	channel->recv_step = STEP_IDLE;
+
+	mif_debug("[MIPI-HSI] hsi_open Done : %d\n", channel->channel_id);
+	return 0;
+}
+
+static int if_hsi_close_channel(struct if_hsi_channel *channel)
+{
+	unsigned long int flags;
+
+	if (!channel->opened) {
+		mif_debug("[MIPI-HSI] channel %d is already closed\n",
+					channel->channel_id);
+		return 0;
+	}
+
+	if_hsi_set_wakeline(channel, 0);
+	hsi_write_cancel(channel->dev);
+	hsi_read_cancel(channel->dev);
+
+	spin_lock_irqsave(&channel->tx_state_lock, flags);
+	channel->tx_state &= ~HSI_CHANNEL_TX_STATE_WRITING;
+	spin_unlock_irqrestore(&channel->tx_state_lock, flags);
+	spin_lock_irqsave(&channel->rx_state_lock, flags);
+	channel->rx_state &= ~HSI_CHANNEL_RX_STATE_READING;
+	spin_unlock_irqrestore(&channel->rx_state_lock, flags);
+
+	hsi_close(channel->dev);
+	channel->opened = 0;
+
+	channel->send_step = STEP_CLOSED;
+	channel->recv_step = STEP_CLOSED;
+
+	mif_debug("[MIPI-HSI] hsi_close Done : %d\n", channel->channel_id);
+	return 0;
+}
+
+static void mipi_hsi_start_work(struct work_struct *work)
+{
+	int ret;
+	u32 start_cmd = 0xC2;
+	struct mipi_link_device *mipi_ld =
+			container_of(work, struct mipi_link_device,
+						start_work.work);
+
+	ret = if_hsi_protocol_send(mipi_ld, HSI_CMD_CHANNEL, &start_cmd, 1);
+	if (ret < 0) {
+		/* TODO: Re Enqueue */
+		mif_err("[MIPI-HSI] First write fail : %d\n", ret);
+	}  else {
+		mif_debug("[MIPI-HSI] First write Done : %d\n", ret);
+		mipi_ld->ld.com_state = COM_ONLINE;
+	}
+}
+
+static int hsi_init_handshake(struct mipi_link_device *mipi_ld, int mode)
+{
+	int ret;
+	int i;
+	struct hst_ctx tx_config;
+	struct hsr_ctx rx_config;
+
+	switch (mode) {
+	case HSI_INIT_MODE_NORMAL:
+		if (timer_pending(&mipi_ld->hsi_acwake_down_timer))
+			del_timer(&mipi_ld->hsi_acwake_down_timer);
+
+		for (i = 0; i < HSI_NUM_OF_USE_CHANNELS; i++) {
+			if (mipi_ld->hsi_channles[i].opened) {
+				hsi_write_cancel(mipi_ld->hsi_channles[i].dev);
+				hsi_read_cancel(mipi_ld->hsi_channles[i].dev);
+			} else {
+				ret = if_hsi_open_channel(
+						&mipi_ld->hsi_channles[i]);
+				if (ret)
+					return ret;
+			}
+
+			hsi_ioctl(mipi_ld->hsi_channles[i].dev,
+						HSI_IOCTL_GET_TX, &tx_config);
+			tx_config.mode = 2;
+			tx_config.divisor = 0; /* Speed : 96MHz */
+			tx_config.channels = HSI_MAX_CHANNELS;
+			hsi_ioctl(mipi_ld->hsi_channles[i].dev,
+						HSI_IOCTL_SET_TX, &tx_config);
+
+			hsi_ioctl(mipi_ld->hsi_channles[i].dev,
+						HSI_IOCTL_GET_RX, &rx_config);
+			rx_config.mode = 2;
+			rx_config.divisor = 0; /* Speed : 96MHz */
+			rx_config.channels = HSI_MAX_CHANNELS;
+			hsi_ioctl(mipi_ld->hsi_channles[i].dev,
+						HSI_IOCTL_SET_RX, &rx_config);
+			mif_debug("[MIPI-HSI] Set TX/RX MIPI-HSI\n");
+
+			hsi_ioctl(mipi_ld->hsi_channles[i].dev,
+				HSI_IOCTL_SET_ACREADY_NORMAL, NULL);
+			mif_debug("[MIPI-HSI] ACREADY_NORMAL\n");
+		}
+
+		if (mipi_ld->ld.com_state != COM_ONLINE)
+			mipi_ld->ld.com_state = COM_HANDSHAKE;
+
+		ret = hsi_read(mipi_ld->hsi_channles[HSI_CONTROL_CHANNEL].dev,
+			mipi_ld->hsi_channles[HSI_CONTROL_CHANNEL].rx_data,
+					1);
+		if (ret)
+			mif_err("[MIPI-HSI] hsi_read fail : %d\n", ret);
+
+		if (mipi_ld->ld.com_state != COM_ONLINE)
+			schedule_delayed_work(&mipi_ld->start_work, 3 * HZ);
+
+		mif_debug("[MIPI-HSI] hsi_init_handshake Done : MODE_NORMAL\n");
+		return 0;
+
+	case HSI_INIT_MODE_FLASHLESS_BOOT:
+		mipi_ld->ld.com_state = COM_BOOT;
+
+		if (mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL].opened) {
+			hsi_write_cancel(mipi_ld->hsi_channles[
+					HSI_FLASHLESS_CHANNEL].dev);
+			hsi_read_cancel(mipi_ld->hsi_channles[
+					HSI_FLASHLESS_CHANNEL].dev);
+		} else {
+			ret = if_hsi_open_channel(
+				&mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL]);
+			if (ret)
+				return ret;
+		}
+
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL].dev,
+					HSI_IOCTL_GET_TX, &tx_config);
+		tx_config.mode = 2;
+		tx_config.divisor = 0; /* Speed : 96MHz */
+		tx_config.channels = 1;
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL].dev,
+					HSI_IOCTL_SET_TX, &tx_config);
+
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL].dev,
+					HSI_IOCTL_GET_RX, &rx_config);
+		rx_config.mode = 2;
+		rx_config.divisor = 0; /* Speed : 96MHz */
+		rx_config.channels = 1;
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL].dev,
+					HSI_IOCTL_SET_RX, &rx_config);
+		mif_debug("[MIPI-HSI] Set TX/RX MIPI-HSI\n");
+
+		if (!wake_lock_active(&mipi_ld->wlock)) {
+			wake_lock(&mipi_ld->wlock);
+			mif_debug("[MIPI-HSI] wake_lock\n");
+		}
+
+		if_hsi_set_wakeline(
+			&mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL], 1);
+
+		ret = hsi_read(mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL].dev,
+			mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL].rx_data,
+					HSI_FLASHBOOT_ACK_LEN / 4);
+		if (ret)
+			mif_err("[MIPI-HSI] hsi_read fail : %d\n", ret);
+
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_FLASHLESS_CHANNEL].dev,
+			HSI_IOCTL_SET_ACREADY_NORMAL, NULL);
+
+		mif_debug("[MIPI-HSI] hsi_init_handshake Done : FLASHLESS_BOOT\n");
+		return 0;
+
+	case HSI_INIT_MODE_CP_RAMDUMP:
+		mipi_ld->ld.com_state = COM_CRASH;
+
+		if (mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL].opened) {
+			hsi_write_cancel(mipi_ld->hsi_channles[
+					HSI_CP_RAMDUMP_CHANNEL].dev);
+			hsi_read_cancel(mipi_ld->hsi_channles[
+					HSI_CP_RAMDUMP_CHANNEL].dev);
+		} else {
+			ret = if_hsi_open_channel(
+				&mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL]);
+			if (ret)
+				return ret;
+		}
+
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL].dev,
+					HSI_IOCTL_GET_TX, &tx_config);
+		tx_config.mode = 2;
+		tx_config.divisor = 0; /* Speed : 96MHz */
+		tx_config.channels = 1;
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL].dev,
+					HSI_IOCTL_SET_TX, &tx_config);
+
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL].dev,
+					HSI_IOCTL_GET_RX, &rx_config);
+		rx_config.mode = 2;
+		rx_config.divisor = 0; /* Speed : 96MHz */
+		rx_config.channels = 1;
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL].dev,
+					HSI_IOCTL_SET_RX, &rx_config);
+		mif_debug("[MIPI-HSI] Set TX/RX MIPI-HSI\n");
+
+		if (!wake_lock_active(&mipi_ld->wlock)) {
+			wake_lock(&mipi_ld->wlock);
+			mif_debug("[MIPI-HSI] wake_lock\n");
+		}
+
+		if_hsi_set_wakeline(
+			&mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL], 1);
+
+		ret = hsi_read(
+			mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL].dev,
+			mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL].rx_data,
+					DUMP_ERR_INFO_SIZE);
+		if (ret)
+			mif_err("[MIPI-HSI] hsi_read fail : %d\n", ret);
+
+		hsi_ioctl(mipi_ld->hsi_channles[HSI_CP_RAMDUMP_CHANNEL].dev,
+			HSI_IOCTL_SET_ACREADY_NORMAL, NULL);
+
+		mif_debug("[MIPI-HSI] hsi_init_handshake Done : RAMDUMP\n");
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static u32 if_hsi_create_cmd(u32 cmd_type, int ch, void *arg)
+{
+	u32 cmd = 0;
+	unsigned int size = 0;
+
+	switch (cmd_type) {
+	case HSI_LL_MSG_BREAK:
+		return 0;
+
+	case HSI_LL_MSG_CONN_CLOSED:
+		cmd =  ((HSI_LL_MSG_CONN_CLOSED & 0x0000000F) << 28)
+					|((ch & 0x000000FF) << 24);
+		return cmd;
+
+	case HSI_LL_MSG_ACK:
+		size = *(unsigned int *)arg;
+
+		cmd = ((HSI_LL_MSG_ACK & 0x0000000F) << 28)
+			|((ch & 0x000000FF) << 24) | ((size & 0x00FFFFFF));
+		return cmd;
+
+	case HSI_LL_MSG_NAK:
+		cmd = ((HSI_LL_MSG_NAK & 0x0000000F) << 28)
+					|((ch & 0x000000FF) << 24);
+		return cmd;
+
+	case HSI_LL_MSG_OPEN_CONN_OCTET:
+		size = *(unsigned int *)arg;
+
+		cmd = ((HSI_LL_MSG_OPEN_CONN_OCTET & 0x0000000F)
+				<< 28) | ((ch & 0x000000FF) << 24)
+					| ((size & 0x00FFFFFF));
+		return cmd;
+
+	case HSI_LL_MSG_OPEN_CONN:
+	case HSI_LL_MSG_CONF_RATE:
+	case HSI_LL_MSG_CANCEL_CONN:
+	case HSI_LL_MSG_CONN_READY:
+	case HSI_LL_MSG_ECHO:
+	case HSI_LL_MSG_INFO_REQ:
+	case HSI_LL_MSG_INFO:
+	case HSI_LL_MSG_CONFIGURE:
+	case HSI_LL_MSG_ALLOCATE_CH:
+	case HSI_LL_MSG_RELEASE_CH:
+	case HSI_LL_MSG_INVALID:
+	default:
+		mif_err("[MIPI-HSI] ERROR... CMD Not supported : %08x\n",
+					cmd_type);
+		return -EINVAL;
+	}
+}
+
+static void if_hsi_cmd_work(struct work_struct *work)
+{
+	int ret;
+	unsigned long int flags;
+	struct mipi_link_device *mipi_ld =
+			container_of(work, struct mipi_link_device, cmd_work);
+	struct if_hsi_channel *channel =
+			&mipi_ld->hsi_channles[HSI_CONTROL_CHANNEL];
+	struct if_hsi_command *hsi_cmd;
+
+	mif_debug("[MIPI-HSI] cmd_work\n");
+
+	do {
+		spin_lock_irqsave(&mipi_ld->list_cmd_lock, flags);
+		if (!list_empty(&mipi_ld->list_of_hsi_cmd)) {
+			hsi_cmd = list_entry(mipi_ld->list_of_hsi_cmd.next,
+					struct if_hsi_command, list);
+			list_del(&hsi_cmd->list);
+			spin_unlock_irqrestore(&mipi_ld->list_cmd_lock, flags);
+
+			channel->send_step = STEP_TX;
+			if_hsi_set_wakeline(channel, 1);
+			mod_timer(&mipi_ld->hsi_acwake_down_timer, jiffies +
+					HSI_ACWAKE_DOWN_TIMEOUT);
+		} else {
+			spin_unlock_irqrestore(&mipi_ld->list_cmd_lock, flags);
+			channel->send_step = STEP_IDLE;
+			break;
+		}
+		mif_debug("[MIPI-HSI] take command : %08x\n", hsi_cmd->command);
+
+		ret = if_hsi_write(channel, &hsi_cmd->command, 4);
+		if (ret < 0) {
+			mif_err("[MIPI-HSI] write command fail : %d\n", ret);
+			if_hsi_set_wakeline(channel, 0);
+			channel->send_step = STEP_IDLE;
+			return;
+		}
+		mif_debug("[MIPI-HSI] SEND CMD : %08x\n", hsi_cmd->command);
+
+		kfree(hsi_cmd);
+	} while (true);
+}
+
+static int if_hsi_send_command(struct mipi_link_device *mipi_ld,
+			u32 cmd_type, int ch, u32 param)
+{
+	unsigned long int flags;
+	struct if_hsi_command *hsi_cmd;
+
+	hsi_cmd = kmalloc(sizeof(struct if_hsi_command), GFP_ATOMIC);
+	if (!hsi_cmd) {
+		mif_err("[MIPI-HSI] hsi_cmd kmalloc fail\n");
+		return -ENOMEM;
+	}
+	INIT_LIST_HEAD(&hsi_cmd->list);
+
+	hsi_cmd->command = if_hsi_create_cmd(cmd_type, ch, &param);
+	mif_debug("[MIPI-HSI] made command : %08x\n", hsi_cmd->command);
+
+	spin_lock_irqsave(&mipi_ld->list_cmd_lock, flags);
+	list_add_tail(&hsi_cmd->list, &mipi_ld->list_of_hsi_cmd);
+	spin_unlock_irqrestore(&mipi_ld->list_cmd_lock, flags);
+
+	mif_debug("[MIPI-HSI] queue_work : cmd_work\n");
+	queue_work(mipi_ld->mipi_wq, &mipi_ld->cmd_work);
+
+	return 0;
+}
+
+static int if_hsi_decode_cmd(u32 *cmd_data, u32 *cmd, u32 *ch,
+			u32 *param)
+{
+	u32 data = *cmd_data;
+	u8 lrc_cal, lrc_act;
+	u8 val1, val2, val3;
+
+	*cmd = ((data & 0xF0000000) >> 28);
+	switch (*cmd) {
+	case HSI_LL_MSG_BREAK:
+		mif_err("[MIPI-HSI] Command MSG_BREAK Received\n");
+		return -1;
+
+	case HSI_LL_MSG_OPEN_CONN:
+		*ch = ((data & 0x0F000000) >> 24);
+		*param = ((data & 0x00FFFF00) >> 8);
+		val1 = ((data & 0xFF000000) >> 24);
+		val2 = ((data & 0x00FF0000) >> 16);
+		val3 = ((data & 0x0000FF00) >>  8);
+		lrc_act = (data & 0x000000FF);
+		lrc_cal = val1 ^ val2 ^ val3;
+
+		if (lrc_cal != lrc_act) {
+			mif_err("[MIPI-HSI] CAL is broken\n");
+			return -1;
+		}
+		return 0;
+
+	case HSI_LL_MSG_CONN_READY:
+	case HSI_LL_MSG_CONN_CLOSED:
+	case HSI_LL_MSG_CANCEL_CONN:
+	case HSI_LL_MSG_NAK:
+		*ch = ((data & 0x0F000000) >> 24);
+		return 0;
+
+	case HSI_LL_MSG_ACK:
+		*ch = ((data & 0x0F000000) >> 24);
+		*param = (data & 0x00FFFFFF);
+		return 0;
+
+	case HSI_LL_MSG_CONF_RATE:
+		*ch = ((data & 0x0F000000) >> 24);
+		*param = ((data & 0x0F000000) >> 24);
+		return 0;
+
+	case HSI_LL_MSG_OPEN_CONN_OCTET:
+		*ch = ((data & 0x0F000000) >> 24);
+		*param = (data & 0x00FFFFFF);
+		return 0;
+
+	case HSI_LL_MSG_ECHO:
+	case HSI_LL_MSG_INFO_REQ:
+	case HSI_LL_MSG_INFO:
+	case HSI_LL_MSG_CONFIGURE:
+	case HSI_LL_MSG_ALLOCATE_CH:
+	case HSI_LL_MSG_RELEASE_CH:
+	case HSI_LL_MSG_INVALID:
+	default:
+		mif_err("[MIPI-HSI] Invalid command received : %08x\n", *cmd);
+		*cmd = HSI_LL_MSG_INVALID;
+		*ch  = HSI_LL_INVALID_CHANNEL;
+		return -1;
+	}
+	return 0;
+}
+
+static int if_hsi_rx_cmd_handle(struct mipi_link_device *mipi_ld, u32 cmd,
+			u32 ch, u32 param)
+{
+	int ret;
+	struct if_hsi_channel *channel = &mipi_ld->hsi_channles[ch];
+
+	mif_debug("[MIPI-HSI] if_hsi_rx_cmd_handle cmd=0x%x, ch=%d, param=%d\n",
+				cmd, ch, param);
+
+	switch (cmd) {
+	case HSI_LL_MSG_OPEN_CONN_OCTET:
+		switch (channel->recv_step) {
+		case STEP_IDLE:
+			channel->recv_step = STEP_TO_ACK;
+
+			if (!wake_lock_active(&mipi_ld->wlock)) {
+				wake_lock(&mipi_ld->wlock);
+				mif_debug("[MIPI-HSI] wake_lock\n");
+			}
+
+			if_hsi_set_wakeline(channel, 1);
+			mod_timer(&mipi_ld->hsi_acwake_down_timer, jiffies +
+						HSI_ACWAKE_DOWN_TIMEOUT);
+			mif_debug("[MIPI-HSI] mod_timer done(%d)\n",
+						HSI_ACWAKE_DOWN_TIMEOUT);
+
+			ret = if_hsi_send_command(mipi_ld, HSI_LL_MSG_ACK, ch,
+						param);
+			if (ret) {
+				mif_err("[MIPI-HSI] if_hsi_send_command fail : %d\n",
+							ret);
+				return ret;
+			}
+
+			channel->packet_size = param;
+			channel->recv_step = STEP_RX;
+			if (param % 4)
+				param += (4 - (param % 4));
+			channel->rx_count = param;
+			ret = hsi_read(channel->dev, channel->rx_data,
+						channel->rx_count / 4);
+			if (ret) {
+				mif_err("[MIPI-HSI] hsi_read fail : %d\n", ret);
+				return ret;
+			}
+			return 0;
+
+		case STEP_NOT_READY:
+			ret = if_hsi_send_command(mipi_ld, HSI_LL_MSG_NAK, ch,
+						param);
+			if (ret) {
+				mif_err("[MIPI-HSI] if_hsi_send_command fail : %d\n",
+							ret);
+				return ret;
+			}
+			return 0;
+
+		default:
+			mif_err("[MIPI-HSI] wrong state : %08x, recv_step : %d\n",
+						cmd, channel->recv_step);
+			return -1;
+		}
+
+	case HSI_LL_MSG_ACK:
+	case HSI_LL_MSG_NAK:
+		switch (channel->send_step) {
+		case STEP_WAIT_FOR_ACK:
+		case STEP_SEND_OPEN_CONN:
+			if (cmd == HSI_LL_MSG_ACK) {
+				channel->send_step = STEP_TX;
+				channel->got_nack = 0;
+				mif_debug("[MIPI-HSI] got ack\n");
+			} else {
+				channel->send_step = STEP_WAIT_FOR_ACK;
+				channel->got_nack = 1;
+				mif_debug("[MIPI-HSI] got nack\n");
+			}
+
+			up(&channel->ack_done_sem);
+			return 0;
+
+		default:
+			mif_err("[MIPI-HSI] wrong state : %08x\n", cmd);
+			return -1;
+		}
+
+	case HSI_LL_MSG_CONN_CLOSED:
+		switch (channel->send_step) {
+		case STEP_TX:
+		case STEP_WAIT_FOR_CONN_CLOSED:
+			mif_debug("[MIPI-HSI] got close\n");
+
+			channel->send_step = STEP_IDLE;
+			up(&channel->close_conn_done_sem);
+			return 0;
+
+		default:
+			mif_err("[MIPI-HSI] wrong state : %08x\n", cmd);
+			return -1;
+		}
+
+	case HSI_LL_MSG_OPEN_CONN:
+	case HSI_LL_MSG_ECHO:
+	case HSI_LL_MSG_CANCEL_CONN:
+	case HSI_LL_MSG_CONF_RATE:
+	default:
+		mif_err("[MIPI-HSI] ERROR... CMD Not supported : %08x\n", cmd);
+		return -EINVAL;
+	}
+}
+
+static int if_hsi_protocol_send(struct mipi_link_device *mipi_ld, int ch,
+			u32 *data, unsigned int len)
+{
+	int ret;
+	int retry_count = 0;
+	int ack_timeout_cnt = 0;
+	struct io_device *iod;
+	struct if_hsi_channel *channel = &mipi_ld->hsi_channles[ch];
+
+	if (channel->send_step != STEP_IDLE) {
+		mif_err("[MIPI-HSI] send step is not IDLE : %d\n",
+					channel->send_step);
+		return -EBUSY;
+	}
+	channel->send_step = STEP_SEND_OPEN_CONN;
+
+	if (!wake_lock_active(&mipi_ld->wlock)) {
+		wake_lock(&mipi_ld->wlock);
+		mif_debug("[MIPI-HSI] wake_lock\n");
+	}
+
+	if_hsi_set_wakeline(channel, 1);
+	mod_timer(&mipi_ld->hsi_acwake_down_timer, jiffies +
+					HSI_ACWAKE_DOWN_TIMEOUT);
+	mif_debug("[MIPI-HSI] mod_timer done(%d)\n",
+				HSI_ACWAKE_DOWN_TIMEOUT);
+
+retry_send:
+
+	ret = if_hsi_send_command(mipi_ld, HSI_LL_MSG_OPEN_CONN_OCTET, ch,
+				len);
+	if (ret) {
+		mif_err("[MIPI-HSI] if_hsi_send_command fail : %d\n", ret);
+		if_hsi_set_wakeline(channel, 0);
+		channel->send_step = STEP_IDLE;
+		return -1;
+	}
+
+	channel->send_step = STEP_WAIT_FOR_ACK;
+
+	if (down_timeout(&channel->ack_done_sem, HSI_ACK_DONE_TIMEOUT) < 0) {
+		mif_err("[MIPI-HSI] ch=%d, ack_done timeout\n",
+					channel->channel_id);
+
+		if_hsi_set_wakeline(channel, 0);
+
+		if (mipi_ld->ld.com_state == COM_ONLINE) {
+			ack_timeout_cnt++;
+			if (ack_timeout_cnt < 10) {
+				if_hsi_set_wakeline(channel, 1);
+				mif_err("[MIPI-HSI] ch=%d, retry send open. cnt : %d\n",
+					channel->channel_id, ack_timeout_cnt);
+				goto retry_send;
+			}
+
+			/* try to recover cp */
+			iod = link_get_iod_with_format(&mipi_ld->ld, IPC_FMT);
+			if (iod)
+				iod->modem_state_changed(iod,
+						STATE_CRASH_RESET);
+		}
+
+		channel->send_step = STEP_IDLE;
+		return -ETIMEDOUT;
+	}
+	mif_debug("[MIPI-HSI] ch=%d, got ack_done=%d\n", channel->channel_id,
+				channel->got_nack);
+
+	if (channel->got_nack && (retry_count < 10)) {
+		mif_debug("[MIPI-HSI] ch=%d, got nack=%d retry=%d\n",
+				channel->channel_id, channel->got_nack,
+				retry_count);
+		retry_count++;
+		msleep_interruptible(1);
+		goto retry_send;
+	}
+	retry_count = 0;
+
+	channel->send_step = STEP_TX;
+
+	ret = if_hsi_write(channel, data, len);
+	if (ret < 0) {
+		mif_err("[MIPI-HSI] if_hsi_write fail : %d\n", ret);
+		if_hsi_set_wakeline(channel, 0);
+		channel->send_step = STEP_IDLE;
+		return ret;
+	}
+	mif_debug("[MIPI-HSI] SEND DATA : %08x(%d)\n", *data, len);
+
+	mif_debug("%08x %08x %08x %08x %08x %08x %08x %08x\n",
+		*channel->tx_data, *(channel->tx_data + 1),
+		*(channel->tx_data + 2), *(channel->tx_data + 3),
+		*(channel->tx_data + 4), *(channel->tx_data + 5),
+		*(channel->tx_data + 6), *(channel->tx_data + 7));
+
+	channel->send_step = STEP_WAIT_FOR_CONN_CLOSED;
+	if (down_timeout(&channel->close_conn_done_sem,
+				HSI_CLOSE_CONN_DONE_TIMEOUT) < 0) {
+		mif_err("[MIPI-HSI] ch=%d, close conn timeout\n",
+					channel->channel_id);
+		if_hsi_set_wakeline(channel, 0);
+		channel->send_step = STEP_IDLE;
+		return -ETIMEDOUT;
+	}
+	mif_debug("[MIPI-HSI] ch=%d, got close_conn_done\n",
+				channel->channel_id);
+
+	channel->send_step = STEP_IDLE;
+
+	mif_debug("[MIPI-HSI] write protocol Done : %d\n", channel->tx_count);
+	return channel->tx_count;
+}
+
+static int if_hsi_write(struct if_hsi_channel *channel, u32 *data,
+			unsigned int size)
+{
+	int ret;
+	unsigned long int flags;
+
+	spin_lock_irqsave(&channel->tx_state_lock, flags);
+	if (channel->tx_state & HSI_CHANNEL_TX_STATE_WRITING) {
+		spin_unlock_irqrestore(&channel->tx_state_lock, flags);
+		return -EBUSY;
+	}
+	channel->tx_state |= HSI_CHANNEL_TX_STATE_WRITING;
+	spin_unlock_irqrestore(&channel->tx_state_lock, flags);
+
+	channel->tx_data = data;
+	if (size % 4)
+		size += (4 - (size % 4));
+	channel->tx_count = size;
+
+	mif_debug("[MIPI-HSI] submit write data : 0x%x(%d)\n",
+				*(u32 *)channel->tx_data, channel->tx_count);
+	ret = hsi_write(channel->dev, channel->tx_data, channel->tx_count / 4);
+	if (ret) {
+		mif_err("[MIPI-HSI] ch=%d, hsi_write fail : %d\n",
+					channel->channel_id, ret);
+
+		spin_lock_irqsave(&channel->tx_state_lock, flags);
+		channel->tx_state &= ~HSI_CHANNEL_TX_STATE_WRITING;
+		spin_unlock_irqrestore(&channel->tx_state_lock, flags);
+
+		return ret;
+	}
+
+	if (down_timeout(&channel->write_done_sem,
+				HSI_WRITE_DONE_TIMEOUT) < 0) {
+		mif_err("[MIPI-HSI] ch=%d, hsi_write_done timeout : %d\n",
+					channel->channel_id, size);
+
+		mif_err("[MIPI-HSI] data : %08x %08x %08x %08x %08x ...\n",
+			*channel->tx_data, *(channel->tx_data + 1),
+			*(channel->tx_data + 2), *(channel->tx_data + 3),
+			*(channel->tx_data + 4));
+
+		hsi_write_cancel(channel->dev);
+
+		spin_lock_irqsave(&channel->tx_state_lock, flags);
+		channel->tx_state &= ~HSI_CHANNEL_TX_STATE_WRITING;
+		spin_unlock_irqrestore(&channel->tx_state_lock, flags);
+
+		return -ETIMEDOUT;
+	}
+
+	if (channel->tx_count != size)
+		mif_err("[MIPI-HSI] ch:%d,write_done fail,write_size:%d,origin_size:%d\n",
+				channel->channel_id, channel->tx_count, size);
+
+	mif_debug("[MIPI-HSI] len:%d, id:%d, data : %08x %08x %08x %08x %08x ...\n",
+		channel->tx_count, channel->channel_id, *channel->tx_data,
+		*(channel->tx_data + 1), *(channel->tx_data + 2),
+		*(channel->tx_data + 3), *(channel->tx_data + 4));
+
+	return channel->tx_count;
+}
+
+static void if_hsi_write_done(struct hsi_device *dev, unsigned int size)
+{
+	unsigned long int flags;
+	struct mipi_link_device *mipi_ld =
+			(struct mipi_link_device *)if_hsi_driver.priv_data;
+	struct if_hsi_channel *channel = &mipi_ld->hsi_channles[dev->n_ch];
+
+	mif_debug("[MIPI-HSI] got write data : 0x%x(%d)\n",
+				*(u32 *)channel->tx_data, size);
+
+	spin_lock_irqsave(&channel->tx_state_lock, flags);
+	channel->tx_state &= ~HSI_CHANNEL_TX_STATE_WRITING;
+	spin_unlock_irqrestore(&channel->tx_state_lock, flags);
+
+	mif_debug("%08x %08x %08x %08x %08x %08x %08x %08x\n",
+			*channel->tx_data, *(channel->tx_data + 1),
+			*(channel->tx_data + 2), *(channel->tx_data + 3),
+			*(channel->tx_data + 4), *(channel->tx_data + 5),
+			*(channel->tx_data + 6), *(channel->tx_data + 7));
+
+	channel->tx_count = 4 * size;
+	up(&channel->write_done_sem);
+}
+
+static void if_hsi_read_done(struct hsi_device *dev, unsigned int size)
+{
+	int ret;
+	unsigned long int flags;
+	u32 cmd = 0, ch = 0, param = 0;
+	struct mipi_link_device *mipi_ld =
+			(struct mipi_link_device *)if_hsi_driver.priv_data;
+	struct if_hsi_channel *channel = &mipi_ld->hsi_channles[dev->n_ch];
+	struct io_device *iod;
+	enum dev_format format_type = 0;
+
+	mif_debug("[MIPI-HSI] got read data : 0x%x(%d)\n",
+				*(u32 *)channel->rx_data, size);
+
+	spin_lock_irqsave(&channel->rx_state_lock, flags);
+	channel->rx_state &= ~HSI_CHANNEL_RX_STATE_READING;
+	spin_unlock_irqrestore(&channel->rx_state_lock, flags);
+
+	channel->rx_count = 4 * size;
+
+	switch (channel->channel_id) {
+	case HSI_CONTROL_CHANNEL:
+		switch (mipi_ld->ld.com_state) {
+		case COM_HANDSHAKE:
+		case COM_ONLINE:
+			mif_debug("[MIPI-HSI] RECV CMD : %08x\n",
+						*channel->rx_data);
+
+			if (channel->rx_count != 4) {
+				mif_err("[MIPI-HSI] wrong command len : %d\n",
+					channel->rx_count);
+				return;
+			}
+
+			ret = if_hsi_decode_cmd(channel->rx_data, &cmd, &ch,
+						&param);
+			if (ret)
+				mif_err("[MIPI-HSI] decode_cmd fail=%d, "
+						"cmd=%x\n", ret, cmd);
+			else {
+				mif_debug("[MIPI-HSI] decode_cmd : %08x\n",
+						cmd);
+				ret = if_hsi_rx_cmd_handle(mipi_ld, cmd, ch,
+							param);
+				if (ret)
+					mif_err("[MIPI-HSI] handle cmd "
+							"cmd=%x\n", cmd);
+			}
+
+			ret = hsi_read(channel->dev, channel->rx_data, 1);
+			if (ret)
+				mif_err("[MIPI-HSI] hsi_read fail : %d\n", ret);
+
+			return;
+
+		case COM_BOOT:
+			mif_debug("[MIPI-HSI] receive data : 0x%x(%d)\n",
+					*channel->rx_data, channel->rx_count);
+
+			iod = link_get_iod_with_format(&mipi_ld->ld, IPC_BOOT);
+			if (iod) {
+				channel->packet_size = *channel->rx_data;
+				mif_debug("[MIPI-HSI] flashless packet size : "
+						"%d\n", channel->packet_size);
+
+				ret = iod->recv(iod,
+						&mipi_ld->ld,
+						(char *)channel->rx_data + 4,
+						HSI_FLASHBOOT_ACK_LEN - 4);
+				if (ret < 0)
+					mif_err("[MIPI-HSI] recv call "
+							"fail : %d\n", ret);
+			}
+
+			ret = hsi_read(channel->dev, channel->rx_data,
+						HSI_FLASHBOOT_ACK_LEN / 4);
+			if (ret)
+				mif_err("[MIPI-HSI] hsi_read fail : %d\n", ret);
+			return;
+
+		case COM_CRASH:
+			mif_debug("[MIPI-HSI] receive data : 0x%x(%d)\n",
+					*channel->rx_data, channel->rx_count);
+
+			iod = link_get_iod_with_format(&mipi_ld->ld,
+								IPC_RAMDUMP);
+			if (iod) {
+				channel->packet_size = *channel->rx_data;
+				mif_debug("[MIPI-HSI] ramdump packet size : "
+						"%d\n", channel->packet_size);
+
+				ret = iod->recv(iod,
+						&mipi_ld->ld,
+						(char *)channel->rx_data + 4,
+						channel->packet_size);
+				if (ret < 0)
+					mif_err("[MIPI-HSI] recv call "
+							"fail : %d\n", ret);
+			}
+
+			ret = hsi_read(channel->dev, channel->rx_data,
+						DUMP_PACKET_SIZE);
+			if (ret)
+				mif_err("[MIPI-HSI] hsi_read fail : %d\n", ret);
+			return;
+
+		case COM_NONE:
+		default:
+			mif_err("[MIPI-HSI] receive data in wrong state : 0x%x(%d)\n",
+					*channel->rx_data, channel->rx_count);
+			return;
+		}
+		break;
+
+	case HSI_FMT_CHANNEL:
+		mif_debug("[MIPI-HSI] iodevice format : IPC_FMT\n");
+		format_type = IPC_FMT;
+		break;
+	case HSI_RAW_CHANNEL:
+		mif_debug("[MIPI-HSI] iodevice format : IPC_MULTI_RAW\n");
+		format_type = IPC_MULTI_RAW;
+		break;
+	case HSI_RFS_CHANNEL:
+		mif_debug("[MIPI-HSI] iodevice format : IPC_RFS\n");
+		format_type = IPC_RFS;
+		break;
+
+	case HSI_CMD_CHANNEL:
+		mif_debug("[MIPI-HSI] receive command data : 0x%x\n",
+					*channel->rx_data);
+
+		ch = channel->channel_id;
+		param = 0;
+		ret = if_hsi_send_command(mipi_ld, HSI_LL_MSG_CONN_CLOSED,
+					ch, param);
+		if (ret)
+			mif_err("[MIPI-HSI] send_cmd fail=%d\n", ret);
+
+		channel->recv_step = STEP_IDLE;
+		return;
+
+	default:
+		return;
+	}
+
+	iod = link_get_iod_with_format(&mipi_ld->ld, format_type);
+	if (iod) {
+		mif_debug("[MIPI-HSI] iodevice format : %d\n", iod->format);
+
+		channel->recv_step = STEP_NOT_READY;
+
+		mif_debug("[MIPI-HSI] RECV DATA : %08x(%d)-%d\n",
+				*channel->rx_data, channel->packet_size,
+				iod->format);
+
+		mif_debug("%08x %08x %08x %08x %08x %08x %08x %08x\n",
+			*channel->rx_data, *(channel->rx_data + 1),
+			*(channel->rx_data + 2), *(channel->rx_data + 3),
+			*(channel->rx_data + 4), *(channel->rx_data + 5),
+			*(channel->rx_data + 6), *(channel->rx_data + 7));
+
+		ret = iod->recv(iod, &mipi_ld->ld,
+				(char *)channel->rx_data, channel->packet_size);
+		if (ret < 0)
+			mif_err("[MIPI-HSI] recv call fail : %d\n", ret);
+
+		ch = channel->channel_id;
+		param = 0;
+		ret = if_hsi_send_command(mipi_ld,
+				HSI_LL_MSG_CONN_CLOSED, ch, param);
+		if (ret)
+			mif_err("[MIPI-HSI] send_cmd fail=%d\n", ret);
+
+		channel->recv_step = STEP_IDLE;
+	}
+}
+
+static void if_hsi_port_event(struct hsi_device *dev, unsigned int event,
+			void *arg)
+{
+	int acwake_level = 1;
+	struct mipi_link_device *mipi_ld =
+			(struct mipi_link_device *)if_hsi_driver.priv_data;
+
+	switch (event) {
+	case HSI_EVENT_BREAK_DETECTED:
+		mif_err("[MIPI-HSI] HSI_EVENT_BREAK_DETECTED\n");
+		return;
+
+	case HSI_EVENT_HSR_DATAAVAILABLE:
+		mif_err("[MIPI-HSI] HSI_EVENT_HSR_DATAAVAILABLE\n");
+		return;
+
+	case HSI_EVENT_CAWAKE_UP:
+		if (dev->n_ch == HSI_CONTROL_CHANNEL) {
+			if (!wake_lock_active(&mipi_ld->wlock)) {
+				wake_lock(&mipi_ld->wlock);
+				mif_debug("[MIPI-HSI] wake_lock\n");
+			}
+			mif_debug("[MIPI-HSI] CAWAKE_%d(1)\n", dev->n_ch);
+		}
+		return;
+
+	case HSI_EVENT_CAWAKE_DOWN:
+		if (dev->n_ch == HSI_CONTROL_CHANNEL)
+			mif_debug("[MIPI-HSI] CAWAKE_%d(0)\n", dev->n_ch);
+
+		if ((dev->n_ch == HSI_CONTROL_CHANNEL) &&
+			mipi_ld->hsi_channles[HSI_CONTROL_CHANNEL].opened) {
+			hsi_ioctl(
+			mipi_ld->hsi_channles[HSI_CONTROL_CHANNEL].dev,
+			HSI_IOCTL_GET_ACWAKE, &acwake_level);
+
+			mif_debug("[MIPI-HSI] GET_ACWAKE. Ch : %d, level : %d\n",
+					dev->n_ch, acwake_level);
+
+			if (!acwake_level) {
+				wake_unlock(&mipi_ld->wlock);
+				mif_debug("[MIPI-HSI] wake_unlock\n");
+			}
+		}
+		return;
+
+	case HSI_EVENT_ERROR:
+		mif_err("[MIPI-HSI] HSI_EVENT_ERROR\n");
+		return;
+
+	default:
+		mif_err("[MIPI-HSI] Unknown Event : %d\n", event);
+		return;
+	}
+}
+
+static int __devinit if_hsi_probe(struct hsi_device *dev)
+{
+	int port = 0;
+	unsigned long *address;
+	struct mipi_link_device *mipi_ld =
+			(struct mipi_link_device *)if_hsi_driver.priv_data;
+
+	for (port = 0; port < HSI_MAX_PORTS; port++) {
+		if (if_hsi_driver.ch_mask[port])
+			break;
+	}
+	address = (unsigned long *)&if_hsi_driver.ch_mask[port];
+
+	if (test_bit(dev->n_ch, address) && (dev->n_p == port)) {
+		/* Register callback func */
+		hsi_set_write_cb(dev, if_hsi_write_done);
+		hsi_set_read_cb(dev, if_hsi_read_done);
+		hsi_set_port_event_cb(dev, if_hsi_port_event);
+
+		/* Init device data */
+		mipi_ld->hsi_channles[dev->n_ch].dev = dev;
+		mipi_ld->hsi_channles[dev->n_ch].tx_count = 0;
+		mipi_ld->hsi_channles[dev->n_ch].rx_count = 0;
+		mipi_ld->hsi_channles[dev->n_ch].tx_state = 0;
+		mipi_ld->hsi_channles[dev->n_ch].rx_state = 0;
+		mipi_ld->hsi_channles[dev->n_ch].packet_size = 0;
+		mipi_ld->hsi_channles[dev->n_ch].acwake = 0;
+		mipi_ld->hsi_channles[dev->n_ch].send_step = STEP_UNDEF;
+		mipi_ld->hsi_channles[dev->n_ch].recv_step = STEP_UNDEF;
+		spin_lock_init(&mipi_ld->hsi_channles[dev->n_ch].tx_state_lock);
+		spin_lock_init(&mipi_ld->hsi_channles[dev->n_ch].rx_state_lock);
+		spin_lock_init(&mipi_ld->hsi_channles[dev->n_ch].acwake_lock);
+		sema_init(&mipi_ld->hsi_channles[dev->n_ch].write_done_sem,
+			  0);
+		sema_init(&mipi_ld->hsi_channles[dev->n_ch].ack_done_sem,
+			  0);
+		sema_init(&mipi_ld->hsi_channles[dev->n_ch].close_conn_done_sem,
+			  0);
+	}
+
+	mif_debug("[MIPI-HSI] if_hsi_probe() done. ch : %d\n", dev->n_ch);
+	return 0;
+}
+
+static int if_hsi_init(struct link_device *ld)
+{
+	int ret;
+	int i = 0;
+	struct mipi_link_device *mipi_ld = to_mipi_link_device(ld);
+
+	for (i = 0; i < HSI_MAX_PORTS; i++)
+		if_hsi_driver.ch_mask[i] = 0;
+
+	for (i = 0; i < HSI_MAX_CHANNELS; i++) {
+		mipi_ld->hsi_channles[i].dev = NULL;
+		mipi_ld->hsi_channles[i].opened = 0;
+		mipi_ld->hsi_channles[i].channel_id = i;
+	}
+	if_hsi_driver.ch_mask[0] = CHANNEL_MASK;
+
+	/* TODO - need to get priv data (request to TI) */
+	if_hsi_driver.priv_data = (void *)mipi_ld;
+	ret = hsi_register_driver(&if_hsi_driver);
+	if (ret) {
+		mif_err("[MIPI-HSI] hsi_register_driver() fail : %d\n", ret);
+		return ret;
+	}
+
+	mipi_ld->mipi_wq = create_singlethread_workqueue("mipi_cmd_wq");
+	if (!mipi_ld->mipi_wq) {
+		mif_err("[MIPI-HSI] fail to create work Q.\n");
+		return -ENOMEM;
+	}
+	INIT_WORK(&mipi_ld->cmd_work, if_hsi_cmd_work);
+	INIT_DELAYED_WORK(&mipi_ld->start_work, mipi_hsi_start_work);
+
+	setup_timer(&mipi_ld->hsi_acwake_down_timer, if_hsi_acwake_down_func,
+				(unsigned long)mipi_ld);
+
+	/* TODO - allocate rx buff */
+	mipi_ld->hsi_channles[HSI_CONTROL_CHANNEL].rx_data =
+				kmalloc(64 * 1024, GFP_DMA | GFP_ATOMIC);
+	if (!mipi_ld->hsi_channles[HSI_CONTROL_CHANNEL].rx_data) {
+		mif_err("[MIPI-HSI] alloc HSI_CONTROL_CHANNEL rx_data fail\n");
+		return -ENOMEM;
+	}
+	mipi_ld->hsi_channles[HSI_FMT_CHANNEL].rx_data =
+				kmalloc(256 * 1024, GFP_DMA | GFP_ATOMIC);
+	if (!mipi_ld->hsi_channles[HSI_FMT_CHANNEL].rx_data) {
+		mif_err("[MIPI-HSI] alloc HSI_FMT_CHANNEL rx_data fail\n");
+		return -ENOMEM;
+	}
+	mipi_ld->hsi_channles[HSI_RAW_CHANNEL].rx_data =
+				kmalloc(256 * 1024, GFP_DMA | GFP_ATOMIC);
+	if (!mipi_ld->hsi_channles[HSI_RAW_CHANNEL].rx_data) {
+		mif_err("[MIPI-HSI] alloc HSI_RAW_CHANNEL rx_data fail\n");
+		return -ENOMEM;
+	}
+	mipi_ld->hsi_channles[HSI_RFS_CHANNEL].rx_data =
+				kmalloc(256 * 1024, GFP_DMA | GFP_ATOMIC);
+	if (!mipi_ld->hsi_channles[HSI_RFS_CHANNEL].rx_data) {
+		mif_err("[MIPI-HSI] alloc HSI_RFS_CHANNEL rx_data fail\n");
+		return -ENOMEM;
+	}
+	mipi_ld->hsi_channles[HSI_CMD_CHANNEL].rx_data =
+				kmalloc(256 * 1024, GFP_DMA | GFP_ATOMIC);
+	if (!mipi_ld->hsi_channles[HSI_CMD_CHANNEL].rx_data) {
+		mif_err("[MIPI-HSI] alloc HSI_CMD_CHANNEL rx_data fail\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+struct link_device *mipi_create_link_device(struct platform_device *pdev)
+{
+	int ret;
+	struct mipi_link_device *mipi_ld;
+	struct link_device *ld;
+
+	/* for dpram int */
+	/* struct modem_data *pdata = pdev->dev.platform_data; */
+
+	mipi_ld = kzalloc(sizeof(struct mipi_link_device), GFP_KERNEL);
+	if (!mipi_ld)
+		return NULL;
+
+	INIT_LIST_HEAD(&mipi_ld->list_of_hsi_cmd);
+	spin_lock_init(&mipi_ld->list_cmd_lock);
+	skb_queue_head_init(&mipi_ld->ld.sk_fmt_tx_q);
+	skb_queue_head_init(&mipi_ld->ld.sk_raw_tx_q);
+
+	wake_lock_init(&mipi_ld->wlock, WAKE_LOCK_SUSPEND, "mipi_link");
+
+	ld = &mipi_ld->ld;
+
+	ld->name = "mipi_hsi";
+	ld->init_comm = mipi_hsi_init_communication;
+	ld->terminate_comm = mipi_hsi_terminate_communication;
+	ld->send = mipi_hsi_send;
+	ld->com_state = COM_NONE;
+
+	/* for dpram int */
+	/* ld->irq = gpio_to_irq(pdata->gpio); s*/
+
+	ld->tx_wq = create_singlethread_workqueue("mipi_tx_wq");
+	if (!ld->tx_wq) {
+		mif_err("[MIPI-HSI] fail to create work Q.\n");
+		return NULL;
+	}
+	INIT_WORK(&ld->tx_work, mipi_hsi_tx_work);
+
+	ret = if_hsi_init(ld);
+	if (ret)
+		return NULL;
+
+	return ld;
+}
diff --git a/drivers/misc/modem_if/modem_link_device_mipi.h b/drivers/misc/modem_if/modem_link_device_mipi.h
new file mode 100644
index 0000000..8ca4968
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_mipi.h
@@ -0,0 +1,158 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#ifndef __MODEM_LINK_DEVICE_MIPI_H__
+#define __MODEM_LINK_DEVICE_MIPI_H__
+
+
+#define HSI_MAX_CHANNELS	16
+#define CHANNEL_MASK	0xFF
+
+#define HSI_CHANNEL_TX_STATE_UNAVAIL	(1 << 0)
+#define HSI_CHANNEL_TX_STATE_WRITING	(1 << 1)
+#define HSI_CHANNEL_RX_STATE_UNAVAIL	(1 << 0)
+#define HSI_CHANNEL_RX_STATE_READING	(1 << 1)
+
+#define HSI_WRITE_DONE_TIMEOUT	(HZ)
+#define HSI_READ_DONE_TIMEOUT	(HZ)
+#define HSI_ACK_DONE_TIMEOUT	(HZ)
+#define HSI_CLOSE_CONN_DONE_TIMEOUT	(HZ)
+#define HSI_ACWAKE_DOWN_TIMEOUT	(HZ / 2)
+
+#define HSI_CONTROL_CHANNEL	0
+#define HSI_FLASHLESS_CHANNEL	0
+#define HSI_CP_RAMDUMP_CHANNEL	0
+#define HSI_FMT_CHANNEL	1
+#define HSI_RAW_CHANNEL	2
+#define HSI_RFS_CHANNEL	3
+#define HSI_CMD_CHANNEL	4
+#define HSI_NUM_OF_USE_CHANNELS	5
+
+#define HSI_LL_INVALID_CHANNEL	0xFF
+
+#define HSI_FLASHBOOT_ACK_LEN	16
+#define DUMP_PACKET_SIZE	12289 /* 48K + 4 length, word unit */
+#define DUMP_ERR_INFO_SIZE	39 /* 150 bytes + 4 length , word unit */
+
+enum {
+	HSI_LL_MSG_BREAK, /* 0x0 */
+	HSI_LL_MSG_ECHO,
+	HSI_LL_MSG_INFO_REQ,
+	HSI_LL_MSG_INFO,
+	HSI_LL_MSG_CONFIGURE,
+	HSI_LL_MSG_ALLOCATE_CH,
+	HSI_LL_MSG_RELEASE_CH,
+	HSI_LL_MSG_OPEN_CONN,
+	HSI_LL_MSG_CONN_READY,
+	HSI_LL_MSG_CONN_CLOSED, /* 0x9 */
+	HSI_LL_MSG_CANCEL_CONN,
+	HSI_LL_MSG_ACK, /* 0xB */
+	HSI_LL_MSG_NAK, /* 0xC */
+	HSI_LL_MSG_CONF_RATE,
+	HSI_LL_MSG_OPEN_CONN_OCTET, /* 0xE */
+	HSI_LL_MSG_INVALID = 0xFF,
+};
+
+enum {
+	STEP_UNDEF,
+	STEP_CLOSED,
+	STEP_NOT_READY,
+	STEP_IDLE,
+	STEP_ERROR,
+	STEP_SEND_OPEN_CONN,
+	STEP_SEND_ACK,
+	STEP_WAIT_FOR_ACK,
+	STEP_TO_ACK,
+	STEP_SEND_NACK,
+	STEP_GET_NACK,
+	STEP_SEND_CONN_READY,
+	STEP_WAIT_FOR_CONN_READY,
+	STEP_SEND_CONF_RATE,
+	STEP_WAIT_FOR_CONF_ACK,
+	STEP_TX,
+	STEP_RX,
+	STEP_SEND_CONN_CLOSED,
+	STEP_WAIT_FOR_CONN_CLOSED,
+	STEP_SEND_BREAK,
+};
+
+
+struct if_hsi_channel {
+	struct hsi_device *dev;
+	unsigned int channel_id;
+
+	u32 *tx_data;
+	unsigned int tx_count;
+	u32 *rx_data;
+	unsigned int rx_count;
+	unsigned int packet_size;
+
+	unsigned int tx_state;
+	unsigned int rx_state;
+	spinlock_t tx_state_lock;
+	spinlock_t rx_state_lock;
+
+	unsigned int send_step;
+	unsigned int recv_step;
+
+	unsigned int got_nack;
+	unsigned int acwake;
+	spinlock_t acwake_lock;
+
+	struct semaphore write_done_sem;
+	struct semaphore ack_done_sem;
+	struct semaphore close_conn_done_sem;
+
+	unsigned int opened;
+};
+
+struct if_hsi_command {
+	u32 command;
+	struct list_head list;
+};
+
+struct mipi_link_device {
+	struct link_device ld;
+
+	/* mipi specific link data */
+	struct if_hsi_channel hsi_channles[HSI_MAX_CHANNELS];
+	struct list_head list_of_hsi_cmd;
+	spinlock_t list_cmd_lock;
+
+	struct workqueue_struct *mipi_wq;
+	struct work_struct cmd_work;
+	struct delayed_work start_work;
+
+	struct wake_lock wlock;
+	struct timer_list hsi_acwake_down_timer;
+};
+/* converts from struct link_device* to struct xxx_link_device* */
+#define to_mipi_link_device(linkdev) \
+			container_of(linkdev, struct mipi_link_device, ld)
+
+
+enum {
+	HSI_INIT_MODE_NORMAL,
+	HSI_INIT_MODE_FLASHLESS_BOOT,
+	HSI_INIT_MODE_CP_RAMDUMP,
+};
+static int hsi_init_handshake(struct mipi_link_device *mipi_ld, int mode);
+static int if_hsi_write(struct if_hsi_channel *channel, u32 *data,
+			unsigned int size);
+static int if_hsi_protocol_send(struct mipi_link_device *mipi_ld, int ch,
+			u32 *data, unsigned int len);
+static int if_hsi_close_channel(struct if_hsi_channel *channel);
+
+#endif
diff --git a/drivers/misc/modem_if/modem_link_device_usb.c b/drivers/misc/modem_if/modem_link_device_usb.c
new file mode 100644
index 0000000..615971e
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_usb.c
@@ -0,0 +1,980 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#define DEBUG
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+#include <linux/irq.h>
+#include <linux/poll.h>
+#include <linux/gpio.h>
+#include <linux/if_arp.h>
+#include <linux/platform_device.h>
+#include <linux/suspend.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_link_device_usb.h"
+#include "modem_utils.h"
+#include "modem_link_pm_usb.h"
+
+#define URB_COUNT	4
+
+static int usb_tx_urb_with_skb(struct usb_link_device *usb_ld,
+		struct sk_buff *skb, struct if_usb_devdata *pipe_data);
+
+static void
+usb_free_urbs(struct usb_link_device *usb_ld, struct if_usb_devdata *pipe)
+{
+	struct usb_device *usbdev = usb_ld->usbdev;
+	struct urb *urb;
+
+	while ((urb = usb_get_from_anchor(&pipe->urbs))) {
+		usb_poison_urb(urb);
+		usb_free_coherent(usbdev, pipe->rx_buf_size,
+				urb->transfer_buffer, urb->transfer_dma);
+		urb->transfer_buffer = NULL;
+		usb_put_urb(urb);
+		usb_free_urb(urb);
+	}
+}
+
+static int start_ipc(struct link_device *ld, struct io_device *iod)
+{
+	struct sk_buff *skb;
+	char data[1] = {'a'};
+	int err;
+	struct usb_link_device *usb_ld = to_usb_link_device(ld);
+	struct if_usb_devdata *pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
+
+	if (usb_ld->link_pm_data->hub_handshake_done) {
+		mif_err("Aleady send start ipc, skip start ipc\n");
+		err = 0;
+		goto exit;
+	}
+
+	if (!usb_ld->if_usb_connected) {
+		mif_err("HSIC/USB not connected, skip start ipc\n");
+		err = -ENODEV;
+		goto exit;
+	}
+
+	if (usb_ld->if_usb_initstates == INIT_IPC_START_DONE) {
+		mif_debug("aleady IPC started\n");
+		err = 0;
+		goto exit;
+	}
+
+	mif_info("send 'a'\n");
+
+	skb = alloc_skb(16, GFP_ATOMIC);
+	if (unlikely(!skb))
+		return -ENOMEM;
+	memcpy(skb_put(skb, 1), data, 1);
+
+	skbpriv(skb)->iod = iod;
+	skbpriv(skb)->ld = &usb_ld->ld;
+	err = usb_tx_urb_with_skb(usb_ld, skb, pipe_data);
+	if (err < 0) {
+		mif_err("usb_tx_urb fail\n");
+		goto exit;
+	}
+	usb_ld->link_pm_data->hub_handshake_done = true;
+	usb_ld->if_usb_initstates = INIT_IPC_START_DONE;
+exit:
+	return err;
+}
+
+static int usb_init_communication(struct link_device *ld,
+			struct io_device *iod)
+{
+	int err = 0;
+	switch (iod->format) {
+	case IPC_BOOT:
+		ld->com_state = COM_BOOT;
+		skb_queue_purge(&ld->sk_fmt_tx_q);
+		break;
+
+	case IPC_RAMDUMP:
+		ld->com_state = COM_CRASH;
+		break;
+
+	case IPC_FMT:
+		err = start_ipc(ld, iod);
+		break;
+
+	case IPC_RFS:
+	case IPC_RAW:
+
+	default:
+		ld->com_state = COM_ONLINE;
+		break;
+	}
+
+	mif_debug("com_state = %d\n", ld->com_state);
+	return err;
+}
+
+static void usb_terminate_communication(
+			struct link_device *ld, struct io_device *iod)
+{
+	mif_debug("com_state = %d\n", ld->com_state);
+}
+
+static int usb_rx_submit(struct if_usb_devdata *pipe, struct urb *urb,
+	gfp_t gfp_flags)
+{
+	int ret;
+
+	usb_anchor_urb(urb, &pipe->reading);
+	ret = usb_submit_urb(urb, gfp_flags);
+	if (ret) {
+		usb_unanchor_urb(urb);
+		usb_anchor_urb(urb, &pipe->urbs);
+		mif_err("submit urb fail with ret (%d)\n", ret);
+	}
+
+	usb_mark_last_busy(urb->dev);
+	return ret;
+}
+
+static void usb_rx_complete(struct urb *urb)
+{
+	struct if_usb_devdata *pipe_data = urb->context;
+	struct usb_link_device *usb_ld = usb_get_intfdata(pipe_data->data_intf);
+	struct io_device *iod;
+	int iod_format = IPC_FMT;
+	int ret;
+
+	usb_mark_last_busy(urb->dev);
+
+	switch (urb->status) {
+	case 0:
+	case -ENOENT:
+		if (!urb->actual_length)
+			goto re_submit;
+		/* call iod recv */
+		/* how we can distinguish boot ch with fmt ch ?? */
+		switch (pipe_data->format) {
+		case IF_USB_FMT_EP:
+			iod_format = IPC_FMT;
+			pr_buffer("rx", (char *)urb->transfer_buffer,
+					(size_t)urb->actual_length, 16);
+			break;
+		case IF_USB_RAW_EP:
+			iod_format = IPC_MULTI_RAW;
+			break;
+		case IF_USB_RFS_EP:
+			iod_format = IPC_RFS;
+			break;
+		default:
+			break;
+		}
+
+		/* during boot stage fmt end point */
+		/* shared with boot io device */
+		/* when we use fmt device only, at boot and ipc exchange
+			it can be reduced to 1 device */
+		if (iod_format == IPC_FMT &&
+			usb_ld->ld.com_state == COM_BOOT)
+			iod_format = IPC_BOOT;
+		if (iod_format == IPC_FMT &&
+			usb_ld->ld.com_state == COM_CRASH)
+			iod_format = IPC_RAMDUMP;
+
+		iod = link_get_iod_with_format(&usb_ld->ld, iod_format);
+		if (iod) {
+			ret = iod->recv(iod,
+					&usb_ld->ld,
+					(char *)urb->transfer_buffer,
+					urb->actual_length);
+			if (ret < 0)
+				mif_err("io device recv error :%d\n", ret);
+		}
+re_submit:
+		if (urb->status || atomic_read(&usb_ld->suspend_count))
+			break;
+
+		usb_mark_last_busy(urb->dev);
+		usb_rx_submit(pipe_data, urb, GFP_ATOMIC);
+		return;
+	case -ESHUTDOWN:
+	case -EPROTO:
+		break;
+	case -EOVERFLOW:
+		mif_err("RX overflow\n");
+		break;
+	default:
+		mif_err("RX complete Status (%d)\n", urb->status);
+		break;
+	}
+
+	usb_anchor_urb(urb, &pipe_data->urbs);
+}
+
+static int usb_send(struct link_device *ld, struct io_device *iod,
+			struct sk_buff *skb)
+{
+	struct sk_buff_head *txq;
+	size_t tx_size;
+
+	if (iod->format == IPC_RAW)
+		txq = &ld->sk_raw_tx_q;
+	else
+		txq = &ld->sk_fmt_tx_q;
+
+	/* store the tx size before run the tx_delayed_work*/
+	tx_size = skb->len;
+
+	/* en queue skb data */
+	skb_queue_tail(txq, skb);
+
+	queue_delayed_work(ld->tx_wq, &ld->tx_delayed_work, 0);
+
+	return tx_size;
+}
+
+static void usb_tx_complete(struct urb *urb)
+{
+	int ret = 0;
+	struct sk_buff *skb = urb->context;
+
+	switch (urb->status) {
+	case 0:
+		break;
+	default:
+		mif_err("TX error (%d)\n", urb->status);
+	}
+
+	usb_mark_last_busy(urb->dev);
+	ret = pm_runtime_put_autosuspend(&urb->dev->dev);
+	if (ret < 0)
+		mif_debug("pm_runtime_put_autosuspend failed : ret(%d)\n", ret);
+	usb_free_urb(urb);
+	dev_kfree_skb_any(skb);
+}
+
+static void if_usb_force_disconnect(struct work_struct *work)
+{
+	struct usb_link_device *usb_ld =
+		container_of(work, struct usb_link_device, disconnect_work);
+	struct usb_device *udev = usb_ld->usbdev;
+
+	pm_runtime_get_sync(&udev->dev);
+	if (udev->state != USB_STATE_NOTATTACHED) {
+		mif_info("force disconnect by modem not responding!!\n");
+	}
+	pm_runtime_put_autosuspend(&udev->dev);
+}
+
+static void
+usb_change_modem_state(struct usb_link_device *usb_ld, enum modem_state state)
+{
+	struct io_device *iod;
+
+	iod = link_get_iod_with_format(&usb_ld->ld, IPC_FMT);
+	if (iod)
+		iod->modem_state_changed(iod, state);
+}
+
+static int usb_tx_urb_with_skb(struct usb_link_device *usb_ld,
+		struct sk_buff *skb, struct if_usb_devdata *pipe_data)
+{
+	int ret, cnt = 0;
+	struct urb *urb;
+	struct usb_device *usbdev = usb_ld->usbdev;
+	unsigned long flags;
+
+	if (!usbdev || (usbdev->state == USB_STATE_NOTATTACHED) ||
+			usb_ld->host_wake_timeout_flag)
+		return -ENODEV;
+
+	pm_runtime_get_noresume(&usbdev->dev);
+
+	if (usbdev->dev.power.runtime_status == RPM_SUSPENDED ||
+		usbdev->dev.power.runtime_status == RPM_SUSPENDING) {
+		usb_ld->resume_status = AP_INITIATED_RESUME;
+		SET_SLAVE_WAKEUP(usb_ld->pdata, 1);
+
+		while (!wait_event_interruptible_timeout(usb_ld->l2_wait,
+				usbdev->dev.power.runtime_status == RPM_ACTIVE
+				|| pipe_data->disconnected,
+				HOST_WAKEUP_TIMEOUT_JIFFIES)) {
+
+			if (cnt == MAX_RETRY) {
+				mif_err("host wakeup timeout !!\n");
+				SET_SLAVE_WAKEUP(usb_ld->pdata, 0);
+				pm_runtime_put_autosuspend(&usbdev->dev);
+				schedule_work(&usb_ld->disconnect_work);
+				usb_ld->host_wake_timeout_flag = 1;
+				return -1;
+			}
+			mif_err("host wakeup timeout ! retry..\n");
+			SET_SLAVE_WAKEUP(usb_ld->pdata, 0);
+			udelay(100);
+			SET_SLAVE_WAKEUP(usb_ld->pdata, 1);
+			cnt++;
+		}
+
+		if (pipe_data->disconnected) {
+			SET_SLAVE_WAKEUP(usb_ld->pdata, 0);
+			pm_runtime_put_autosuspend(&usbdev->dev);
+			return -ENODEV;
+		}
+
+		mif_debug("wait_q done (runtime_status=%d)\n",
+				usbdev->dev.power.runtime_status);
+	}
+
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		mif_err("alloc urb error\n");
+		if (pm_runtime_put_autosuspend(&usbdev->dev) < 0)
+			mif_debug("pm_runtime_put_autosuspend fail\n");
+		return -ENOMEM;
+	}
+
+	urb->transfer_flags = URB_ZERO_PACKET;
+	usb_fill_bulk_urb(urb, usbdev, pipe_data->tx_pipe, skb->data,
+			skb->len, usb_tx_complete, (void *)skb);
+
+	spin_lock_irqsave(&usb_ld->lock, flags);
+	if (atomic_read(&usb_ld->suspend_count)) {
+		/* transmission will be done in resume */
+		usb_anchor_urb(urb, &usb_ld->deferred);
+		usb_put_urb(urb);
+		mif_debug("anchor urb (0x%p)\n", urb);
+		spin_unlock_irqrestore(&usb_ld->lock, flags);
+		return 0;
+	}
+	spin_unlock_irqrestore(&usb_ld->lock, flags);
+
+	ret = usb_submit_urb(urb, GFP_KERNEL);
+	if (ret < 0) {
+		mif_err("usb_submit_urb with ret(%d)\n", ret);
+		if (pm_runtime_put_autosuspend(&usbdev->dev) < 0)
+			mif_debug("pm_runtime_put_autosuspend fail\n");
+	}
+	return ret;
+}
+
+static void usb_tx_work(struct work_struct *work)
+{
+	int ret = 0;
+	struct link_device *ld =
+		container_of(work, struct link_device, tx_delayed_work.work);
+	struct usb_link_device *usb_ld = to_usb_link_device(ld);
+	struct io_device *iod;
+	struct sk_buff *skb;
+	struct if_usb_devdata *pipe_data;
+	struct link_pm_data *pm_data = usb_ld->link_pm_data;
+
+	/*TODO: check the PHONE ACTIVE STATES */
+	/* because tx data wait until hub on with wait_for_complettion, it
+	 should queue to single_threaded work queue */
+	if (!link_pm_set_active(usb_ld))
+		return;
+
+	while (ld->sk_fmt_tx_q.qlen || ld->sk_raw_tx_q.qlen) {
+		/* send skb from fmt_txq and raw_txq,
+		 * one by one for fair flow control */
+		skb = skb_dequeue(&ld->sk_fmt_tx_q);
+		if (skb) {
+			iod = skbpriv(skb)->iod;
+			switch (iod->format) {
+			case IPC_BOOT:
+			case IPC_RAMDUMP:
+			case IPC_FMT:
+				/* boot device uses same intf with fmt*/
+				pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
+				break;
+			case IPC_RFS:
+				pipe_data = &usb_ld->devdata[IF_USB_RFS_EP];
+				break;
+			default:
+				/* wrong packet for fmt tx q , drop it */
+				dev_kfree_skb_any(skb);
+				continue;
+			}
+
+			ret = usb_tx_urb_with_skb(usb_ld, skb, pipe_data);
+			if (ret < 0) {
+				mif_err("usb_tx_urb_with_skb, ret(%d)\n",
+					ret);
+				skb_queue_head(&ld->sk_fmt_tx_q, skb);
+				return;
+			}
+		}
+
+		skb = skb_dequeue(&ld->sk_raw_tx_q);
+		if (skb) {
+			pipe_data = &usb_ld->devdata[IF_USB_RAW_EP];
+			ret = usb_tx_urb_with_skb(usb_ld, skb, pipe_data);
+			if (ret < 0) {
+				mif_err("usb_tx_urb_with_skb "
+						"for raw, ret(%d)\n",
+						ret);
+				skb_queue_head(&ld->sk_raw_tx_q, skb);
+				return;
+			}
+		}
+	}
+}
+
+static int if_usb_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct usb_link_device *usb_ld = usb_get_intfdata(intf);
+	int i;
+
+	if (atomic_inc_return(&usb_ld->suspend_count) == IF_USB_DEVNUM_MAX) {
+		mif_debug("L2\n");
+
+		for (i = 0; i < IF_USB_DEVNUM_MAX; i++)
+			usb_kill_anchored_urbs(&usb_ld->devdata[i].reading);
+
+		if (usb_ld->link_pm_data->cpufreq_unlock)
+			usb_ld->link_pm_data->cpufreq_unlock();
+
+		wake_unlock(&usb_ld->susplock);
+	}
+
+	return 0;
+}
+
+static void runtime_pm_work(struct work_struct *work)
+{
+	struct usb_link_device *usb_ld = container_of(work,
+		struct usb_link_device, runtime_pm_work.work);
+	int ret;
+
+	ret = pm_request_autosuspend(&usb_ld->usbdev->dev);
+
+	if (ret == -EAGAIN || ret == 1)
+		queue_delayed_work(system_nrt_wq, &usb_ld->runtime_pm_work,
+							msecs_to_jiffies(50));
+}
+
+static void post_resume_work(struct work_struct *work)
+{
+	struct usb_link_device *usb_ld = container_of(work,
+			struct usb_link_device, post_resume_work.work);
+
+	/* lock cpu frequency when L2->L0 */
+	if (usb_ld->link_pm_data->cpufreq_lock)
+		usb_ld->link_pm_data->cpufreq_lock();
+}
+
+static void wait_enumeration_work(struct work_struct *work)
+{
+	struct usb_link_device *usb_ld = container_of(work,
+		struct usb_link_device, wait_enumeration.work);
+	if (usb_ld->if_usb_connected == 0) {
+		mif_err("USB disconnected and not enumerated for long time\n");
+		usb_change_modem_state(usb_ld, STATE_CRASH_EXIT);
+	}
+}
+
+static int if_usb_resume(struct usb_interface *intf)
+{
+	int i, ret;
+	struct sk_buff *skb;
+	struct usb_link_device *usb_ld = usb_get_intfdata(intf);
+	struct if_usb_devdata *pipe;
+	struct urb *urb;
+
+	spin_lock_irq(&usb_ld->lock);
+	if (!atomic_dec_return(&usb_ld->suspend_count)) {
+		spin_unlock_irq(&usb_ld->lock);
+
+		mif_debug("\n");
+		wake_lock(&usb_ld->susplock);
+
+		/* HACK: Runtime pm does not allow requesting autosuspend from
+		 * resume callback, delayed it after resume */
+		queue_delayed_work(system_nrt_wq, &usb_ld->runtime_pm_work,
+							msecs_to_jiffies(50));
+
+		for (i = 0; i < IF_USB_DEVNUM_MAX; i++) {
+			pipe = &usb_ld->devdata[i];
+			while ((urb = usb_get_from_anchor(&pipe->urbs))) {
+				ret = usb_rx_submit(pipe, urb, GFP_KERNEL);
+				if (ret < 0) {
+					usb_put_urb(urb);
+					mif_err(
+					"usb_rx_submit error with (%d)\n",
+						ret);
+					return ret;
+				}
+				usb_put_urb(urb);
+			}
+		}
+
+		while ((urb = usb_get_from_anchor(&usb_ld->deferred))) {
+			mif_debug("got urb (0x%p) from anchor & resubmit\n",
+					urb);
+			ret = usb_submit_urb(urb, GFP_KERNEL);
+			if (ret < 0) {
+				mif_err("resubmit failed\n");
+				skb = urb->context;
+				dev_kfree_skb_any(skb);
+				usb_free_urb(urb);
+				if (pm_runtime_put_autosuspend(
+					&usb_ld->usbdev->dev) < 0)
+					mif_debug(
+					"pm_runtime_put_autosuspend fail\n");
+			}
+		}
+		SET_SLAVE_WAKEUP(usb_ld->pdata, 1);
+		udelay(100);
+		SET_SLAVE_WAKEUP(usb_ld->pdata, 0);
+
+		/* if_usb_resume() is atomic. post_resume_work is
+		 * a kind of bottom halves
+		 */
+		queue_delayed_work(system_nrt_wq, &usb_ld->post_resume_work, 0);
+
+		return 0;
+	}
+
+	spin_unlock_irq(&usb_ld->lock);
+	return 0;
+}
+
+static int if_usb_reset_resume(struct usb_interface *intf)
+{
+	int ret;
+
+	mif_debug("\n");
+	ret = if_usb_resume(intf);
+	return ret;
+}
+
+static struct usb_device_id if_usb_ids[] = {
+	{ USB_DEVICE(0x04e8, 0x6999), /* CMC221 LTE Modem */
+	/*.driver_info = 0,*/
+	},
+	{ } /* terminating entry */
+};
+MODULE_DEVICE_TABLE(usb, if_usb_ids);
+
+static struct usb_driver if_usb_driver;
+static void if_usb_disconnect(struct usb_interface *intf)
+{
+	struct usb_link_device *usb_ld  = usb_get_intfdata(intf);
+	struct usb_device *usbdev = usb_ld->usbdev;
+	int dev_id = intf->altsetting->desc.bInterfaceNumber;
+	struct if_usb_devdata *pipe_data = &usb_ld->devdata[dev_id];
+
+	usb_set_intfdata(intf, NULL);
+
+	pipe_data->disconnected = 1;
+	smp_wmb();
+
+	wake_up(&usb_ld->l2_wait);
+
+	usb_ld->if_usb_connected = 0;
+	usb_ld->flow_suspend = 1;
+
+	dev_dbg(&usbdev->dev, "%s\n", __func__);
+	usb_ld->dev_count--;
+	usb_driver_release_interface(&if_usb_driver, pipe_data->data_intf);
+
+	usb_kill_anchored_urbs(&pipe_data->reading);
+	usb_free_urbs(usb_ld, pipe_data);
+
+	if (usb_ld->dev_count == 0) {
+		cancel_delayed_work_sync(&usb_ld->runtime_pm_work);
+		cancel_delayed_work_sync(&usb_ld->post_resume_work);
+		cancel_delayed_work_sync(&usb_ld->ld.tx_delayed_work);
+		usb_put_dev(usbdev);
+		usb_ld->usbdev = NULL;
+	}
+}
+
+static int __devinit if_usb_probe(struct usb_interface *intf,
+					const struct usb_device_id *id)
+{
+	struct usb_host_interface *data_desc;
+	struct usb_link_device *usb_ld =
+			(struct usb_link_device *)id->driver_info;
+	struct link_device *ld = &usb_ld->ld;
+	struct usb_interface *data_intf;
+	struct usb_device *usbdev = interface_to_usbdev(intf);
+	struct device *dev, *ehci_dev, *root_hub;
+	struct if_usb_devdata *pipe;
+	struct urb *urb;
+	int i;
+	int j;
+	int dev_id;
+	int err;
+
+	/* To detect usb device order probed */
+	dev_id = intf->cur_altsetting->desc.bInterfaceNumber;
+
+	if (dev_id >= IF_USB_DEVNUM_MAX) {
+		dev_err(&intf->dev, "Device id %d cannot support\n",
+								dev_id);
+		return -EINVAL;
+	}
+
+	if (!usb_ld) {
+		dev_err(&intf->dev,
+		"if_usb device doesn't be allocated\n");
+		err = ENOMEM;
+		goto out;
+	}
+
+	mif_info("probe dev_id=%d usb_device_id(0x%p), usb_ld (0x%p)\n",
+				dev_id, id, usb_ld);
+
+	usb_ld->usbdev = usbdev;
+
+	usb_get_dev(usbdev);
+
+	for (i = 0; i < IF_USB_DEVNUM_MAX; i++) {
+		data_intf = usb_ifnum_to_if(usbdev, i);
+
+		/* remap endpoint of RAW to no.1 for LTE modem */
+		if (i == 0)
+			pipe = &usb_ld->devdata[1];
+		else if (i == 1)
+			pipe = &usb_ld->devdata[0];
+		else
+			pipe = &usb_ld->devdata[i];
+
+		pipe->disconnected = 0;
+		pipe->data_intf = data_intf;
+		data_desc = data_intf->cur_altsetting;
+
+		/* Endpoints */
+		if (usb_pipein(data_desc->endpoint[0].desc.bEndpointAddress)) {
+			pipe->rx_pipe = usb_rcvbulkpipe(usbdev,
+				data_desc->endpoint[0].desc.bEndpointAddress);
+			pipe->tx_pipe = usb_sndbulkpipe(usbdev,
+				data_desc->endpoint[1].desc.bEndpointAddress);
+			pipe->rx_buf_size = 1024*4;
+		} else {
+			pipe->rx_pipe = usb_rcvbulkpipe(usbdev,
+				data_desc->endpoint[1].desc.bEndpointAddress);
+			pipe->tx_pipe = usb_sndbulkpipe(usbdev,
+				data_desc->endpoint[0].desc.bEndpointAddress);
+			pipe->rx_buf_size = 1024*4;
+		}
+
+		if (i == 0) {
+			dev_info(&usbdev->dev, "USB IF USB device found\n");
+		} else {
+			err = usb_driver_claim_interface(&if_usb_driver,
+					data_intf, usb_ld);
+			if (err < 0) {
+				mif_err("failed to cliam usb interface\n");
+				goto out;
+			}
+		}
+
+		usb_set_intfdata(data_intf, usb_ld);
+		usb_ld->dev_count++;
+		pm_suspend_ignore_children(&data_intf->dev, true);
+
+		for (j = 0; j < URB_COUNT; j++) {
+			urb = usb_alloc_urb(0, GFP_KERNEL);
+			if (!urb) {
+				mif_err("alloc urb fail\n");
+				err = -ENOMEM;
+				goto out2;
+			}
+
+			urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+			urb->transfer_buffer = usb_alloc_coherent(usbdev,
+				pipe->rx_buf_size, GFP_KERNEL,
+				&urb->transfer_dma);
+			if (!urb->transfer_buffer) {
+				mif_err(
+				"Failed to allocate transfer buffer\n");
+				usb_free_urb(urb);
+				err = -ENOMEM;
+				goto out2;
+			}
+
+			usb_fill_bulk_urb(urb, usbdev, pipe->rx_pipe,
+				urb->transfer_buffer, pipe->rx_buf_size,
+				usb_rx_complete, pipe);
+			usb_anchor_urb(urb, &pipe->urbs);
+		}
+	}
+
+	/* temporary call reset_resume */
+	atomic_set(&usb_ld->suspend_count, 1);
+	if_usb_reset_resume(data_intf);
+	atomic_set(&usb_ld->suspend_count, 0);
+
+	SET_HOST_ACTIVE(usb_ld->pdata, 1);
+	usb_ld->host_wake_timeout_flag = 0;
+
+	if (gpio_get_value(usb_ld->pdata->gpio_phone_active)) {
+		int delay = usb_ld->link_pm_data->autosuspend_delay_ms ?:
+				DEFAULT_AUTOSUSPEND_DELAY_MS;
+		pm_runtime_set_autosuspend_delay(&usbdev->dev, delay);
+		dev = &usbdev->dev;
+		if (dev->parent) {
+			dev_dbg(&usbdev->dev, "if_usb Runtime PM Start!!\n");
+			usb_enable_autosuspend(usb_ld->usbdev);
+			/* s5p-ehci runtime pm allow - usb phy suspend mode */
+			root_hub = &usbdev->bus->root_hub->dev;
+			ehci_dev = root_hub->parent;
+			mif_debug("ehci device = %s, %s\n",
+					dev_driver_string(ehci_dev),
+					dev_name(ehci_dev));
+			pm_runtime_allow(ehci_dev);
+
+			if (has_hub(usb_ld)) {
+				usb_ld->link_pm_data->hub_status =
+					(usb_ld->link_pm_data->root_hub) ?
+					HUB_STATE_PREACTIVE : HUB_STATE_ACTIVE;
+			}
+
+			usb_ld->link_pm_data->root_hub = root_hub;
+		}
+
+		usb_ld->flow_suspend = 0;
+		/* Queue work if skbs were pending before a disconnect/probe */
+		if (ld->sk_fmt_tx_q.qlen || ld->sk_raw_tx_q.qlen)
+			queue_delayed_work(ld->tx_wq, &ld->tx_delayed_work, 0);
+
+		usb_ld->if_usb_connected = 1;
+		/*USB3503*/
+		mif_debug("hub active complete\n");
+
+		usb_change_modem_state(usb_ld, STATE_ONLINE);
+	} else {
+		usb_change_modem_state(usb_ld, STATE_LOADER_DONE);
+	}
+
+	return 0;
+
+out2:
+	usb_ld->dev_count--;
+	for (i = 0; i < IF_USB_DEVNUM_MAX; i++)
+		usb_free_urbs(usb_ld, &usb_ld->devdata[i]);
+out:
+	usb_set_intfdata(intf, NULL);
+	return err;
+}
+
+irqreturn_t usb_resume_irq(int irq, void *data)
+{
+	int ret;
+	struct usb_link_device *usb_ld = data;
+	int hwup;
+	static int wake_status = -1;
+	struct device *dev;
+
+	hwup = gpio_get_value(usb_ld->pdata->gpio_host_wakeup);
+	if (hwup == wake_status) {
+		mif_err("Received spurious wake irq: %d", hwup);
+		return IRQ_HANDLED;
+	}
+	wake_status = hwup;
+
+	irq_set_irq_type(irq, hwup ? IRQF_TRIGGER_LOW : IRQF_TRIGGER_HIGH);
+	wake_lock_timeout(&usb_ld->gpiolock, 100);
+
+	mif_err("< H-WUP %d\n", hwup);
+
+	if (!link_pm_is_connected(usb_ld))
+		return IRQ_HANDLED;
+
+	if (hwup) {
+		dev = &usb_ld->usbdev->dev;
+		mif_info("runtime status=%d\n",
+				dev->power.runtime_status);
+
+		/* if usb3503 was on, usb_if was resumed by probe */
+		if (has_hub(usb_ld) &&
+				(dev->power.runtime_status == RPM_ACTIVE ||
+				dev->power.runtime_status == RPM_RESUMING))
+			return IRQ_HANDLED;
+
+		device_lock(dev);
+		if (dev->power.is_prepared || dev->power.is_suspended) {
+			pm_runtime_get_noresume(dev);
+			ret = 0;
+		} else {
+			ret = pm_runtime_get_sync(dev);
+		}
+		device_unlock(dev);
+		if (ret < 0) {
+			mif_err("pm_runtime_get fail (%d)\n", ret);
+			return IRQ_HANDLED;
+		}
+	} else {
+		if (usb_ld->resume_status == AP_INITIATED_RESUME)
+			wake_up(&usb_ld->l2_wait);
+		usb_ld->resume_status = CP_INITIATED_RESUME;
+		pm_runtime_mark_last_busy(&usb_ld->usbdev->dev);
+		pm_runtime_put_autosuspend(&usb_ld->usbdev->dev);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int if_usb_init(struct usb_link_device *usb_ld)
+{
+	int ret;
+	int i;
+	struct if_usb_devdata *pipe;
+
+	/* give it to probe, or global variable needed */
+	if_usb_ids[0].driver_info = (unsigned long)usb_ld;
+
+	for (i = 0; i < IF_USB_DEVNUM_MAX; i++) {
+		pipe = &usb_ld->devdata[i];
+		pipe->format = i;
+		pipe->disconnected = 1;
+		init_usb_anchor(&pipe->urbs);
+		init_usb_anchor(&pipe->reading);
+	}
+
+	init_waitqueue_head(&usb_ld->l2_wait);
+	init_usb_anchor(&usb_ld->deferred);
+
+	ret = usb_register(&if_usb_driver);
+	if (ret) {
+		mif_err("usb_register_driver() fail : %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+struct link_device *usb_create_link_device(void *data)
+{
+	int ret;
+	struct modem_data *pdata;
+	struct platform_device *pdev = (struct platform_device *)data;
+	struct usb_link_device *usb_ld = NULL;
+	struct link_device *ld = NULL;
+
+	pdata = pdev->dev.platform_data;
+
+	usb_ld = kzalloc(sizeof(struct usb_link_device), GFP_KERNEL);
+	if (!usb_ld)
+		goto err;
+
+	INIT_LIST_HEAD(&usb_ld->ld.list);
+	skb_queue_head_init(&usb_ld->ld.sk_fmt_tx_q);
+	skb_queue_head_init(&usb_ld->ld.sk_raw_tx_q);
+	spin_lock_init(&usb_ld->lock);
+
+	ld = &usb_ld->ld;
+	usb_ld->pdata = pdata;
+
+	ld->name = "usb";
+	ld->init_comm = usb_init_communication;
+	ld->terminate_comm = usb_terminate_communication;
+	ld->send = usb_send;
+	ld->com_state = COM_NONE;
+
+	/*ld->tx_wq = create_singlethread_workqueue("usb_tx_wq");*/
+	ld->tx_wq = alloc_workqueue("usb_tx_wq",
+		WQ_HIGHPRI | WQ_UNBOUND | WQ_RESCUER, 1);
+
+	if (!ld->tx_wq) {
+		mif_err("fail to create work Q.\n");
+		goto err;
+	}
+
+	usb_ld->pdata->irq_host_wakeup = platform_get_irq(pdev, 1);
+	wake_lock_init(&usb_ld->gpiolock, WAKE_LOCK_SUSPEND,
+		"modem_usb_gpio_wake");
+	wake_lock_init(&usb_ld->susplock, WAKE_LOCK_SUSPEND,
+		"modem_usb_suspend_block");
+
+	INIT_DELAYED_WORK(&ld->tx_delayed_work, usb_tx_work);
+	INIT_DELAYED_WORK(&usb_ld->runtime_pm_work, runtime_pm_work);
+	INIT_DELAYED_WORK(&usb_ld->post_resume_work, post_resume_work);
+	INIT_DELAYED_WORK(&usb_ld->wait_enumeration, wait_enumeration_work);
+	INIT_WORK(&usb_ld->disconnect_work, if_usb_force_disconnect);
+
+	/* create link pm device */
+	ret = link_pm_init(usb_ld, data);
+	if (ret)
+		goto err;
+
+	ret = if_usb_init(usb_ld);
+	if (ret)
+		goto err;
+
+	return ld;
+err:
+	if (ld && ld->tx_wq)
+		destroy_workqueue(ld->tx_wq);
+
+	kfree(usb_ld);
+
+	return NULL;
+}
+
+static struct usb_driver if_usb_driver = {
+	.name =		"if_usb_driver",
+	.probe =	if_usb_probe,
+	.disconnect =	if_usb_disconnect,
+	.id_table =	if_usb_ids,
+	.suspend =	if_usb_suspend,
+	.resume =	if_usb_resume,
+	.reset_resume =	if_usb_reset_resume,
+	.supports_autosuspend = 1,
+};
+
+static void __exit if_usb_exit(void)
+{
+	usb_deregister(&if_usb_driver);
+}
+
+
+/* lte specific functions */
+
+static int lte_wake_resume(struct device *pdev)
+{
+	struct modem_data *pdata = pdev->platform_data;
+	int val;
+
+	val = gpio_get_value(pdata->gpio_host_wakeup);
+	if (!val) {
+		mif_debug("> S-WUP 1\n");
+		gpio_set_value(pdata->gpio_slave_wakeup, 1);
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops lte_wake_pm_ops = {
+	.resume     = lte_wake_resume,
+};
+
+static struct platform_driver lte_wake_driver = {
+	.driver = {
+		.name = "modem_lte_wake",
+		.pm   = &lte_wake_pm_ops,
+	},
+};
+
+static int __init lte_wake_init(void)
+{
+	return platform_driver_register(&lte_wake_driver);
+}
+module_init(lte_wake_init);
diff --git a/drivers/misc/modem_if/modem_link_device_usb.h b/drivers/misc/modem_if/modem_link_device_usb.h
new file mode 100644
index 0000000..44f6b1b
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_device_usb.h
@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#ifndef __MODEM_LINK_DEVICE_USB_H__
+#define __MODEM_LINK_DEVICE_USB_H__
+
+#include <linux/usb.h>
+#include <linux/wakelock.h>
+
+#define IF_USB_DEVNUM_MAX	3
+
+#define IF_USB_FMT_EP		0
+#define IF_USB_RAW_EP		1
+#define IF_USB_RFS_EP		2
+
+#define DEFAULT_AUTOSUSPEND_DELAY_MS		500
+#define HOST_WAKEUP_TIMEOUT_JIFFIES		msecs_to_jiffies(500)
+#define WAIT_ENUMURATION_TIMEOUT_JIFFIES	msecs_to_jiffies(15000)
+#define MAX_RETRY	30
+
+#define IOCTL_LINK_CONTROL_ENABLE	_IO('o', 0x30)
+#define IOCTL_LINK_CONTROL_ACTIVE	_IO('o', 0x31)
+#define IOCTL_LINK_GET_HOSTWAKE		_IO('o', 0x32)
+#define IOCTL_LINK_CONNECTED		_IO('o', 0x33)
+#define IOCTL_LINK_SET_BIAS_CLEAR	_IO('o', 0x34)
+
+#define IOCTL_LINK_PORT_ON		_IO('o', 0x35)
+#define IOCTL_LINK_PORT_OFF		_IO('o', 0x36)
+
+enum RESUME_STATUS {
+	CP_INITIATED_RESUME,
+	AP_INITIATED_RESUME,
+};
+
+enum IPC_INIT_STATUS {
+	INIT_IPC_NOT_READY,
+	INIT_IPC_START_DONE,	/* send 'a' done */
+};
+
+enum hub_status {
+	HUB_STATE_OFF,		/* usb3503 0ff*/
+	HUB_STATE_RESUMMING,	/* usb3503 on, but enummerattion was not yet*/
+	HUB_STATE_PREACTIVE,
+	HUB_STATE_ACTIVE,	/* hub and CMC221 enumerate */
+};
+
+struct if_usb_devdata {
+	struct usb_interface *data_intf;
+	unsigned int tx_pipe;
+	unsigned int rx_pipe;
+	u8 disconnected;
+
+	int format;
+	struct usb_anchor urbs;
+	struct usb_anchor reading;
+	unsigned int rx_buf_size;
+};
+
+struct usb_link_device {
+	/*COMMON LINK DEVICE*/
+	struct link_device ld;
+
+	struct modem_data *pdata;
+
+	/*USB SPECIFIC LINK DEVICE*/
+	struct usb_device	*usbdev;
+	struct if_usb_devdata	devdata[IF_USB_DEVNUM_MAX];
+	struct delayed_work	runtime_pm_work;
+	struct delayed_work	post_resume_work;
+	struct delayed_work     wait_enumeration;
+	struct work_struct	disconnect_work;
+
+	struct wake_lock	gpiolock;
+	struct wake_lock	susplock;
+
+	unsigned int		dev_count;
+	unsigned int		suspended;
+	atomic_t		suspend_count;
+	enum RESUME_STATUS	resume_status;
+	int if_usb_connected;
+	int if_usb_initstates;
+	int flow_suspend;
+	int host_wake_timeout_flag;
+
+	unsigned gpio_slave_wakeup;
+	unsigned gpio_host_wakeup;
+	unsigned gpio_host_active;
+	int irq_host_wakeup;
+	struct delayed_work dwork;
+	struct work_struct resume_work;
+	int cpcrash_flag;
+	wait_queue_head_t l2_wait;
+
+	spinlock_t		lock;
+	struct usb_anchor	deferred;
+
+	/* LINK PM DEVICE DATA */
+	struct link_pm_data *link_pm_data;
+};
+/* converts from struct link_device* to struct xxx_link_device* */
+#define to_usb_link_device(linkdev) \
+			container_of(linkdev, struct usb_link_device, ld)
+
+#define SET_SLAVE_WAKEUP(_pdata, _value)			\
+do {								\
+	gpio_set_value(_pdata->gpio_slave_wakeup, _value);	\
+	mif_debug("> S-WUP %s\n", _value ? "1" : "0");	\
+} while (0)
+
+#define SET_HOST_ACTIVE(_pdata, _value)			\
+do {								\
+	gpio_set_value(_pdata->gpio_host_active, _value);	\
+	mif_debug("> H-ACT %s\n", _value ? "1" : "0");	\
+} while (0)
+
+#define has_hub(usb_ld) ((usb_ld)->link_pm_data->has_usbhub)
+
+irqreturn_t usb_resume_irq(int irq, void *data);
+
+#endif
diff --git a/drivers/misc/modem_if/modem_link_pm_usb.c b/drivers/misc/modem_if/modem_link_pm_usb.c
new file mode 100644
index 0000000..244256f
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_pm_usb.c
@@ -0,0 +1,342 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#define DEBUG
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+#include <linux/irq.h>
+#include <linux/poll.h>
+#include <linux/gpio.h>
+#include <linux/if_arp.h>
+#include <linux/platform_device.h>
+#include <linux/suspend.h>
+
+#include "modem_link_pm_usb.h"
+
+bool link_pm_is_connected(struct usb_link_device *usb_ld)
+{
+	if (has_hub(usb_ld)) {
+		if (usb_ld->link_pm_data->hub_init_lock)
+			return false;
+
+		if (usb_ld->link_pm_data->hub_status != HUB_STATE_ACTIVE) {
+			schedule_delayed_work(
+					&usb_ld->link_pm_data->link_pm_hub, 0);
+			return false;
+		}
+	}
+
+	if (!usb_ld->if_usb_connected) {
+		mif_err("mif: if not connected\n");
+		return false;
+	}
+
+	return true;
+}
+
+static void link_pm_hub_work(struct work_struct *work)
+{
+	int err;
+	struct link_pm_data *pm_data =
+		container_of(work, struct link_pm_data, link_pm_hub.work);
+
+	if (pm_data->hub_status == HUB_STATE_ACTIVE)
+		return;
+
+	if (!pm_data->port_enable) {
+		mif_err("mif: hub power func not assinged\n");
+		return;
+	}
+	wake_lock(&pm_data->hub_lock);
+
+	/* If kernel if suspend, wait the ehci resume */
+	if (pm_data->dpm_suspending) {
+		mif_info("dpm_suspending\n");
+		schedule_delayed_work(&pm_data->link_pm_hub,
+						msecs_to_jiffies(500));
+		goto exit;
+	}
+
+	switch (pm_data->hub_status) {
+	case HUB_STATE_OFF:
+		pm_data->hub_status = HUB_STATE_RESUMMING;
+		mif_trace("hub off->on\n");
+
+		/* skip 1st time before first probe */
+		if (pm_data->root_hub)
+			pm_runtime_get_sync(pm_data->root_hub);
+		err = pm_data->port_enable(2, 1);
+		if (err < 0) {
+			mif_err("hub on fail err=%d\n", err);
+			err = pm_data->port_enable(2, 0);
+			if (err < 0)
+				mif_err("hub off fail err=%d\n", err);
+			pm_data->hub_status = HUB_STATE_OFF;
+			if (pm_data->root_hub)
+				pm_runtime_put_sync(pm_data->root_hub);
+			goto exit;
+		}
+		/* resume root hub */
+		schedule_delayed_work(&pm_data->link_pm_hub,
+						msecs_to_jiffies(100));
+		break;
+	case HUB_STATE_RESUMMING:
+		if (pm_data->hub_on_retry_cnt++ > 50) {
+			pm_data->hub_on_retry_cnt = 0;
+			pm_data->hub_status = HUB_STATE_OFF;
+			if (pm_data->root_hub)
+				pm_runtime_put_sync(pm_data->root_hub);
+		}
+		mif_trace("hub resumming\n");
+		schedule_delayed_work(&pm_data->link_pm_hub,
+						msecs_to_jiffies(200));
+		break;
+	case HUB_STATE_PREACTIVE:
+		pm_data->hub_status = HUB_STATE_ACTIVE;
+		mif_trace("hub active\n");
+		pm_data->hub_on_retry_cnt = 0;
+		wake_unlock(&pm_data->hub_lock);
+		complete(&pm_data->hub_active);
+		if (pm_data->root_hub)
+			pm_runtime_put_sync(pm_data->root_hub);
+		break;
+	}
+exit:
+	return;
+}
+
+static int link_pm_hub_standby(struct link_pm_data *pm_data)
+{
+	struct usb_link_device *usb_ld = pm_data->usb_ld;
+	int err = 0;
+
+	mif_info("wait hub standby\n");
+
+	if (!pm_data->port_enable) {
+		mif_err("hub power func not assinged\n");
+		return -ENODEV;
+	}
+
+	err = pm_data->port_enable(2, 0);
+	if (err < 0)
+		mif_err("hub off fail err=%d\n", err);
+
+	pm_data->hub_status = HUB_STATE_OFF;
+	return err;
+}
+
+bool link_pm_set_active(struct usb_link_device *usb_ld)
+{
+	int ret;
+	struct link_pm_data *pm_data = usb_ld->link_pm_data;
+
+	if (has_hub(usb_ld)) {
+		if (pm_data->hub_status != HUB_STATE_ACTIVE) {
+			INIT_COMPLETION(pm_data->hub_active);
+			SET_SLAVE_WAKEUP(usb_ld->pdata, 1);
+			ret = wait_for_completion_timeout(&pm_data->hub_active,
+				msecs_to_jiffies(2000));
+			if (!ret) { /*timeout*/
+				mif_err("hub on timeout - retry\n");
+				SET_SLAVE_WAKEUP(usb_ld->pdata, 0);
+				queue_delayed_work(usb_ld->ld.tx_wq,
+						&usb_ld->ld.tx_delayed_work, 0);
+				return false;
+			}
+		}
+	} else {
+		/* TODO do something */
+	}
+	return true;
+}
+
+static long link_pm_ioctl(struct file *file, unsigned int cmd,
+						unsigned long arg)
+{
+	int value, err = 0;
+	struct link_pm_data *pm_data = file->private_data;
+
+	mif_info("cmd: 0x%08x\n", cmd);
+
+	switch (cmd) {
+	case IOCTL_LINK_CONTROL_ACTIVE:
+		if (copy_from_user(&value, (const void __user *)arg,
+							sizeof(int)))
+			return -EFAULT;
+		gpio_set_value(pm_data->gpio_link_active, value);
+		break;
+	case IOCTL_LINK_GET_HOSTWAKE:
+		return !gpio_get_value(pm_data->gpio_link_hostwake);
+	case IOCTL_LINK_CONNECTED:
+		return pm_data->usb_ld->if_usb_connected;
+	case IOCTL_LINK_PORT_ON: /* hub only */
+		/* ignore cp host wakeup irq, set the hub_init_lock when AP try
+		 CP off and release hub_init_lock when CP boot done */
+		pm_data->hub_init_lock = 0;
+		if (pm_data->root_hub) {
+			pm_runtime_resume(pm_data->root_hub);
+			pm_runtime_forbid(pm_data->root_hub->parent);
+		}
+		if (pm_data->port_enable) {
+			err = pm_data->port_enable(2, 1);
+			if (err < 0) {
+				mif_err("hub on fail err=%d\n", err);
+				goto exit;
+			}
+			pm_data->hub_status = HUB_STATE_RESUMMING;
+		}
+		break;
+	case IOCTL_LINK_PORT_OFF: /* hub only */
+		if (pm_data->usb_ld->if_usb_connected) {
+			struct usb_device *udev =
+					pm_data->usb_ld->usbdev->parent;
+			pm_runtime_get_sync(&udev->dev);
+			if (udev->state != USB_STATE_NOTATTACHED) {
+				usb_force_disconnect(udev);
+				pr_info("force disconnect maybe cp-reset!!\n");
+			}
+			pm_runtime_put_autosuspend(&udev->dev);
+		}
+		err = link_pm_hub_standby(pm_data);
+		if (err < 0) {
+			mif_err("usb3503 active fail\n");
+			goto exit;
+		}
+		pm_data->hub_init_lock = 1;
+		pm_data->hub_handshake_done = 0;
+
+		break;
+	default:
+		break;
+	}
+exit:
+	return err;
+}
+
+static int link_pm_open(struct inode *inode, struct file *file)
+{
+	struct link_pm_data *pm_data =
+		(struct link_pm_data *)file->private_data;
+	file->private_data = (void *)pm_data;
+	return 0;
+}
+
+static int link_pm_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+
+static const struct file_operations link_pm_fops = {
+	.owner = THIS_MODULE,
+	.open = link_pm_open,
+	.release = link_pm_release,
+	.unlocked_ioctl = link_pm_ioctl,
+};
+
+static int link_pm_notifier_event(struct notifier_block *this,
+				unsigned long event, void *ptr)
+{
+	struct link_pm_data *pm_data =
+			container_of(this, struct link_pm_data,	pm_notifier);
+
+	switch (event) {
+	case PM_SUSPEND_PREPARE:
+		pm_data->dpm_suspending = true;
+		link_pm_hub_standby(pm_data);
+		return NOTIFY_OK;
+	case PM_POST_SUSPEND:
+		pm_data->dpm_suspending = false;
+		return NOTIFY_OK;
+	}
+	return NOTIFY_DONE;
+}
+
+int link_pm_init(struct usb_link_device *usb_ld, void *data)
+{
+	int err;
+	int irq;
+	struct platform_device *pdev = (struct platform_device *)data;
+	struct modem_data *pdata =
+			(struct modem_data *)pdev->dev.platform_data;
+	struct modemlink_pm_data *pm_pdata = pdata->link_pm_data;
+	struct link_pm_data *pm_data =
+			kzalloc(sizeof(struct link_pm_data), GFP_KERNEL);
+	if (!pm_data) {
+		mif_err("link_pm_data is NULL\n");
+		return -ENOMEM;
+	}
+	/* get link pm data from modemcontrol's platform data */
+	pm_data->gpio_link_active = pm_pdata->gpio_link_active;
+	pm_data->gpio_link_hostwake = pm_pdata->gpio_link_hostwake;
+	pm_data->gpio_link_slavewake = pm_pdata->gpio_link_slavewake;
+	pm_data->link_reconnect = pm_pdata->link_reconnect;
+	pm_data->port_enable = pm_pdata->port_enable;
+	pm_data->cpufreq_lock = pm_pdata->cpufreq_lock;
+	pm_data->cpufreq_unlock = pm_pdata->cpufreq_unlock;
+	pm_data->autosuspend_delay_ms = pm_pdata->autosuspend_delay_ms;
+
+	pm_data->usb_ld = usb_ld;
+	pm_data->link_pm_active = false;
+	usb_ld->link_pm_data = pm_data;
+
+	pm_data->miscdev.minor = MISC_DYNAMIC_MINOR;
+	pm_data->miscdev.name = "link_pm";
+	pm_data->miscdev.fops = &link_pm_fops;
+
+	err = misc_register(&pm_data->miscdev);
+	if (err < 0) {
+		mif_err("fail to register pm device(%d)\n", err);
+		goto err_misc_register;
+	}
+
+	pm_data->hub_init_lock = 1;
+	irq = gpio_to_irq(usb_ld->pdata->gpio_host_wakeup);
+	err = request_threaded_irq(irq, NULL, usb_resume_irq,
+		IRQF_TRIGGER_HIGH | IRQF_ONESHOT, "modem_usb_wake", usb_ld);
+	if (err) {
+		mif_err("Failed to allocate an interrupt(%d)\n", irq);
+		goto err_request_irq;
+	}
+	enable_irq_wake(irq);
+
+	pm_data->has_usbhub = pm_pdata->has_usbhub;
+
+	if (has_hub(usb_ld)) {
+		init_completion(&pm_data->hub_active);
+		pm_data->hub_status = HUB_STATE_OFF;
+		pm_pdata->p_hub_status = &pm_data->hub_status;
+		pm_data->hub_handshake_done = 0;
+		pm_data->root_hub = NULL;
+		wake_lock_init(&pm_data->hub_lock, WAKE_LOCK_SUSPEND,
+				"modem_hub_enum_lock");
+		INIT_DELAYED_WORK(&pm_data->link_pm_hub, link_pm_hub_work);
+	}
+
+	pm_data->pm_notifier.notifier_call = link_pm_notifier_event;
+	register_pm_notifier(&pm_data->pm_notifier);
+
+	return 0;
+
+err_request_irq:
+	misc_deregister(&pm_data->miscdev);
+err_misc_register:
+	kfree(pm_data);
+	return err;
+}
+
+
diff --git a/drivers/misc/modem_if/modem_link_pm_usb.h b/drivers/misc/modem_if/modem_link_pm_usb.h
new file mode 100644
index 0000000..103e4f4
--- /dev/null
+++ b/drivers/misc/modem_if/modem_link_pm_usb.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#ifndef __MODEM_LINK_PM_USB_H__
+#define __MODEM_LINK_PM_USB_H__
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_link_device_usb.h"
+
+struct link_pm_data {
+	struct miscdevice miscdev;
+	struct usb_link_device *usb_ld;
+	unsigned gpio_link_active;
+	unsigned gpio_link_hostwake;
+	unsigned gpio_link_slavewake;
+	int (*link_reconnect)(void);
+	int link_reconnect_cnt;
+
+	struct workqueue_struct *wq;
+	struct completion active_done;
+
+/*USB3503*/
+	struct completion hub_active;
+	int hub_status;
+	bool has_usbhub;
+	/* ignore hub on by host wakeup irq before cp power on*/
+	int hub_init_lock;
+	/* C1 stay disconnect status after send 'a', skip 'a' next enumeration*/
+	int hub_handshake_done;
+	struct wake_lock hub_lock;
+	struct delayed_work link_pm_hub;
+	int hub_on_retry_cnt;
+	struct device *root_hub;
+
+	struct delayed_work link_pm_work;
+	struct delayed_work link_pm_start;
+	struct delayed_work link_reconnect_work;
+	bool resume_requested;
+	bool link_pm_active;
+
+	struct wake_lock l2_wake;
+	struct wake_lock boot_wake;
+	struct notifier_block pm_notifier;
+	bool dpm_suspending;
+
+	int (*port_enable)(int, int);
+
+	int (*cpufreq_lock)(void);
+	int (*cpufreq_unlock)(void);
+
+	int autosuspend_delay_ms; /* if zero, the default value is used */
+};
+
+bool link_pm_set_active(struct usb_link_device *usb_ld);
+bool link_pm_is_connected(struct usb_link_device *usb_ld);
+int link_pm_init(struct usb_link_device *usb_ld, void *data);
+
+#endif
diff --git a/drivers/misc/modem_if/modem_modemctl_device_cbp71.c b/drivers/misc/modem_if/modem_modemctl_device_cbp71.c
new file mode 100644
index 0000000..28f2ce7
--- /dev/null
+++ b/drivers/misc/modem_if/modem_modemctl_device_cbp71.c
@@ -0,0 +1,233 @@
+/* /linux/drivers/misc/modem_if/modem_modemctl_device_cbp7.1.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_link_device_dpram.h"
+
+#define PIF_TIMEOUT		(180 * HZ)
+#define DPRAM_INIT_TIMEOUT	(15 * HZ)
+
+static int cbp71_on(struct modem_ctl *mc)
+{
+	int RetVal = 0;
+	int dpram_init_RetVal = 0;
+	struct link_device *ld = get_current_link(mc->iod);
+	struct dpram_link_device *dpram_ld = to_dpram_link_device(ld);
+
+	mif_info("cbp71_on()\n");
+
+	if (!mc->gpio_cp_off || !mc->gpio_cp_reset) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	msleep(600);
+	gpio_set_value(mc->gpio_cp_reset, 1);
+	msleep(100);
+	gpio_set_value(mc->gpio_cp_off, 0);
+	msleep(300);
+
+	gpio_set_value(mc->gpio_pda_active, 1);
+
+	mc->iod->modem_state_changed(mc->iod, STATE_BOOTING);
+
+	/* Wait here until the PHONE is up.
+	* Waiting as the this called from IOCTL->UM thread */
+	mif_debug("power control waiting for INT_MASK_CMD_PIF_INIT_DONE\n");
+
+	/* 1HZ = 1 clock tick, 100 default */
+	dpram_ld->clear_interrupt(dpram_ld);
+
+	dpram_init_RetVal =
+		wait_event_interruptible_timeout(
+		dpram_ld->dpram_init_cmd_wait_q,
+					dpram_ld->dpram_init_cmd_wait_condition,
+					DPRAM_INIT_TIMEOUT);
+
+	if (!dpram_init_RetVal) {
+		/*RetVal will be 0 on timeout, non zero if interrupted */
+		mif_err("INIT_START cmd was not arrived.\n");
+		mif_err("init_cmd_wait_condition is 0 and wait timeout happend\n");
+		return -ENXIO;
+	}
+
+	RetVal = wait_event_interruptible_timeout(
+		dpram_ld->modem_pif_init_done_wait_q,
+					dpram_ld->modem_pif_init_wait_condition,
+					PIF_TIMEOUT);
+
+	if (!RetVal) {
+		/*RetVal will be 0 on timeout, non zero if interrupted */
+		mif_err("PIF init failed\n");
+		mif_err("pif_init_wait_condition is 0 and wait timeout happend\n");
+		return -ENXIO;
+	}
+
+	mif_debug("complete cbp71_on\n");
+
+	mc->iod->modem_state_changed(mc->iod, STATE_ONLINE);
+
+	return 0;
+}
+
+static int cbp71_off(struct modem_ctl *mc)
+{
+	mif_debug("cbp71_off()\n");
+
+	if (!mc->gpio_cp_off || !mc->gpio_cp_reset) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+
+	mif_err("Phone power Off. - do nothing\n");
+
+	mc->iod->modem_state_changed(mc->iod, STATE_OFFLINE);
+
+	return 0;
+}
+
+static int cbp71_reset(struct modem_ctl *mc)
+{
+	int ret = 0;
+
+	mif_debug("cbp71_reset()\n");
+
+	ret = cbp71_off(mc);
+	if (ret)
+		return -ENXIO;
+
+	msleep(100);
+
+	ret = cbp71_on(mc);
+	if (ret)
+		return -ENXIO;
+
+	return 0;
+}
+
+static int cbp71_boot_on(struct modem_ctl *mc)
+{
+	mif_debug("cbp71_boot_on()\n");
+
+	if (!mc->gpio_cp_reset) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	msleep(600);
+	gpio_set_value(mc->gpio_cp_reset, 1);
+
+	mc->iod->modem_state_changed(mc->iod, STATE_BOOTING);
+
+	return 0;
+}
+
+static int cbp71_boot_off(struct modem_ctl *mc)
+{
+	mif_debug("cbp71_boot_off()\n");
+	return 0;
+}
+
+static irqreturn_t phone_active_irq_handler(int irq, void *_mc)
+{
+	int phone_reset = 0;
+	int phone_active_value = 0;
+	int phone_state = 0;
+	struct modem_ctl *mc = (struct modem_ctl *)_mc;
+
+	if (!mc->gpio_cp_reset || !mc->gpio_phone_active) {
+		mif_err("no gpio data\n");
+		return IRQ_HANDLED;
+	}
+
+	phone_reset = gpio_get_value(mc->gpio_cp_reset);
+	phone_active_value = gpio_get_value(mc->gpio_phone_active);
+
+	if (phone_reset && phone_active_value)
+		phone_state = STATE_ONLINE;
+	else if (phone_reset && !phone_active_value)
+		phone_state = STATE_CRASH_EXIT;
+	else
+		phone_state = STATE_OFFLINE;
+
+	if (mc->iod && mc->iod->modem_state_changed)
+		mc->iod->modem_state_changed(mc->iod, phone_state);
+
+	if (phone_active_value)
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_LOW);
+	else
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_HIGH);
+
+	mif_info("phone_active_irq_handler : phone_state=%d\n", phone_state);
+
+	return IRQ_HANDLED;
+}
+
+static void cbp71_get_ops(struct modem_ctl *mc)
+{
+	mc->ops.modem_on = cbp71_on;
+	mc->ops.modem_off = cbp71_off;
+	mc->ops.modem_reset = cbp71_reset;
+	mc->ops.modem_boot_on = cbp71_boot_on;
+	mc->ops.modem_boot_off = cbp71_boot_off;
+}
+
+int cbp71_init_modemctl_device(struct modem_ctl *mc,
+			struct modem_data *pdata)
+{
+	int ret = 0;
+	struct platform_device *pdev;
+
+	mc->gpio_cp_on = pdata->gpio_cp_on;
+	mc->gpio_reset_req_n = pdata->gpio_reset_req_n;
+	mc->gpio_cp_reset = pdata->gpio_cp_reset;
+	mc->gpio_pda_active = pdata->gpio_pda_active;
+	mc->gpio_phone_active = pdata->gpio_phone_active;
+	mc->gpio_cp_dump_int = pdata->gpio_cp_dump_int;
+	mc->gpio_flm_uart_sel = pdata->gpio_flm_uart_sel;
+	mc->gpio_cp_warm_reset = pdata->gpio_cp_warm_reset;
+	mc->gpio_cp_off = pdata->gpio_cp_off;
+
+	pdev = to_platform_device(mc->dev);
+	mc->irq_phone_active = platform_get_irq(pdev, 0);
+
+	cbp71_get_ops(mc);
+
+	/*TODO: check*/
+	ret = request_irq(mc->irq_phone_active, phone_active_irq_handler,
+			IRQF_TRIGGER_HIGH, "phone_active", mc);
+	if (ret) {
+		mif_err("failed to irq_phone_active request_irq: %d\n"
+			, ret);
+		return ret;
+	}
+
+	ret = enable_irq_wake(mc->irq_phone_active);
+	if (ret)
+		mif_err("failed to enable_irq_wake:%d\n", ret);
+
+	return ret;
+}
diff --git a/drivers/misc/modem_if/modem_modemctl_device_cbp72.c b/drivers/misc/modem_if/modem_modemctl_device_cbp72.c
new file mode 100644
index 0000000..b8d2711
--- /dev/null
+++ b/drivers/misc/modem_if/modem_modemctl_device_cbp72.c
@@ -0,0 +1,262 @@
+/* /linux/drivers/misc/modem_if/modem_modemctl_device_cbp7.1.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/platform_device.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_link_device_dpram.h"
+
+#define PIF_TIMEOUT		(180 * HZ)
+#define DPRAM_INIT_TIMEOUT	(30 * HZ)
+
+
+static irqreturn_t phone_active_handler(int irq, void *arg)
+{
+	struct modem_ctl *mc = (struct modem_ctl *)arg;
+	int phone_reset = gpio_get_value(mc->gpio_cp_reset);
+	int phone_active = gpio_get_value(mc->gpio_phone_active);
+	int phone_state = mc->phone_state;
+
+	mif_info("state = %d, phone_reset = %d, phone_active = %d\n",
+		phone_state, phone_reset, phone_active);
+
+	if (phone_reset && phone_active) {
+		phone_state = STATE_ONLINE;
+		mc->bootd->modem_state_changed(mc->bootd, phone_state);
+	} else if (phone_reset && !phone_active) {
+		if (mc->phone_state == STATE_ONLINE) {
+			phone_state = STATE_CRASH_EXIT;
+			mc->bootd->modem_state_changed(mc->bootd, phone_state);
+		}
+	} else {
+		phone_state = STATE_OFFLINE;
+		if (mc->bootd && mc->bootd->modem_state_changed)
+			mc->bootd->modem_state_changed(mc->bootd, phone_state);
+	}
+
+	if (phone_active)
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_LOW);
+	else
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_HIGH);
+
+	mif_info("phone_state = %d\n", phone_state);
+
+	return IRQ_HANDLED;
+}
+
+static int cbp72_on(struct modem_ctl *mc)
+{
+	mif_info("start!!!\n");
+
+	gpio_set_value(mc->gpio_cp_on, 0);
+	if (mc->gpio_cp_off)
+		gpio_set_value(mc->gpio_cp_off, 1);
+	gpio_set_value(mc->gpio_cp_reset, 0);
+
+	msleep(500);
+
+	gpio_set_value(mc->gpio_cp_on, 1);
+	if (mc->gpio_cp_off)
+		gpio_set_value(mc->gpio_cp_off, 0);
+
+	msleep(100);
+
+	gpio_set_value(mc->gpio_cp_reset, 1);
+
+	msleep(300);
+
+	gpio_set_value(mc->gpio_pda_active, 1);
+
+	mc->bootd->modem_state_changed(mc->bootd, STATE_BOOTING);
+
+	mif_info("complete!!!\n");
+
+	return 0;
+}
+
+static int cbp72_off(struct modem_ctl *mc)
+{
+	mif_info("cbp72_off()\n");
+
+	if (!mc->gpio_cp_off || !mc->gpio_cp_reset) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	gpio_set_value(mc->gpio_cp_on, 0);
+	gpio_set_value(mc->gpio_cp_off, 1);
+
+	mc->bootd->modem_state_changed(mc->bootd, STATE_OFFLINE);
+
+	return 0;
+}
+
+static int cbp72_reset(struct modem_ctl *mc)
+{
+	int ret = 0;
+
+	mif_debug("cbp72_reset()\n");
+
+	ret = cbp72_off(mc);
+	if (ret)
+		return -ENXIO;
+
+	msleep(100);
+
+	ret = cbp72_on(mc);
+	if (ret)
+		return -ENXIO;
+
+	return 0;
+}
+
+static int cbp72_boot_on(struct modem_ctl *mc)
+{
+	mif_info("\n");
+
+	if (!mc->gpio_cp_reset) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_cp_reset, 0);
+
+	msleep(600);
+
+	gpio_set_value(mc->gpio_cp_reset, 1);
+
+	mc->bootd->modem_state_changed(mc->bootd, STATE_BOOTING);
+
+	return 0;
+}
+
+static int cbp72_boot_off(struct modem_ctl *mc)
+{
+	int ret;
+	struct link_device *ld = get_current_link(mc->bootd);
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+	mif_debug("\n");
+	/* Wait here until the PHONE is up.
+	 * Waiting as the this called from IOCTL->UM thread */
+	mif_info("Waiting for INT_CMD_PHONE_START\n");
+	ret = wait_for_completion_interruptible_timeout(
+			&dpld->dpram_init_cmd, DPRAM_INIT_TIMEOUT);
+	if (!ret) {
+		/* ret == 0 on timeout, ret < 0 if interrupted */
+		mif_err("Timeout!!! (PHONE_START was not arrived.)\n");
+		return -ENXIO;
+	}
+
+	mif_info("Waiting for INT_CMD_PIF_INIT_DONE\n");
+	ret = wait_for_completion_interruptible_timeout(
+			&dpld->modem_pif_init_done, PIF_TIMEOUT);
+	if (!ret) {
+		mif_err("Timeout!!! (PIF_INIT_DONE was not arrived.)\n");
+		return -ENXIO;
+	}
+	mc->bootd->modem_state_changed(mc->bootd, STATE_ONLINE);
+	return 0;
+}
+
+static int cbp72_force_crash_exit(struct modem_ctl *mc)
+{
+	struct link_device *ld = get_current_link(mc->bootd);
+
+	mif_err("device = %s\n", mc->bootd->name);
+
+	/* Make DUMP start */
+	ld->force_dump(ld, mc->bootd);
+
+	msleep_interruptible(1000);
+
+	mc->bootd->modem_state_changed(mc->bootd, STATE_CRASH_EXIT);
+
+	return 0;
+}
+
+static void cbp72_get_ops(struct modem_ctl *mc)
+{
+	mc->ops.modem_on = cbp72_on;
+	mc->ops.modem_off = cbp72_off;
+	mc->ops.modem_reset = cbp72_reset;
+	mc->ops.modem_boot_on = cbp72_boot_on;
+	mc->ops.modem_boot_off = cbp72_boot_off;
+	mc->ops.modem_force_crash_exit = cbp72_force_crash_exit;
+}
+
+int cbp72_init_modemctl_device(struct modem_ctl *mc, struct modem_data *pdata)
+{
+	int ret = 0;
+	int irq = 0;
+	unsigned long flag = 0;
+	struct platform_device *pdev = NULL;
+
+	mc->gpio_cp_on         = pdata->gpio_cp_on;
+	mc->gpio_cp_off        = pdata->gpio_cp_off;
+	mc->gpio_reset_req_n   = pdata->gpio_reset_req_n;
+	mc->gpio_cp_reset      = pdata->gpio_cp_reset;
+	mc->gpio_pda_active    = pdata->gpio_pda_active;
+	mc->gpio_phone_active  = pdata->gpio_phone_active;
+	mc->gpio_cp_dump_int   = pdata->gpio_cp_dump_int;
+	mc->gpio_flm_uart_sel  = pdata->gpio_flm_uart_sel;
+	mc->gpio_cp_warm_reset = pdata->gpio_cp_warm_reset;
+
+	if (!mc->gpio_cp_on || !mc->gpio_cp_reset || !mc->gpio_phone_active) {
+		mif_err("no GPIO data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	if (mc->gpio_cp_off)
+		gpio_set_value(mc->gpio_cp_off, 1);
+	gpio_set_value(mc->gpio_cp_on, 0);
+
+	cbp72_get_ops(mc);
+
+	pdev = to_platform_device(mc->dev);
+	mc->irq_phone_active = platform_get_irq_byname(pdev, "cp_active_irq");
+	if (!mc->irq_phone_active) {
+		mif_err("get irq fail\n");
+		return -1;
+	}
+
+	irq = mc->irq_phone_active;
+	mif_info("PHONE_ACTIVE IRQ# = %d\n", irq);
+
+	flag = IRQF_TRIGGER_HIGH;
+	ret = request_irq(irq, phone_active_handler, flag, "cbp_active", mc);
+	if (ret) {
+		mif_err("request_irq fail (%d)\n", ret);
+		return ret;
+	}
+
+	ret = enable_irq_wake(irq);
+	if (ret)
+		mif_err("enable_irq_wake fail (%d)\n", ret);
+
+	return 0;
+}
+
diff --git a/drivers/misc/modem_if/modem_modemctl_device_cmc221.c b/drivers/misc/modem_if/modem_modemctl_device_cmc221.c
new file mode 100644
index 0000000..2d564e9
--- /dev/null
+++ b/drivers/misc/modem_if/modem_modemctl_device_cmc221.c
@@ -0,0 +1,291 @@
+/* /linux/drivers/misc/modem_if/modem_modemctl_device_cmc221.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_link_device_usb.h"
+#include "modem_link_device_dpram.h"
+#include "modem_utils.h"
+
+#define PIF_TIMEOUT		(180 * HZ)
+#define DPRAM_INIT_TIMEOUT	(30 * HZ)
+
+
+static void mc_state_fsm(struct modem_ctl *mc)
+{
+	int cp_on = gpio_get_value(mc->gpio_cp_on);
+	int cp_reset  = gpio_get_value(mc->gpio_cp_reset);
+	int cp_active = gpio_get_value(mc->gpio_phone_active);
+	int old_state = mc->phone_state;
+	int new_state = mc->phone_state;
+
+	mif_err("%s: old_state:%d cp_on:%d cp_reset:%d cp_active:%d\n",
+		mc->name, old_state, cp_on, cp_reset, cp_active);
+
+	if (!cp_active) {
+		if (!cp_on) {
+			gpio_set_value(mc->gpio_cp_reset, 0);
+			new_state = STATE_OFFLINE;
+			mif_err("%s: new_state = PHONE_PWR_OFF\n", mc->name);
+		} else if (old_state == STATE_ONLINE) {
+			new_state = STATE_CRASH_EXIT;
+			mif_err("%s: new_state = CRASH_EXIT\n", mc->name);
+		} else {
+			mif_err("%s: Don't care!!!\n", mc->name);
+		}
+	}
+
+exit:
+	if (old_state != new_state) {
+		mc->bootd->modem_state_changed(mc->bootd, new_state);
+		mc->iod->modem_state_changed(mc->iod, new_state);
+	}
+}
+
+static irqreturn_t phone_active_handler(int irq, void *arg)
+{
+	struct modem_ctl *mc = (struct modem_ctl *)arg;
+	int cp_reset  = gpio_get_value(mc->gpio_cp_reset);
+
+	if (cp_reset)
+		mc_state_fsm(mc);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t dynamic_switching_handler(int irq, void *arg)
+{
+	struct modem_ctl *mc = (struct modem_ctl *)arg;
+	int txpath = gpio_get_value(mc->gpio_dynamic_switching);
+
+	rawdevs_set_tx_link(&mc->commons, txpath ? LINKDEV_USB : LINKDEV_DPRAM);
+
+	return IRQ_HANDLED;
+}
+
+static int cmc221_on(struct modem_ctl *mc)
+{
+	struct link_device *ld = get_current_link(mc->bootd);
+
+	mif_err("%s\n", mc->bootd->name);
+
+	disable_irq_nosync(mc->irq_phone_active);
+
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	gpio_set_value(mc->gpio_cp_on, 0);
+
+	msleep(500);
+
+	gpio_set_value(mc->gpio_cp_on, 1);
+
+	msleep(100);
+
+	gpio_set_value(mc->gpio_cp_reset, 1);
+
+	mc->phone_state = STATE_OFFLINE;
+
+	return 0;
+}
+
+static int cmc221_off(struct modem_ctl *mc)
+{
+	int cp_on = gpio_get_value(mc->gpio_cp_on);
+
+	if (mc->phone_state == STATE_OFFLINE || cp_on == 0)
+		return 0;
+
+	mif_err("%s\n", mc->bootd->name);
+
+	if (mc->log_fp)
+		mif_close_log_file(mc);
+
+	gpio_set_value(mc->gpio_cp_on, 0);
+
+	return 0;
+}
+
+static int cmc221_force_crash_exit(struct modem_ctl *mc)
+{
+	struct link_device *ld = get_current_link(mc->bootd);
+
+	mif_err("%s\n", mc->bootd->name);
+
+	/* Make DUMP start */
+	ld->force_dump(ld, mc->bootd);
+
+	return 0;
+}
+
+static int cmc221_dump_reset(struct modem_ctl *mc)
+{
+	mif_err("%s\n", mc->bootd->name);
+
+	if (mc->log_fp)
+		mif_close_log_file(mc);
+
+	gpio_set_value(mc->gpio_host_active, 0);
+	gpio_set_value(mc->gpio_cp_reset, 0);
+
+	udelay(200);
+
+	gpio_set_value(mc->gpio_cp_reset, 1);
+
+	msleep(300);
+
+	return 0;
+}
+
+static int cmc221_reset(struct modem_ctl *mc)
+{
+	mif_err("%s\n", mc->bootd->name);
+
+	if (cmc221_off(mc))
+		return -ENXIO;
+
+	msleep(100);
+
+	if (cmc221_on(mc))
+		return -ENXIO;
+
+	return 0;
+}
+
+static int cmc221_boot_on(struct modem_ctl *mc)
+{
+	mif_err("%s\n", mc->bootd->name);
+
+	mc->bootd->modem_state_changed(mc->bootd, STATE_BOOTING);
+	mc->iod->modem_state_changed(mc->iod, STATE_BOOTING);
+
+	mif_set_log_level(mc);
+
+	return 0;
+}
+
+static int cmc221_boot_off(struct modem_ctl *mc)
+{
+	int ret;
+	struct link_device *ld = get_current_link(mc->bootd);
+	struct dpram_link_device *dpld = to_dpram_link_device(ld);
+
+	mif_err("%s\n", mc->bootd->name);
+
+	ret = wait_for_completion_interruptible_timeout(&dpld->dpram_init_cmd,
+			DPRAM_INIT_TIMEOUT);
+	if (!ret) {
+		/* ret == 0 on timeout, ret < 0 if interrupted */
+		mif_err("%s: ERR! timeout (CP_START not arrived)\n", mc->name);
+		return -ENXIO;
+	}
+
+	if (!mc->fs_ready)
+		mc->fs_ready = true;
+
+	if (mc->use_mif_log && mc->log_level && !mc->fs_failed &&
+	    mc->fs_ready && !mc->log_fp)
+		mif_open_log_file(mc);
+
+	enable_irq(mc->irq_phone_active);
+
+	return 0;
+}
+
+static void cmc221_get_ops(struct modem_ctl *mc)
+{
+	mc->ops.modem_on = cmc221_on;
+	mc->ops.modem_off = cmc221_off;
+	mc->ops.modem_reset = cmc221_reset;
+	mc->ops.modem_boot_on = cmc221_boot_on;
+	mc->ops.modem_boot_off = cmc221_boot_off;
+	mc->ops.modem_force_crash_exit = cmc221_force_crash_exit;
+	mc->ops.modem_dump_reset = cmc221_dump_reset;
+}
+
+int cmc221_init_modemctl_device(struct modem_ctl *mc, struct modem_data *pdata)
+{
+	int ret = 0;
+	int irq = 0;
+	unsigned long flag = 0;
+	struct platform_device *pdev = NULL;
+
+	mc->gpio_cp_on        = pdata->gpio_cp_on;
+	mc->gpio_cp_reset     = pdata->gpio_cp_reset;
+	mc->gpio_phone_active = pdata->gpio_phone_active;
+#if 0	/*TODO: check the GPIO map*/
+	mc->gpio_pda_active   = pdata->gpio_pda_active;
+	mc->gpio_cp_dump_int  = pdata->gpio_cp_dump_int;
+	mc->gpio_flm_uart_sel = pdata->gpio_flm_uart_sel;
+	mc->gpio_slave_wakeup = pdata->gpio_slave_wakeup;
+	mc->gpio_host_active  = pdata->gpio_host_active;
+	mc->gpio_host_wakeup  = pdata->gpio_host_wakeup;
+#endif
+	mc->gpio_dynamic_switching = pdata->gpio_dynamic_switching;
+
+	if (!mc->gpio_cp_on || !mc->gpio_cp_reset || !mc->gpio_phone_active) {
+		mif_err("%s: ERR! no GPIO data\n", mc->name);
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	gpio_set_value(mc->gpio_cp_on, 0);
+
+	cmc221_get_ops(mc);
+	dev_set_drvdata(mc->dev, mc);
+
+	pdev = to_platform_device(mc->dev);
+	mc->irq_phone_active = platform_get_irq_byname(pdev, "cp_active_irq");
+	if (!mc->irq_phone_active) {
+		mif_err("%s: ERR! get cp_active_irq fail\n", mc->name);
+		return -1;
+	}
+	mif_err("%s: PHONE_ACTIVE IRQ# = %d\n", mc->name, mc->irq_phone_active);
+
+	flag = IRQF_TRIGGER_FALLING | IRQF_NO_SUSPEND;
+	irq = mc->irq_phone_active;
+	ret = request_irq(irq, phone_active_handler, flag, "cmc_active", mc);
+	if (ret) {
+		mif_err("%s: ERR! request_irq(#%d) fail (err %d)\n",
+			mc->name, irq, ret);
+		return ret;
+	}
+	ret = enable_irq_wake(irq);
+	if (ret) {
+		mif_err("%s: WARNING! enable_irq_wake(#%d) fail (err %d)\n",
+			mc->name, irq, ret);
+	}
+
+	flag = IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING | IRQF_NO_SUSPEND;
+	if (mc->gpio_dynamic_switching) {
+		irq = gpio_to_irq(mc->gpio_dynamic_switching);
+		mif_err("%s: DYNAMIC_SWITCH IRQ# = %d\n", mc->name, irq);
+		ret = request_irq(irq, dynamic_switching_handler, flag,
+				"dynamic_switching", mc);
+		if (ret) {
+			mif_err("%s: ERR! request_irq(#%d) fail (err %d)\n",
+				mc->name, irq, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/misc/modem_if/modem_modemctl_device_mdm6600.c b/drivers/misc/modem_if/modem_modemctl_device_mdm6600.c
new file mode 100644
index 0000000..6f1807e
--- /dev/null
+++ b/drivers/misc/modem_if/modem_modemctl_device_mdm6600.c
@@ -0,0 +1,562 @@
+/* /linux/drivers/misc/modem_if/modem_modemctl_device_mdm6600.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/platform_device.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include <linux/regulator/consumer.h>
+
+#include <plat/gpio-cfg.h>
+
+#if defined(CONFIG_MACH_M0_CTC)
+#include <linux/mfd/max77693.h>
+#endif
+
+#if defined(CONFIG_MACH_U1_KOR_LGT)
+#include <linux/mfd/max8997.h>
+
+static int mdm6600_on(struct modem_ctl *mc)
+{
+	pr_info("[MODEM_IF] mdm6600_on()\n");
+
+	if (!mc->gpio_cp_reset || !mc->gpio_cp_reset_msm || !mc->gpio_cp_on) {
+		pr_err("[MODEM_IF] no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_pda_active, 0);
+	gpio_set_value(mc->gpio_cp_reset, 1);
+	gpio_set_value(mc->gpio_cp_reset_msm, 1);
+	msleep(30);
+	gpio_set_value(mc->gpio_cp_on, 1);
+	msleep(300);
+	gpio_set_value(mc->gpio_cp_on, 0);
+	msleep(500);
+
+	gpio_set_value(mc->gpio_pda_active, 1);
+
+	mc->iod->modem_state_changed(mc->iod, STATE_BOOTING);
+
+	return 0;
+}
+
+static int mdm6600_off(struct modem_ctl *mc)
+{
+	pr_info("[MODEM_IF] mdm6600_off()\n");
+
+	if (!mc->gpio_cp_reset || !mc->gpio_cp_reset_msm || !mc->gpio_cp_on) {
+		pr_err("[MODEM_IF] no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_cp_on, 0);
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	gpio_set_value(mc->gpio_cp_reset_msm, 0);
+
+	mc->iod->modem_state_changed(mc->iod, STATE_OFFLINE);
+
+	return 0;
+}
+
+static int mdm6600_reset(struct modem_ctl *mc)
+{
+	int ret;
+
+	pr_info("[MODEM_IF] mdm6600_reset()\n");
+
+	if (!mc->gpio_cp_reset || !mc->gpio_cp_reset_msm || !mc->gpio_cp_on) {
+		pr_err("[MODEM_IF] no gpio data\n");
+		return -ENXIO;
+	}
+
+	if (system_rev >= 0x05) {
+		dev_err(mc->dev, "[%s] system_rev: %d\n", __func__, system_rev);
+
+		gpio_set_value(mc->gpio_cp_reset_msm, 0);
+		msleep(100);	/* no spec, confirm later exactly how much time
+				   needed to initialize CP with RESET_PMU_N */
+		gpio_set_value(mc->gpio_cp_reset_msm, 1);
+		msleep(40);	/* > 37.2 + 2 msec */
+	} else {
+		dev_err(mc->dev, "[%s] system_rev: %d\n", __func__, system_rev);
+
+		gpio_set_value(mc->gpio_cp_reset, 0);
+		msleep(500);	/* no spec, confirm later exactly how much time
+				   needed to initialize CP with RESET_PMU_N */
+		gpio_set_value(mc->gpio_cp_reset, 1);
+		msleep(40);	/* > 37.2 + 2 msec */
+	}
+
+	return 0;
+}
+
+static int mdm6600_boot_on(struct modem_ctl *mc)
+{
+	pr_info("[MODEM_IF] mdm6600_boot_on()\n");
+
+	if (!mc->gpio_boot_sw_sel) {
+		pr_err("[MODEM_IF] no gpio data\n");
+		return -ENXIO;
+	}
+
+	if (mc->vbus_on)
+		mc->vbus_on();
+
+	if (mc->gpio_boot_sw_sel)
+		gpio_set_value(mc->gpio_boot_sw_sel, 0);
+	mc->usb_boot = true;
+
+	return 0;
+}
+
+static int mdm6600_boot_off(struct modem_ctl *mc)
+{
+	pr_info("[MODEM_IF] mdm6600_boot_off()\n");
+
+	if (!mc->gpio_boot_sw_sel) {
+		pr_err("[MODEM_IF] no gpio data\n");
+		return -ENXIO;
+	}
+
+	if (mc->vbus_off)
+		mc->vbus_off();
+
+	if (mc->gpio_boot_sw_sel)
+		gpio_set_value(mc->gpio_boot_sw_sel, 1);
+	mc->usb_boot = false;
+
+	return 0;
+}
+
+static int count;
+
+static irqreturn_t phone_active_irq_handler(int irq, void *_mc)
+{
+	int phone_reset = 0;
+	int phone_active_value = 0;
+	int cp_dump_value = 0;
+	int phone_state = 0;
+	struct modem_ctl *mc = (struct modem_ctl *)_mc;
+
+	if (!mc->gpio_cp_reset || !mc->gpio_phone_active
+/*|| !mc->gpio_cp_dump_int */) {
+		pr_err("[MODEM_IF] no gpio data\n");
+		return IRQ_HANDLED;
+	}
+
+	phone_reset = gpio_get_value(mc->gpio_cp_reset);
+	phone_active_value = gpio_get_value(mc->gpio_phone_active);
+
+	pr_info("[MODEM_IF] PA EVENT : reset =%d, pa=%d, cp_dump=%d\n",
+		phone_reset, phone_active_value, cp_dump_value);
+
+	if (phone_reset && phone_active_value) {
+		phone_state = STATE_ONLINE;
+		if (mc->iod && mc->iod->modem_state_changed)
+			mc->iod->modem_state_changed(mc->iod, phone_state);
+	} else if (phone_reset && !phone_active_value) {
+		if (count == 1) {
+			phone_state = STATE_CRASH_EXIT;
+			if (mc->iod) {
+				ld = get_current_link(mc->iod);
+				if (ld->terminate_comm)
+					ld->terminate_comm(ld, mc->iod);
+			}
+			if (mc->iod && mc->iod->modem_state_changed)
+				mc->iod->modem_state_changed
+				    (mc->iod, phone_state);
+			count = 0;
+		} else {
+			count++;
+		}
+	} else {
+		phone_state = STATE_OFFLINE;
+		if (mc->iod && mc->iod->modem_state_changed)
+			mc->iod->modem_state_changed(mc->iod, phone_state);
+	}
+
+	pr_info("phone_active_irq_handler : phone_state=%d\n", phone_state);
+
+	return IRQ_HANDLED;
+}
+
+static void mdm6600_get_ops(struct modem_ctl *mc)
+{
+	mc->ops.modem_on = mdm6600_on;
+	mc->ops.modem_off = mdm6600_off;
+	mc->ops.modem_reset = mdm6600_reset;
+	mc->ops.modem_boot_on = mdm6600_boot_on;
+	mc->ops.modem_boot_off = mdm6600_boot_off;
+}
+
+int mdm6600_init_modemctl_device(struct modem_ctl *mc, struct modem_data *pdata)
+{
+	int ret;
+	struct platform_device *pdev;
+
+	mc->gpio_cp_on = pdata->gpio_cp_on;
+	mc->gpio_cp_reset = pdata->gpio_cp_reset;
+	mc->gpio_pda_active = pdata->gpio_pda_active;
+	mc->gpio_phone_active = pdata->gpio_phone_active;
+	mc->gpio_cp_reset_msm = pdata->gpio_cp_reset_msm;
+	mc->gpio_boot_sw_sel = pdata->gpio_boot_sw_sel;
+
+	mc->vbus_on = pdata->vbus_on;
+	mc->vbus_off = pdata->vbus_off;
+
+	pdev = to_platform_device(mc->dev);
+	mc->irq_phone_active = platform_get_irq_byname(pdev, "cp_active_irq");
+	pr_info("[MODEM_IF] <%s> PHONE_ACTIVE IRQ# = %d\n",
+		__func__, mc->irq_phone_active);
+
+	mdm6600_get_ops(mc);
+
+	ret = request_irq(mc->irq_phone_active, phone_active_irq_handler,
+			  IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			  "phone_active", mc);
+	if (ret) {
+		pr_err("[MODEM_IF] %s: failed to request_irq:%d\n",
+		       __func__, ret);
+		goto err_request_irq;
+	}
+
+	ret = enable_irq_wake(mc->irq_phone_active);
+	if (ret) {
+		pr_err("[MODEM_IF] %s: failed to enable_irq_wake:%d\n",
+		       __func__, ret);
+		goto err_set_wake_irq;
+	}
+
+	return ret;
+
+ err_set_wake_irq:
+	free_irq(mc->irq_phone_active, mc);
+ err_request_irq:
+	return ret;
+}
+#endif				/* CONFIG_MACH_U1_KOR_LGT */
+
+#if defined(CONFIG_MACH_C1CTC) || defined(CONFIG_MACH_M0_CTC)
+#include "modem_link_device_dpram.h"
+
+#define PIF_TIMEOUT		(180 * HZ)
+#define DPRAM_INIT_TIMEOUT	(30 * HZ)
+
+static int mdm6600_on(struct modem_ctl *mc)
+{
+	pr_info("[MSM] <%s>\n", __func__);
+
+	if (!mc->gpio_reset_req_n || !mc->gpio_cp_reset
+	    || !mc->gpio_cp_on || !mc->gpio_pda_active) {
+		pr_err("[MSM] no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_reset_req_n, 1);
+
+	gpio_set_value(mc->gpio_cp_reset, 1);
+	msleep(30);
+
+	gpio_set_value(mc->gpio_cp_on, 1);
+	msleep(500);
+
+	gpio_set_value(mc->gpio_cp_on, 0);
+	msleep(500);
+
+	gpio_set_value(mc->gpio_pda_active, 1);
+
+	mc->iod->modem_state_changed(mc->iod, STATE_BOOTING);
+
+	return 0;
+}
+
+static int mdm6600_off(struct modem_ctl *mc)
+{
+	pr_info("[MSM] <%s>\n", __func__);
+
+	if (!mc->gpio_cp_reset || !mc->gpio_cp_on) {
+		pr_err("[MSM] no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	gpio_set_value(mc->gpio_cp_on, 0);
+
+	mc->iod->modem_state_changed(mc->iod, STATE_OFFLINE);
+
+	return 0;
+}
+
+static int mdm6600_reset(struct modem_ctl *mc)
+{
+	int ret = 0;
+
+	pr_info("[MSM] <%s>\n", __func__);
+
+	ret = mdm6600_off(mc);
+	if (ret)
+		return -ENXIO;
+
+#if 0
+	msleep(100);
+#endif
+
+	ret = mdm6600_on(mc);
+	if (ret)
+		return -ENXIO;
+
+	return 0;
+}
+
+static int mdm6600_boot_on(struct modem_ctl *mc)
+{
+	pr_info("[MSM] <%s>\n", __func__);
+
+	if (!mc->gpio_flm_uart_sel) {
+		pr_err("[MSM] no gpio data\n");
+		return -ENXIO;
+	}
+
+	pr_info("[MSM] <%s> %s\n", __func__, "USB_BOOT_EN initializing");
+
+	if (system_rev < 11) {
+
+		gpio_direction_output(GPIO_USB_BOOT_EN, 0);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN, 0);
+
+		gpio_direction_output(GPIO_USB_BOOT_EN, 1);
+		gpio_set_value(GPIO_USB_BOOT_EN, 1);
+
+		pr_info("[MSM] <%s> USB_BOOT_EN:[%d]\n", __func__,
+			gpio_get_value(GPIO_USB_BOOT_EN));
+	} else if (system_rev == 11) {
+		gpio_direction_output(GPIO_USB_BOOT_EN, 0);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN, 0);
+
+		gpio_direction_output(GPIO_USB_BOOT_EN, 1);
+		gpio_set_value(GPIO_USB_BOOT_EN, 1);
+
+		pr_info("[MSM] <%s> USB_BOOT_EN:[%d]\n", __func__,
+			gpio_get_value(GPIO_USB_BOOT_EN));
+
+		gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 0);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN_REV06, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN_REV06, 0);
+
+		gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 1);
+		gpio_set_value(GPIO_USB_BOOT_EN_REV06, 1);
+
+		pr_info("[MSM] <%s> USB_BOOT_EN:[%d]\n", __func__,
+			gpio_get_value(GPIO_USB_BOOT_EN_REV06));
+
+	} else {	/* system_rev>11 */
+		gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 0);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN_REV06, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN_REV06, 0);
+
+		gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 1);
+		gpio_set_value(GPIO_USB_BOOT_EN_REV06, 1);
+
+		pr_info("[MSM] <%s> USB_BOOT_EN:[%d]\n", __func__,
+			gpio_get_value(GPIO_USB_BOOT_EN_REV06));
+
+	}
+
+	gpio_direction_output(mc->gpio_flm_uart_sel, 0);
+	s3c_gpio_setpull(mc->gpio_flm_uart_sel, S3C_GPIO_PULL_NONE);
+	gpio_set_value(mc->gpio_flm_uart_sel, 0);
+
+	gpio_direction_output(mc->gpio_flm_uart_sel, 1);
+	gpio_set_value(mc->gpio_flm_uart_sel, 1);
+
+	pr_info("[MSM] <%s> BOOT_SW_SEL : [%d]\n", __func__,
+		gpio_get_value(mc->gpio_flm_uart_sel));
+
+	mc->iod->modem_state_changed(mc->iod, STATE_BOOTING);
+
+	return 0;
+}
+
+static int mdm6600_boot_off(struct modem_ctl *mc)
+{
+	pr_info("[MSM] <%s>\n", __func__);
+
+	if (!mc->gpio_flm_uart_sel || !mc->gpio_flm_uart_sel_rev06) {
+		pr_err("[MSM] no gpio data\n");
+		return -ENXIO;
+	}
+
+	if (system_rev < 11) {
+		gpio_direction_output(GPIO_USB_BOOT_EN, 0);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN, 0);
+		gpio_direction_output(GPIO_BOOT_SW_SEL, 0);
+		s3c_gpio_setpull(GPIO_BOOT_SW_SEL, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_BOOT_SW_SEL, 0);
+
+	} else if (system_rev == 11) {
+		gpio_direction_output(GPIO_USB_BOOT_EN, 0);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN, 0);
+		gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 0);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN_REV06, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN_REV06, 0);
+
+		gpio_direction_output(GPIO_BOOT_SW_SEL, 0);
+		s3c_gpio_setpull(GPIO_BOOT_SW_SEL, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_BOOT_SW_SEL, 0);
+
+		gpio_direction_output(GPIO_BOOT_SW_SEL_REV06, 0);
+		s3c_gpio_setpull(GPIO_BOOT_SW_SEL_REV06, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_BOOT_SW_SEL_REV06, 0);
+
+	} else {	/* system_rev>11 */
+		gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 0);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN_REV06, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN_REV06, 0);
+
+		gpio_direction_output(GPIO_BOOT_SW_SEL_REV06, 0);
+		s3c_gpio_setpull(GPIO_BOOT_SW_SEL_REV06, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_BOOT_SW_SEL_REV06, 0);
+
+	}
+
+	if (max7693_muic_cp_usb_state()) {
+		msleep(30);
+		gpio_direction_output(GPIO_USB_BOOT_EN, 1);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN, 1);
+		gpio_direction_output(GPIO_USB_BOOT_EN_REV06, 1);
+		s3c_gpio_setpull(GPIO_USB_BOOT_EN_REV06, S3C_GPIO_PULL_NONE);
+		gpio_set_value(GPIO_USB_BOOT_EN_REV06, 1);
+	}
+
+	gpio_set_value(mc->gpio_flm_uart_sel, 0);
+
+	return 0;
+}
+
+static irqreturn_t phone_active_irq_handler(int irq, void *arg)
+{
+	struct modem_ctl *mc = (struct modem_ctl *)arg;
+	int phone_reset = 0;
+	int phone_active = 0;
+	int phone_state = 0;
+
+	if (!mc->gpio_cp_reset || !mc->gpio_phone_active) {
+		pr_err("[MSM] no gpio data\n");
+		return IRQ_HANDLED;
+	}
+
+	phone_reset = gpio_get_value(mc->gpio_cp_reset);
+	phone_active = gpio_get_value(mc->gpio_phone_active);
+	pr_info("[MSM] <%s> phone_reset = %d, phone_active = %d\n",
+		__func__, phone_reset, phone_active);
+
+	if (phone_reset && phone_active) {
+		phone_state = STATE_ONLINE;
+		if (mc->iod && mc->iod->modem_state_changed)
+			mc->iod->modem_state_changed(mc->iod, phone_state);
+	} else if (phone_reset && !phone_active) {
+		if (mc->phone_state == STATE_ONLINE) {
+			phone_state = STATE_CRASH_EXIT;
+			if (mc->iod && mc->iod->modem_state_changed)
+				mc->iod->modem_state_changed(mc->iod,
+							     phone_state);
+		}
+	} else {
+		phone_state = STATE_OFFLINE;
+		if (mc->iod && mc->iod->modem_state_changed)
+			mc->iod->modem_state_changed(mc->iod, phone_state);
+	}
+
+	if (phone_active)
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_LOW);
+	else
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_HIGH);
+
+	pr_info("[MSM] <%s> phone_state = %d\n", __func__, phone_state);
+
+	return IRQ_HANDLED;
+}
+
+static void mdm6600_get_ops(struct modem_ctl *mc)
+{
+	mc->ops.modem_on = mdm6600_on;
+	mc->ops.modem_off = mdm6600_off;
+	mc->ops.modem_reset = mdm6600_reset;
+	mc->ops.modem_boot_on = mdm6600_boot_on;
+	mc->ops.modem_boot_off = mdm6600_boot_off;
+}
+
+int mdm6600_init_modemctl_device(struct modem_ctl *mc, struct modem_data *pdata)
+{
+	int ret = 0;
+	struct platform_device *pdev;
+
+	mc->gpio_cp_on = pdata->gpio_cp_on;
+	mc->gpio_reset_req_n = pdata->gpio_reset_req_n;
+	mc->gpio_cp_reset = pdata->gpio_cp_reset;
+	mc->gpio_pda_active = pdata->gpio_pda_active;
+	mc->gpio_phone_active = pdata->gpio_phone_active;
+	mc->gpio_cp_dump_int = pdata->gpio_cp_dump_int;
+	mc->gpio_flm_uart_sel = pdata->gpio_flm_uart_sel;
+#if 1
+	mc->gpio_flm_uart_sel_rev06 = pdata->gpio_flm_uart_sel_rev06;
+#endif
+	mc->gpio_cp_warm_reset = pdata->gpio_cp_warm_reset;
+
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	gpio_set_value(mc->gpio_cp_on, 0);
+
+	pdev = to_platform_device(mc->dev);
+	mc->irq_phone_active = platform_get_irq_byname(pdev, "cp_active_irq");
+	pr_info("[MSM] <%s> PHONE_ACTIVE IRQ# = %d\n",
+		__func__, mc->irq_phone_active);
+
+	mdm6600_get_ops(mc);
+
+	ret = request_irq(mc->irq_phone_active,
+			  phone_active_irq_handler,
+			  IRQF_TRIGGER_HIGH, "msm_active", mc);
+	if (ret) {
+		pr_err("[MSM] <%s> failed to request_irq IRQ# %d (err=%d)\n",
+		       __func__, mc->irq_phone_active, ret);
+		return ret;
+	}
+
+	ret = enable_irq_wake(mc->irq_phone_active);
+	if (ret) {
+		pr_err("[MSM] %s: failed to enable_irq_wake IRQ# %d (err=%d)\n",
+		       __func__, mc->irq_phone_active, ret);
+		free_irq(mc->irq_phone_active, mc);
+		return ret;
+	}
+
+	return ret;
+}
+#endif
diff --git a/drivers/misc/modem_if/modem_modemctl_device_xmm6260.c b/drivers/misc/modem_if/modem_modemctl_device_xmm6260.c
new file mode 100644
index 0000000..c2d5067
--- /dev/null
+++ b/drivers/misc/modem_if/modem_modemctl_device_xmm6260.c
@@ -0,0 +1,303 @@
+/* /linux/drivers/misc/modem_if/modem_modemctl_device_xmm6260.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+
+static int xmm6260_on(struct modem_ctl *mc)
+{
+	mif_info("xmm6260_on()\n");
+
+	if (!mc->gpio_cp_reset || !mc->gpio_cp_on || !mc->gpio_reset_req_n) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+
+	if (mc->gpio_revers_bias_clear)
+		mc->gpio_revers_bias_clear();
+
+
+	gpio_set_value(mc->gpio_cp_on, 0);
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	udelay(160);
+	gpio_set_value(mc->gpio_pda_active, 0);
+	msleep(500); /* must be >500ms for CP can boot up under -20 degrees */
+	gpio_set_value(mc->gpio_cp_reset, 1);
+	udelay(160);
+	gpio_set_value(mc->gpio_reset_req_n, 1);
+	udelay(160);
+	gpio_set_value(mc->gpio_cp_on, 1);
+	udelay(60);
+	gpio_set_value(mc->gpio_cp_on, 0);
+	msleep(20);
+
+	if (mc->gpio_revers_bias_restore)
+		mc->gpio_revers_bias_restore();
+
+
+	gpio_set_value(mc->gpio_pda_active, 1);
+
+	mc->phone_state = STATE_BOOTING;
+
+	return 0;
+
+}
+
+static int xmm6260_off(struct modem_ctl *mc)
+{
+	mif_info("xmm6260_off()\n");
+
+	if (!mc->gpio_cp_reset || !mc->gpio_cp_on) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_cp_on, 0);
+	gpio_set_value(mc->gpio_cp_reset, 0);
+
+	mc->phone_state = STATE_OFFLINE;
+
+	return 0;
+}
+
+
+static int xmm6260_reset(struct modem_ctl *mc)
+{
+
+	mif_info("xmm6260_reset()\n");
+
+	if (!mc->gpio_cp_reset || !mc->gpio_reset_req_n)
+		return -ENXIO;
+
+	if (mc->gpio_revers_bias_clear)
+		mc->gpio_revers_bias_clear();
+
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	gpio_set_value(mc->gpio_reset_req_n, 0);
+
+	mc->phone_state = STATE_OFFLINE;
+
+	msleep(20);
+
+	gpio_set_value(mc->gpio_cp_reset, 1);
+/* TODO: check the reset timming with C2C connection */
+	udelay(160);
+
+	gpio_set_value(mc->gpio_reset_req_n, 1);
+	udelay(100);
+
+	gpio_set_value(mc->gpio_cp_on, 1);
+	udelay(60);
+	gpio_set_value(mc->gpio_cp_on, 0);
+	msleep(20);
+
+	if (mc->gpio_revers_bias_restore)
+		mc->gpio_revers_bias_restore();
+
+	mc->phone_state = STATE_BOOTING;
+
+	return 0;
+}
+
+static int xmm6260_boot_on(struct modem_ctl *mc)
+{
+	mif_info("xmm6260_boot_on()\n");
+
+	if (!mc->gpio_flm_uart_sel) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_flm_uart_sel, 0);
+
+	return 0;
+}
+
+static int xmm6260_boot_off(struct modem_ctl *mc)
+{
+	mif_info("xmm6260_boot_off()\n");
+
+	if (!mc->gpio_flm_uart_sel) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_flm_uart_sel, 1);
+
+	return 0;
+}
+
+static irqreturn_t phone_active_irq_handler(int irq, void *_mc)
+{
+	int phone_reset = 0;
+	int phone_active_value = 0;
+	int cp_dump_value = 0;
+	int phone_state = 0;
+	struct modem_ctl *mc = (struct modem_ctl *)_mc;
+	struct link_device *ld;
+
+	disable_irq_nosync(mc->irq_phone_active);
+
+	if (!mc->gpio_cp_reset || !mc->gpio_phone_active ||
+			!mc->gpio_cp_dump_int) {
+		mif_err("no gpio data\n");
+		return IRQ_HANDLED;
+	}
+
+	phone_reset = gpio_get_value(mc->gpio_cp_reset);
+	phone_active_value = gpio_get_value(mc->gpio_phone_active);
+	cp_dump_value = gpio_get_value(mc->gpio_cp_dump_int);
+
+	mif_info("PA EVENT : reset =%d, pa=%d, cp_dump=%d\n",
+				phone_reset, phone_active_value, cp_dump_value);
+
+	if (phone_reset && phone_active_value)
+		phone_state = STATE_BOOTING;
+	else if (phone_reset && !phone_active_value) {
+		if (mc->phone_state == STATE_BOOTING)
+			goto set_type;
+		if (cp_dump_value)
+			phone_state = STATE_CRASH_EXIT;
+		else
+			phone_state = STATE_CRASH_RESET;
+		if (mc->iod) {
+			ld = get_current_link(mc->iod);
+			if (ld->terminate_comm)
+				ld->terminate_comm(ld, mc->iod);
+		}
+	} else
+		phone_state = STATE_OFFLINE;
+
+	if (mc->iod && mc->iod->modem_state_changed)
+		mc->iod->modem_state_changed(mc->iod, phone_state);
+
+	if (mc->bootd && mc->bootd->modem_state_changed)
+		mc->bootd->modem_state_changed(mc->bootd, phone_state);
+
+set_type:
+	if (phone_active_value)
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_LOW);
+	else
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_HIGH);
+	enable_irq(mc->irq_phone_active);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t sim_detect_irq_handler(int irq, void *_mc)
+{
+	struct modem_ctl *mc = (struct modem_ctl *)_mc;
+
+	if (mc->iod && mc->iod->sim_state_changed)
+		mc->iod->sim_state_changed(mc->iod,
+				!gpio_get_value(mc->gpio_sim_detect));
+
+	return IRQ_HANDLED;
+}
+
+static void xmm6260_get_ops(struct modem_ctl *mc)
+{
+	mc->ops.modem_on = xmm6260_on;
+	mc->ops.modem_off = xmm6260_off;
+	mc->ops.modem_reset = xmm6260_reset;
+	mc->ops.modem_boot_on = xmm6260_boot_on;
+	mc->ops.modem_boot_off = xmm6260_boot_off;
+}
+
+int xmm6260_init_modemctl_device(struct modem_ctl *mc,
+			struct modem_data *pdata)
+{
+	int ret;
+	struct platform_device *pdev;
+
+	mc->gpio_cp_on = pdata->gpio_cp_on;
+	mc->gpio_reset_req_n = pdata->gpio_reset_req_n;
+	mc->gpio_cp_reset = pdata->gpio_cp_reset;
+	mc->gpio_pda_active = pdata->gpio_pda_active;
+	mc->gpio_phone_active = pdata->gpio_phone_active;
+	mc->gpio_cp_dump_int = pdata->gpio_cp_dump_int;
+	mc->gpio_flm_uart_sel = pdata->gpio_flm_uart_sel;
+	mc->gpio_cp_warm_reset = pdata->gpio_cp_warm_reset;
+	mc->gpio_revers_bias_clear = pdata->gpio_revers_bias_clear;
+	mc->gpio_revers_bias_restore = pdata->gpio_revers_bias_restore;
+	mc->gpio_sim_detect = pdata->gpio_sim_detect;
+
+	pdev = to_platform_device(mc->dev);
+	/* mc->irq_phone_active = platform_get_irq(pdev, 0); */
+	mc->irq_phone_active = gpio_to_irq(mc->gpio_phone_active);
+
+	if (mc->gpio_sim_detect)
+		mc->irq_sim_detect = gpio_to_irq(mc->gpio_sim_detect);
+
+	xmm6260_get_ops(mc);
+
+	/* initialize phone active */
+	ret = request_irq(mc->irq_phone_active, phone_active_irq_handler,
+				IRQF_NO_SUSPEND | IRQF_TRIGGER_HIGH,
+				"phone_active", mc);
+	if (ret) {
+		mif_err("failed to request_irq:%d\n", ret);
+		goto err_phone_active_request_irq;
+	}
+
+	ret = enable_irq_wake(mc->irq_phone_active);
+	if (ret) {
+		mif_err("failed to enable_irq_wake:%d\n", ret);
+		goto err_phone_active_set_wake_irq;
+	}
+
+	/* initialize sim_state if gpio_sim_detect exists */
+	mc->sim_state.online = false;
+	mc->sim_state.changed = false;
+	if (mc->gpio_sim_detect) {
+		ret = request_irq(mc->irq_sim_detect, sim_detect_irq_handler,
+				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				"sim_detect", mc);
+		if (ret) {
+			mif_err("failed to request_irq: %d\n", ret);
+			goto err_sim_detect_request_irq;
+		}
+
+		ret = enable_irq_wake(mc->irq_sim_detect);
+		if (ret) {
+			mif_err("failed to enable_irq_wake: %d\n", ret);
+			goto err_sim_detect_set_wake_irq;
+		}
+
+		/* initialize sim_state => insert: gpio=0, remove: gpio=1 */
+		mc->sim_state.online = !gpio_get_value(mc->gpio_sim_detect);
+	}
+
+	return ret;
+
+err_sim_detect_set_wake_irq:
+	free_irq(mc->irq_sim_detect, mc);
+err_sim_detect_request_irq:
+	mc->sim_state.online = false;
+	mc->sim_state.changed = false;
+err_phone_active_set_wake_irq:
+	free_irq(mc->irq_phone_active, mc);
+err_phone_active_request_irq:
+	return ret;
+}
diff --git a/drivers/misc/modem_if/modem_modemctl_device_xmm6262.c b/drivers/misc/modem_if/modem_modemctl_device_xmm6262.c
new file mode 100644
index 0000000..10b0811
--- /dev/null
+++ b/drivers/misc/modem_if/modem_modemctl_device_xmm6262.c
@@ -0,0 +1,258 @@
+/* /linux/drivers/misc/modem_if/modem_modemctl_device_xmm6262.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#define DEBUG
+
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/cma.h>
+#include <plat/devs.h>
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+
+static int xmm6262_on(struct modem_ctl *mc)
+{
+	mif_info("\n");
+
+	if (!mc->gpio_cp_reset || !mc->gpio_cp_on || !mc->gpio_reset_req_n) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+
+	if (mc->gpio_revers_bias_clear)
+		mc->gpio_revers_bias_clear();
+
+	/* TODO */
+	gpio_set_value(mc->gpio_reset_req_n, 0);
+	gpio_set_value(mc->gpio_cp_on, 0);
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	msleep(100);
+	gpio_set_value(mc->gpio_cp_reset, 1);
+	/* If XMM6262 was connected with C2C, AP wait 50ms to BB Reset*/
+	msleep(50);
+	gpio_set_value(mc->gpio_reset_req_n, 1);
+
+	gpio_set_value(mc->gpio_cp_on, 1);
+	udelay(60);
+	gpio_set_value(mc->gpio_cp_on, 0);
+	msleep(20);
+	if (mc->gpio_revers_bias_restore)
+		mc->gpio_revers_bias_restore();
+	gpio_set_value(mc->gpio_pda_active, 1);
+
+	mc->phone_state = STATE_BOOTING;
+
+	return 0;
+}
+
+static int xmm6262_off(struct modem_ctl *mc)
+{
+	mif_info("\n");
+
+	if (!mc->gpio_cp_reset || !mc->gpio_cp_on) {
+		mif_err("no gpio data\n");
+		return -ENXIO;
+	}
+
+	gpio_set_value(mc->gpio_cp_on, 0);
+	gpio_set_value(mc->gpio_cp_reset, 0);
+
+	return 0;
+}
+
+static int xmm6262_reset(struct modem_ctl *mc)
+{
+	mif_info("\n");
+
+	if (!mc->gpio_cp_reset || !mc->gpio_reset_req_n)
+		return -ENXIO;
+
+	if (mc->gpio_revers_bias_clear)
+		mc->gpio_revers_bias_clear();
+
+	gpio_set_value(mc->gpio_cp_reset, 0);
+	gpio_set_value(mc->gpio_reset_req_n, 0);
+
+	mc->phone_state = STATE_OFFLINE;
+
+	msleep(20);
+
+	gpio_set_value(mc->gpio_cp_reset, 1);
+	/* TODO: check the reset timming with C2C connection */
+	udelay(160);
+
+	gpio_set_value(mc->gpio_reset_req_n, 1);
+	udelay(100);
+
+	gpio_set_value(mc->gpio_cp_on, 1);
+	udelay(60);
+	gpio_set_value(mc->gpio_cp_on, 0);
+	msleep(20);
+
+	if (mc->gpio_revers_bias_restore)
+		mc->gpio_revers_bias_restore();
+
+	mc->phone_state = STATE_BOOTING;
+
+	return 0;
+}
+
+static irqreturn_t phone_active_irq_handler(int irq, void *_mc)
+{
+	int phone_reset = 0;
+	int phone_active_value = 0;
+	int cp_dump_value = 0;
+	int phone_state = 0;
+	struct modem_ctl *mc = (struct modem_ctl *)_mc;
+
+	disable_irq_nosync(mc->irq_phone_active);
+
+	if (!mc->gpio_cp_reset || !mc->gpio_phone_active ||
+			!mc->gpio_cp_dump_int) {
+		mif_err("no gpio data\n");
+		return IRQ_HANDLED;
+	}
+
+	phone_reset = gpio_get_value(mc->gpio_cp_reset);
+	phone_active_value = gpio_get_value(mc->gpio_phone_active);
+	cp_dump_value = gpio_get_value(mc->gpio_cp_dump_int);
+
+	mif_info("PA EVENT : reset =%d, pa=%d, cp_dump=%d\n",
+				phone_reset, phone_active_value, cp_dump_value);
+
+	if (phone_reset && phone_active_value)
+		phone_state = STATE_BOOTING;
+	else if (phone_reset && !phone_active_value) {
+		if (cp_dump_value)
+			phone_state = STATE_CRASH_EXIT;
+		else
+			phone_state = STATE_CRASH_RESET;
+	} else
+		phone_state = STATE_OFFLINE;
+
+	if (mc->iod && mc->iod->modem_state_changed)
+		mc->iod->modem_state_changed(mc->iod, phone_state);
+
+	if (mc->bootd && mc->bootd->modem_state_changed)
+		mc->bootd->modem_state_changed(mc->bootd, phone_state);
+
+	if (phone_active_value)
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_LOW);
+	else
+		irq_set_irq_type(mc->irq_phone_active, IRQ_TYPE_LEVEL_HIGH);
+	enable_irq(mc->irq_phone_active);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t sim_detect_irq_handler(int irq, void *_mc)
+{
+	struct modem_ctl *mc = (struct modem_ctl *)_mc;
+
+	if (mc->iod && mc->iod->sim_state_changed)
+		mc->iod->sim_state_changed(mc->iod,
+				!gpio_get_value(mc->gpio_sim_detect));
+
+	return IRQ_HANDLED;
+}
+
+static void xmm6262_get_ops(struct modem_ctl *mc)
+{
+	mc->ops.modem_on = xmm6262_on;
+	mc->ops.modem_off = xmm6262_off;
+	mc->ops.modem_reset = xmm6262_reset;
+}
+
+int xmm6262_init_modemctl_device(struct modem_ctl *mc,
+			struct modem_data *pdata)
+{
+	int ret = 0;
+	struct platform_device *pdev;
+
+	mc->gpio_reset_req_n = pdata->gpio_reset_req_n;
+	mc->gpio_cp_on = pdata->gpio_cp_on;
+	mc->gpio_cp_reset = pdata->gpio_cp_reset;
+	mc->gpio_pda_active = pdata->gpio_pda_active;
+	mc->gpio_phone_active = pdata->gpio_phone_active;
+	mc->gpio_cp_dump_int = pdata->gpio_cp_dump_int;
+	mc->gpio_ap_dump_int = pdata->gpio_ap_dump_int;
+	mc->gpio_flm_uart_sel = pdata->gpio_flm_uart_sel;
+	mc->gpio_cp_warm_reset = pdata->gpio_cp_warm_reset;
+	mc->gpio_sim_detect = pdata->gpio_sim_detect;
+
+	mc->gpio_revers_bias_clear = pdata->gpio_revers_bias_clear;
+	mc->gpio_revers_bias_restore = pdata->gpio_revers_bias_restore;
+
+	pdev = to_platform_device(mc->dev);
+	mc->irq_phone_active = gpio_to_irq(mc->gpio_phone_active);
+
+	if (mc->gpio_sim_detect)
+		mc->irq_sim_detect = gpio_to_irq(mc->gpio_sim_detect);
+
+	xmm6262_get_ops(mc);
+
+	ret = request_irq(mc->irq_phone_active, phone_active_irq_handler,
+				IRQF_NO_SUSPEND | IRQF_TRIGGER_HIGH,
+				"phone_active", mc);
+	if (ret) {
+		mif_err("failed to request_irq:%d\n", ret);
+		goto err_phone_active_request_irq;
+	}
+
+	ret = enable_irq_wake(mc->irq_phone_active);
+	if (ret) {
+		mif_err("failed to enable_irq_wake:%d\n", ret);
+		goto err_phone_active_set_wake_irq;
+	}
+
+	/* initialize sim_state if gpio_sim_detect exists */
+	mc->sim_state.online = false;
+	mc->sim_state.changed = false;
+	if (mc->gpio_sim_detect) {
+		ret = request_irq(mc->irq_sim_detect, sim_detect_irq_handler,
+				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				"sim_detect", mc);
+		if (ret) {
+			mif_err("failed to request_irq: %d\n", ret);
+			goto err_sim_detect_request_irq;
+		}
+
+		ret = enable_irq_wake(mc->irq_sim_detect);
+		if (ret) {
+			mif_err("failed to enable_irq_wake: %d\n", ret);
+			goto err_sim_detect_set_wake_irq;
+		}
+
+		/* initialize sim_state => insert: gpio=0, remove: gpio=1 */
+		mc->sim_state.online = !gpio_get_value(mc->gpio_sim_detect);
+	}
+
+	return ret;
+
+err_sim_detect_set_wake_irq:
+	free_irq(mc->irq_sim_detect, mc);
+err_sim_detect_request_irq:
+	mc->sim_state.online = false;
+	mc->sim_state.changed = false;
+err_phone_active_set_wake_irq:
+	free_irq(mc->irq_phone_active, mc);
+err_phone_active_request_irq:
+	return ret;
+}
diff --git a/drivers/misc/modem_if/modem_net_flowcontrol_device.c b/drivers/misc/modem_if/modem_net_flowcontrol_device.c
new file mode 100644
index 0000000..b3f055d
--- /dev/null
+++ b/drivers/misc/modem_if/modem_net_flowcontrol_device.c
@@ -0,0 +1,116 @@
+/* /linux/drivers/misc/modem_if/modem_net_flowcontrol_device.c
+ *
+ * Copyright (C) 2011 Google, Inc.
+ * Copyright (C) 2011 Samsung Electronics.
+ *
+ * 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/fs.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/sched.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/platform_data/modem.h>
+
+#include "modem_prj.h"
+
+
+#define NET_FLOWCONTROL_DEV_NAME_LEN 8
+
+static int modem_net_flowcontrol_device_open(
+			struct inode *inode, struct file *filp)
+{
+	return 0;
+}
+
+static int modem_net_flowcontrol_device_release(
+			struct inode *inode, struct file *filp)
+{
+	return 0;
+}
+
+static long modem_net_flowcontrol_device_ioctl(
+			struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	struct net *this_net;
+	struct net_device *ndev;
+	char dev_name[NET_FLOWCONTROL_DEV_NAME_LEN];
+	u8 chan;
+
+	if (copy_from_user(&chan, (void __user *)arg, sizeof(char)))
+		return -EFAULT;
+
+	if (chan > 15)
+		return -ENODEV;
+
+	snprintf(dev_name, NET_FLOWCONTROL_DEV_NAME_LEN, "rmnet%d", (int)chan);
+	this_net = get_net_ns_by_pid(current->pid);
+	ndev = __dev_get_by_name(this_net, dev_name);
+	if (ndev == NULL) {
+		mif_err("device = %s not exist\n", dev_name);
+		return -ENODEV;
+	}
+
+	switch (cmd) {
+	case IOCTL_MODEM_NET_SUSPEND:
+		netif_stop_queue(ndev);
+		mif_info("NET SUSPEND(%s)\n", dev_name);
+		break;
+	case IOCTL_MODEM_NET_RESUME:
+		netif_wake_queue(ndev);
+		mif_info("NET RESUME(%s)\n", dev_name);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static const struct file_operations modem_net_flowcontrol_device_fops = {
+	.owner = THIS_MODULE,
+	.open = modem_net_flowcontrol_device_open,
+	.release = modem_net_flowcontrol_device_release,
+	.unlocked_ioctl = modem_net_flowcontrol_device_ioctl,
+};
+
+static int __init modem_net_flowcontrol_device_init(void)
+{
+	int ret = 0;
+	struct io_device *net_flowcontrol_dev;
+
+	net_flowcontrol_dev = kzalloc(sizeof(struct io_device), GFP_KERNEL);
+	if (!net_flowcontrol_dev) {
+		mif_err("net_flowcontrol_dev io device memory alloc fail\n");
+		return -ENOMEM;
+	}
+
+	net_flowcontrol_dev->miscdev.minor = MISC_DYNAMIC_MINOR;
+	net_flowcontrol_dev->miscdev.name = "modem_br";
+	net_flowcontrol_dev->miscdev.fops = &modem_net_flowcontrol_device_fops;
+
+	ret = misc_register(&net_flowcontrol_dev->miscdev);
+	if (ret) {
+		mif_err("failed to register misc br device : %s\n",
+			net_flowcontrol_dev->miscdev.name);
+		kfree(net_flowcontrol_dev);
+	}
+
+	return ret;
+}
+
+module_init(modem_net_flowcontrol_device_init);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Samsung Modem IF Net Flowcontrol Driver");
diff --git a/drivers/misc/modem_if/modem_prj.h b/drivers/misc/modem_if/modem_prj.h
new file mode 100644
index 0000000..464370f
--- /dev/null
+++ b/drivers/misc/modem_if/modem_prj.h
@@ -0,0 +1,634 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#ifndef __MODEM_PRJ_H__
+#define __MODEM_PRJ_H__
+
+#include <linux/wait.h>
+#include <linux/miscdevice.h>
+#include <linux/skbuff.h>
+#include <linux/completion.h>
+#include <linux/wakelock.h>
+#include <linux/rbtree.h>
+#include <linux/spinlock.h>
+
+#define MAX_CPINFO_SIZE		512
+
+#define MAX_LINK_DEVTYPE	3
+
+#define MAX_FMT_DEVS	10
+#define MAX_RAW_DEVS	32
+#define MAX_RFS_DEVS	10
+#define MAX_NUM_IO_DEV	(MAX_FMT_DEVS + MAX_RAW_DEVS + MAX_RFS_DEVS)
+
+#define IOCTL_MODEM_ON			_IO('o', 0x19)
+#define IOCTL_MODEM_OFF			_IO('o', 0x20)
+#define IOCTL_MODEM_RESET		_IO('o', 0x21)
+#define IOCTL_MODEM_BOOT_ON		_IO('o', 0x22)
+#define IOCTL_MODEM_BOOT_OFF		_IO('o', 0x23)
+#define IOCTL_MODEM_START		_IO('o', 0x24)
+
+#define IOCTL_MODEM_PROTOCOL_SUSPEND	_IO('o', 0x25)
+#define IOCTL_MODEM_PROTOCOL_RESUME	_IO('o', 0x26)
+
+#define IOCTL_MODEM_STATUS		_IO('o', 0x27)
+#define IOCTL_MODEM_DL_START		_IO('o', 0x28)
+#define IOCTL_MODEM_FW_UPDATE		_IO('o', 0x29)
+
+#define IOCTL_MODEM_NET_SUSPEND		_IO('o', 0x30)
+#define IOCTL_MODEM_NET_RESUME		_IO('o', 0x31)
+
+#define IOCTL_MODEM_DUMP_START		_IO('o', 0x32)
+#define IOCTL_MODEM_DUMP_UPDATE		_IO('o', 0x33)
+#define IOCTL_MODEM_FORCE_CRASH_EXIT	_IO('o', 0x34)
+#define IOCTL_MODEM_CP_UPLOAD		_IO('o', 0x35)
+#define IOCTL_MODEM_DUMP_RESET		_IO('o', 0x36)
+
+#define IOCTL_DPRAM_SEND_BOOT		_IO('o', 0x40)
+#define IOCTL_DPRAM_INIT_STATUS		_IO('o', 0x43)
+
+/* ioctl command definitions. */
+#define IOCTL_DPRAM_PHONE_POWON		_IO('o', 0xd0)
+#define IOCTL_DPRAM_PHONEIMG_LOAD	_IO('o', 0xd1)
+#define IOCTL_DPRAM_NVDATA_LOAD		_IO('o', 0xd2)
+#define IOCTL_DPRAM_PHONE_BOOTSTART	_IO('o', 0xd3)
+
+#define IOCTL_DPRAM_PHONE_UPLOAD_STEP1	_IO('o', 0xde)
+#define IOCTL_DPRAM_PHONE_UPLOAD_STEP2	_IO('o', 0xdf)
+
+/* modem status */
+#define MODEM_OFF		0
+#define MODEM_CRASHED		1
+#define MODEM_RAMDUMP		2
+#define MODEM_POWER_ON		3
+#define MODEM_BOOTING_NORMAL	4
+#define MODEM_BOOTING_RAMDUMP	5
+#define MODEM_DUMPING		6
+#define MODEM_RUNNING		7
+
+#define HDLC_HEADER_MAX_SIZE	6 /* fmt 3, raw 6, rfs 6 */
+
+#define PSD_DATA_CHID_BEGIN	0x2A
+#define PSD_DATA_CHID_END	0x38
+
+#define PS_DATA_CH_0	10
+#define PS_DATA_CH_LAST	24
+
+#define IP6VERSION		6
+
+#define SOURCE_MAC_ADDR		{0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC}
+
+/* Debugging features */
+#define MAX_MIF_LOG_PATH_LEN	128
+#define MAX_MIF_LOG_FILE_SIZE	0x800000	/* 8 MB */
+
+#define MAX_MIF_EVT_BUFF_SIZE	256
+#define MAX_MIF_TIME_LEN	32
+#define MAX_MIF_NAME_LEN	16
+#define MAX_MIF_STR_LEN		127
+#define MAX_MIF_LOG_LEN		128
+
+enum mif_event_id {
+	MIF_IRQ_EVT = 0,
+	MIF_LNK_RX_EVT,
+	MIF_MUX_RX_EVT,
+	MIF_IOD_RX_EVT,
+	MIF_IOD_TX_EVT,
+	MIF_MUX_TX_EVT,
+	MIF_LNK_TX_EVT,
+	MAX_MIF_EVT
+};
+
+struct dpram_queue_status {
+	unsigned in;
+	unsigned out;
+};
+
+struct dpram_queue_status_pair {
+	struct dpram_queue_status txq;
+	struct dpram_queue_status rxq;
+};
+
+struct dpram_irq_buff {
+	unsigned magic;
+	unsigned access;
+	struct dpram_queue_status_pair qsp[MAX_IPC_DEV];
+	unsigned int2ap;
+	unsigned int2cp;
+};
+
+struct mif_event_buff {
+	char time[MAX_MIF_TIME_LEN];
+
+	struct timeval tv;
+	enum mif_event_id evt;
+
+	char mc[MAX_MIF_NAME_LEN];
+
+	char iod[MAX_MIF_NAME_LEN];
+
+	char ld[MAX_MIF_NAME_LEN];
+	enum modem_link link_type;
+
+	unsigned rcvd;
+	unsigned len;
+	union {
+		u8 data[MAX_MIF_LOG_LEN];
+		struct dpram_irq_buff dpram_irqb;
+	};
+};
+
+#define MIF_LOG_DIR	"/sdcard"
+#define MIF_LOG_LV_FILE	"/data/.mif_log_level"
+
+/* Does modem ctl structure will use state ? or status defined below ?*/
+enum modem_state {
+	STATE_OFFLINE,
+	STATE_CRASH_RESET, /* silent reset */
+	STATE_CRASH_EXIT, /* cp ramdump */
+	STATE_BOOTING,
+	STATE_ONLINE,
+	STATE_NV_REBUILDING, /* <= rebuilding start */
+	STATE_LOADER_DONE,
+	STATE_SIM_ATTACH,
+	STATE_SIM_DETACH,
+};
+
+enum com_state {
+	COM_NONE,
+	COM_ONLINE,
+	COM_HANDSHAKE,
+	COM_BOOT,
+	COM_CRASH,
+};
+
+enum link_mode {
+	LINK_MODE_INVALID = 0,
+	LINK_MODE_IPC,
+	LINK_MODE_BOOT,
+	LINK_MODE_DLOAD,
+	LINK_MODE_ULOAD,
+};
+
+struct sim_state {
+	bool online;	/* SIM is online? */
+	bool changed;	/* online is changed? */
+};
+
+#define HDLC_START		0x7F
+#define HDLC_END		0x7E
+#define SIZE_OF_HDLC_START	1
+#define SIZE_OF_HDLC_END	1
+#define MAX_LINK_PADDING_SIZE	3
+
+struct header_data {
+	char hdr[HDLC_HEADER_MAX_SIZE];
+	unsigned len;
+	unsigned frag_len;
+	char start; /*hdlc start header 0x7F*/
+};
+
+struct fmt_hdr {
+	u16 len;
+	u8 control;
+} __packed;
+
+struct raw_hdr {
+	u32 len;
+	u8 channel;
+	u8 control;
+} __packed;
+
+struct rfs_hdr {
+	u32 len;
+	u8 cmd;
+	u8 id;
+} __packed;
+
+struct sipc_fmt_hdr {
+	u16 len;
+	u8  msg_seq;
+	u8  ack_seq;
+	u8  main_cmd;
+	u8  sub_cmd;
+	u8  cmd_type;
+} __packed;
+
+#define SIPC5_START_MASK	0b11111000
+#define SIPC5_CONFIG_MASK	0b00000111
+#define SIPC5_EXT_FIELD_MASK	0b00000011
+
+#define SIPC5_PADDING_EXIST	0b00000100
+#define SIPC5_EXT_FIELD_EXIST	0b00000010
+#define SIPC5_CTL_FIELD_EXIST	0b00000001
+
+#define SIPC5_MAX_HEADER_SIZE	6
+#define SIPC5_HEADER_SIZE_WITH_EXT_LEN	6
+#define SIPC5_HEADER_SIZE_WITH_CTL_FLD	5
+#define SIPC5_MIN_HEADER_SIZE	4
+#define SIPC5_CONFIG_SIZE	1
+#define SIPC5_CH_ID_SIZE	1
+
+#define SIPC5_CONFIG_OFFSET	0
+#define SIPC5_CH_ID_OFFSET	1
+#define SIPC5_LEN_OFFSET	2
+#define SIPC5_CTL_OFFSET	4
+
+#define SIPC5_CH_ID_RAW_0	0
+#define SIPC5_CH_ID_FMT_0	235
+#define SIPC5_CH_ID_RFS_0	245
+#define SIPC5_CH_ID_MAX		255
+
+/* If iod->id is 0, do not need to store to `iodevs_tree_fmt' in SIPC4 */
+#define sipc4_is_not_reserved_channel(ch) ((ch) != 0)
+
+/* Channel 0, 5, 6, 27, 255 are reserved in SIPC5.
+ * see SIPC5 spec: 2.2.2 Channel Identification (Ch ID) Field.
+ * They do not need to store in `iodevs_tree_fmt'
+ */
+#define sipc5_is_not_reserved_channel(ch) \
+	((ch) != 0 && (ch) != 5 && (ch) != 6 && (ch) != 27 && (ch) != 255)
+
+struct sipc5_link_hdr {
+	u8 cfg;
+	u8 ch;
+	u16 len;
+	u8 ctl;
+} __packed;
+
+struct sipc5_frame_data {
+	/* Config octet */
+	u8 config;
+
+	/* Channel ID */
+	u8 ch_id;
+
+	/* Control for multiple FMT frame */
+	u8 control;
+
+	/* Frame configuration set by header analysis */
+	bool padding;
+	bool ext_fld;
+	bool ctl_fld;
+	bool ext_len;
+
+	/* Frame length calculated from the length fields */
+	unsigned len;
+
+	/* The length of link layer header */
+	unsigned hdr_len;
+
+	/* The length of received header */
+	unsigned hdr_rcvd;
+
+	/* The length of data payload */
+	unsigned data_len;
+
+	/* The length of received data */
+	unsigned data_rcvd;
+
+	/* Header buffer */
+	u8 hdr[SIPC5_MAX_HEADER_SIZE];
+};
+
+static inline unsigned sipc5_get_hdr_size(u8 cfg)
+{
+	if (cfg & SIPC5_EXT_FIELD_EXIST) {
+		if (cfg & SIPC5_CTL_FIELD_EXIST)
+			return SIPC5_HEADER_SIZE_WITH_CTL_FLD;
+		else
+			return SIPC5_HEADER_SIZE_WITH_EXT_LEN;
+	} else {
+		return SIPC5_MIN_HEADER_SIZE;
+	}
+}
+
+static inline unsigned sipc5_calc_padding_size(unsigned len)
+{
+	unsigned residue = len & 0x3;
+	return residue ? (4 - residue) : 0;
+}
+
+struct vnet {
+	struct io_device *iod;
+};
+
+/* for fragmented data from link devices */
+struct fragmented_data {
+	struct sk_buff *skb_recv;
+	struct header_data h_data;
+	struct sipc5_frame_data f_data;
+	/* page alloc fail retry*/
+	unsigned realloc_offset;
+};
+#define fragdata(iod, ld) (&(iod)->fragments[(ld)->link_type])
+
+/** struct skbuff_priv - private data of struct sk_buff
+ * this is matched to char cb[48] of struct sk_buff
+ */
+struct skbuff_private {
+	struct io_device *iod;
+	struct link_device *ld;
+	struct io_device *real_iod; /* for rx multipdp */
+};
+
+static inline struct skbuff_private *skbpriv(struct sk_buff *skb)
+{
+	BUILD_BUG_ON(sizeof(struct skbuff_private) > sizeof(skb->cb));
+	return (struct skbuff_private *)&skb->cb;
+}
+
+/** rx_alloc_skb - allocate an skbuff and set skb's iod, ld
+ * @length:	length to allocate
+ * @gfp_mask:	get_free_pages mask, passed to alloc_skb
+ * @iod:	struct io_device *
+ * @ld:		struct link_device *
+ *
+ * %NULL is returned if there is no free memory.
+ */
+static inline struct sk_buff *rx_alloc_skb(unsigned int length,
+		gfp_t gfp_mask, struct io_device *iod, struct link_device *ld)
+{
+	struct sk_buff *skb = alloc_skb(length, gfp_mask);
+	if (likely(skb)) {
+		skbpriv(skb)->iod = iod;
+		skbpriv(skb)->ld = ld;
+	}
+	return skb;
+}
+
+struct io_device {
+	/* rb_tree node for an io device */
+	struct rb_node node_chan;
+	struct rb_node node_fmt;
+
+	/* Name of the IO device */
+	char *name;
+
+	atomic_t opened;
+
+	/* Wait queue for the IO device */
+	wait_queue_head_t wq;
+
+	/* Misc and net device structures for the IO device */
+	struct miscdevice  miscdev;
+	struct net_device *ndev;
+
+	/* ID and Format for channel on the link */
+	unsigned id;
+	enum modem_link link_types;
+	enum dev_format format;
+	enum modem_io io_typ;
+	enum modem_network net_typ;
+
+	bool use_handover;	/* handover 2+ link devices */
+
+	/* SIPC version */
+	enum sipc_ver ipc_version;
+
+	/* Tx header buffer */
+	struct sipc5_frame_data meta_frame;
+
+	/* Rx queue of sk_buff */
+	struct sk_buff_head sk_rx_q;
+
+	/*
+	** work for each io device, when delayed work needed
+	** use this for private io device rx action
+	*/
+	struct delayed_work rx_work;
+
+	struct fragmented_data fragments[LINKDEV_MAX];
+
+	/* for multi-frame */
+	struct sk_buff *skb[128];
+
+	/* called from linkdevice when a packet arrives for this iodevice */
+	int (*recv)(struct io_device *iod, struct link_device *ld,
+					const char *data, unsigned int len);
+
+	/* inform the IO device that the modem is now online or offline or
+	 * crashing or whatever...
+	 */
+	void (*modem_state_changed)(struct io_device *iod, enum modem_state);
+
+	/* inform the IO device that the SIM is not inserting or removing */
+	void (*sim_state_changed)(struct io_device *iod, bool sim_online);
+
+	struct modem_ctl *mc;
+
+	struct wake_lock wakelock;
+	long waketime;
+
+	/* DO NOT use __current_link directly
+	 * you MUST use skbpriv(skb)->ld in mc, link, etc..
+	 */
+	struct link_device *__current_link;
+};
+#define to_io_device(misc) container_of(misc, struct io_device, miscdev)
+
+/* get_current_link, set_current_link don't need to use locks.
+ * In ARM, set_current_link and get_current_link are compiled to
+ * each one instruction (str, ldr) as atomic_set, atomic_read.
+ * And, the order of set_current_link and get_current_link is not important.
+ */
+#define get_current_link(iod) ((iod)->__current_link)
+#define set_current_link(iod, ld) ((iod)->__current_link = (ld))
+
+struct link_device {
+	struct list_head  list;
+	char *name;
+
+	enum modem_link link_type;
+	unsigned aligned;
+
+	/* SIPC version */
+	enum sipc_ver ipc_version;
+
+	/* Modem data */
+	struct modem_data *mdm_data;
+
+	/* Modem control */
+	struct modem_ctl *mc;
+
+	/* Operation mode of the link device */
+	enum link_mode mode;
+
+	struct io_device *fmt_iods[4];
+
+	/* TX queue of socket buffers */
+	struct sk_buff_head sk_fmt_tx_q;
+	struct sk_buff_head sk_raw_tx_q;
+	struct sk_buff_head sk_rfs_tx_q;
+
+	struct sk_buff_head *skb_txq[MAX_IPC_DEV];
+
+	bool raw_tx_suspended; /* for misc dev */
+	struct completion raw_tx_resumed_by_cp;
+
+	struct workqueue_struct *tx_wq;
+	struct work_struct tx_work;
+	struct delayed_work tx_delayed_work;
+	struct delayed_work tx_dwork;
+
+	struct workqueue_struct *rx_wq;
+	struct work_struct rx_work;
+	struct delayed_work rx_delayed_work;
+
+	enum com_state com_state;
+
+	/* init communication - setting link driver */
+	int (*init_comm)(struct link_device *ld, struct io_device *iod);
+
+	/* terminate communication */
+	void (*terminate_comm)(struct link_device *ld, struct io_device *iod);
+
+	/* called by an io_device when it has a packet to send over link
+	 * - the io device is passed so the link device can look at id and
+	 *   format fields to determine how to route/format the packet
+	 */
+	int (*send)(struct link_device *ld, struct io_device *iod,
+			struct sk_buff *skb);
+
+	int (*udl_start)(struct link_device *ld, struct io_device *iod);
+
+	int (*force_dump)(struct link_device *ld, struct io_device *iod);
+
+	int (*dump_start)(struct link_device *ld, struct io_device *iod);
+
+	int (*modem_update)(struct link_device *ld, struct io_device *iod,
+			unsigned long arg);
+
+	int (*dump_update)(struct link_device *ld, struct io_device *iod,
+			unsigned long arg);
+
+	int (*ioctl)(struct link_device *ld, struct io_device *iod,
+			unsigned cmd, unsigned long _arg);
+};
+
+struct modemctl_ops {
+	int (*modem_on) (struct modem_ctl *);
+	int (*modem_off) (struct modem_ctl *);
+	int (*modem_reset) (struct modem_ctl *);
+	int (*modem_boot_on) (struct modem_ctl *);
+	int (*modem_boot_off) (struct modem_ctl *);
+	int (*modem_force_crash_exit) (struct modem_ctl *);
+	int (*modem_dump_reset) (struct modem_ctl *);
+};
+
+/* mif_common - common data for all io devices and link devices and a modem ctl
+ * commons : mc : iod : ld = 1 : 1 : M : N
+ */
+struct mif_common {
+	/* list of link devices */
+	struct list_head link_dev_list;
+
+	/* rb_tree root of io devices. */
+	struct rb_root iodevs_tree_chan; /* group by channel */
+	struct rb_root iodevs_tree_fmt; /* group by dev_format */
+};
+
+struct modem_ctl {
+	struct device *dev;
+	char *name;
+	struct modem_data *mdm_data;
+
+	struct mif_common commons;
+
+	enum modem_state phone_state;
+	struct sim_state sim_state;
+
+	unsigned gpio_cp_on;
+	unsigned gpio_reset_req_n;
+	unsigned gpio_cp_reset;
+	unsigned gpio_pda_active;
+	unsigned gpio_phone_active;
+	unsigned gpio_cp_dump_int;
+	unsigned gpio_ap_dump_int;
+	unsigned gpio_flm_uart_sel;
+#if defined(CONFIG_MACH_M0_CTC)
+	unsigned gpio_flm_uart_sel_rev06;
+#endif
+	unsigned gpio_cp_warm_reset;
+	unsigned gpio_cp_off;
+	unsigned gpio_sim_detect;
+	unsigned gpio_dynamic_switching;
+
+	int irq_phone_active;
+	int irq_sim_detect;
+
+#ifdef CONFIG_LTE_MODEM_CMC221
+	const struct attribute_group *group;
+	unsigned gpio_slave_wakeup;
+	unsigned gpio_host_wakeup;
+	unsigned gpio_host_active;
+	int      irq_host_wakeup;
+
+	struct delayed_work dwork;
+#endif /*CONFIG_LTE_MODEM_CMC221*/
+
+	struct work_struct work;
+
+#if defined(CONFIG_MACH_U1_KOR_LGT)
+	unsigned gpio_cp_reset_msm;
+	unsigned gpio_boot_sw_sel;
+	void (*vbus_on)(void);
+	void (*vbus_off)(void);
+	bool usb_boot;
+#endif
+
+	struct modemctl_ops ops;
+	struct io_device *iod;
+	struct io_device *bootd;
+
+	void (*gpio_revers_bias_clear)(void);
+	void (*gpio_revers_bias_restore)(void);
+
+	/* TODO this will be move to struct mif_common */
+	/* For debugging log */
+	bool use_mif_log;
+	enum mif_event_id log_level;
+	atomic_t log_open;
+
+	struct workqueue_struct *evt_wq;
+	struct work_struct evt_work;
+	struct sk_buff_head evtq;
+
+	char log_path[MAX_MIF_LOG_PATH_LEN];
+	struct file *log_fp;
+
+	bool fs_ready;
+	bool fs_failed;
+
+	char *buff;
+};
+#define to_modem_ctl(mif_common) \
+		container_of(mif_common, struct modem_ctl, commons)
+
+int sipc4_init_io_device(struct io_device *iod);
+int sipc5_init_io_device(struct io_device *iod);
+
+int mif_init_log(struct modem_ctl *mc);
+void mif_set_log_level(struct modem_ctl *mc);
+int mif_open_log_file(struct modem_ctl *mc);
+void mif_close_log_file(struct modem_ctl *mc);
+
+void mif_irq_log(struct modem_ctl *mc, struct sk_buff *skb);
+void mif_ipc_log(struct modem_ctl *mc, enum mif_event_id evt,
+		struct io_device *iod, struct link_device *ld,
+		u8 *data, unsigned size);
+void mif_flush_logs(struct modem_ctl *mc);
+
+#endif
diff --git a/drivers/misc/modem_if/modem_utils.c b/drivers/misc/modem_if/modem_utils.c
new file mode 100644
index 0000000..24a9d19
--- /dev/null
+++ b/drivers/misc/modem_if/modem_utils.c
@@ -0,0 +1,552 @@
+/*
+ * Copyright (C) 2011 Samsung Electronics.
+ *
+ * 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/netdevice.h>
+#include <linux/platform_data/modem.h>
+#include <linux/platform_device.h>
+#include <linux/skbuff.h>
+#include <linux/ip.h>
+#include <linux/tcp.h>
+#include <linux/udp.h>
+#include <net/ip.h>
+
+#include "modem_prj.h"
+#include "modem_utils.h"
+
+#define CMD_SUSPEND	((unsigned short)(0x00CA))
+#define CMD_RESUME	((unsigned short)(0x00CB))
+
+
+/* dump2hex
+ * dump data to hex as fast as possible.
+ * the length of @buf must be greater than "@len * 3"
+ * it need 3 bytes per one data byte to print.
+ */
+static inline int dump2hex(char *buf, const char *data, size_t len)
+{
+	static const char *hex = "0123456789abcdef";
+	char *dest = buf;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		*dest++ = hex[(data[i] >> 4) & 0xf];
+		*dest++ = hex[data[i] & 0xf];
+		*dest++ = ' ';
+	}
+	if (likely(len > 0))
+		dest--; /* last space will be overwrited with null */
+
+	*dest = '\0';
+
+	return dest - buf;
+}
+
+/* print buffer as hex string */
+int pr_buffer(const char *tag, const char *data, size_t data_len,
+							size_t max_len)
+{
+	size_t len = min(data_len, max_len);
+	unsigned char hexstr[len ? len * 3 : 1]; /* 1 <= sizeof <= max_len*3 */
+	dump2hex(hexstr, data, len);
+
+	/* don't change this printk to mif_debug for print this as level7 */
+	return printk(KERN_DEBUG "%s(%u): %s%s\n", tag, data_len, hexstr,
+			len == data_len ? "" : " ...");
+}
+
+/* flow control CMfrom CP, it use in serial devices */
+int link_rx_flowctl_cmd(struct link_device *ld, const char *data, size_t len)
+{
+	struct mif_common *commons = &ld->mc->commons;
+	unsigned short *cmd, *end = (unsigned short *)(data + len);
+
+	mif_debug("flow control cmd: size=%d\n", len);
+
+	for (cmd = (unsigned short *)data; cmd < end; cmd++) {
+		switch (*cmd) {
+		case CMD_SUSPEND:
+			iodevs_for_each(commons, iodev_netif_stop, 0);
+			ld->raw_tx_suspended = true;
+			mif_info("flowctl CMD_SUSPEND(%04X)\n", *cmd);
+			break;
+
+		case CMD_RESUME:
+			iodevs_for_each(commons, iodev_netif_wake, 0);
+			ld->raw_tx_suspended = false;
+			complete_all(&ld->raw_tx_resumed_by_cp);
+			mif_info("flowctl CMD_RESUME(%04X)\n", *cmd);
+			break;
+
+		default:
+			mif_err("flowctl BACMD: %04X\n", *cmd);
+			break;
+		}
+	}
+
+	return 0;
+}
+
+struct io_device *get_iod_with_channel(struct mif_common *commons,
+					unsigned channel)
+{
+	struct rb_node *n = commons->iodevs_tree_chan.rb_node;
+	struct io_device *iodev;
+	while (n) {
+		iodev = rb_entry(n, struct io_device, node_chan);
+		if (channel < iodev->id)
+			n = n->rb_left;
+		else if (channel > iodev->id)
+			n = n->rb_right;
+		else
+			return iodev;
+	}
+	return NULL;
+}
+
+struct io_device *get_iod_with_format(struct mif_common *commons,
+			enum dev_format format)
+{
+	struct rb_node *n = commons->iodevs_tree_fmt.rb_node;
+	struct io_device *iodev;
+	while (n) {
+		iodev = rb_entry(n, struct io_device, node_fmt);
+		if (format < iodev->format)
+			n = n->rb_left;
+		else if (format > iodev->format)
+			n = n->rb_right;
+		else
+			return iodev;
+	}
+	return NULL;
+}
+
+struct io_device *insert_iod_with_channel(struct mif_common *commons,
+		unsigned channel, struct io_device *iod)
+{
+	struct rb_node **p = &commons->iodevs_tree_chan.rb_node;
+	struct rb_node *parent = NULL;
+	struct io_device *iodev;
+	while (*p) {
+		parent = *p;
+		iodev = rb_entry(parent, struct io_device, node_chan);
+		if (channel < iodev->id)
+			p = &(*p)->rb_left;
+		else if (channel > iodev->id)
+			p = &(*p)->rb_right;
+		else
+			return iodev;
+	}
+	rb_link_node(&iod->node_chan, parent, p);
+	rb_insert_color(&iod->node_chan, &commons->iodevs_tree_chan);
+	return NULL;
+}
+
+struct io_device *insert_iod_with_format(struct mif_common *commons,
+		enum dev_format format, struct io_device *iod)
+{
+	struct rb_node **p = &commons->iodevs_tree_fmt.rb_node;
+	struct rb_node *parent = NULL;
+	struct io_device *iodev;
+	while (*p) {
+		parent = *p;
+		iodev = rb_entry(parent, struct io_device, node_fmt);
+		if (format < iodev->format)
+			p = &(*p)->rb_left;
+		else if (format > iodev->format)
+			p = &(*p)->rb_right;
+		else
+			return iodev;
+	}
+	rb_link_node(&iod->node_fmt, parent, p);
+	rb_insert_color(&iod->node_fmt, &commons->iodevs_tree_fmt);
+	return NULL;
+}
+
+void iodevs_for_each(struct mif_common *commons, action_fn action, void *args)
+{
+	struct io_device *iod;
+	struct rb_node *node = rb_first(&commons->iodevs_tree_chan);
+	for (; node; node = rb_next(node)) {
+		iod = rb_entry(node, struct io_device, node_chan);
+		action(iod, args);
+	}
+}
+
+void iodev_netif_wake(struct io_device *iod, void *args)
+{
+	if (iod->io_typ == IODEV_NET && iod->ndev) {
+		netif_wake_queue(iod->ndev);
+		mif_info("%s\n", iod->name);
+	}
+}
+
+void iodev_netif_stop(struct io_device *iod, void *args)
+{
+	if (iod->io_typ == IODEV_NET && iod->ndev) {
+		netif_stop_queue(iod->ndev);
+		mif_info("%s\n", iod->name);
+	}
+}
+
+static void iodev_set_tx_link(struct io_device *iod, void *args)
+{
+	struct link_device *ld = (struct link_device *)args;
+	if (iod->io_typ == IODEV_NET && IS_CONNECTED(iod, ld)) {
+		set_current_link(iod, ld);
+		mif_err("%s -> %s\n", iod->name, ld->name);
+	}
+}
+
+void rawdevs_set_tx_link(struct mif_common *commons, enum modem_link link_type)
+{
+	struct link_device *ld = find_linkdev(commons, link_type);
+	if (ld)
+		iodevs_for_each(commons, iodev_set_tx_link, ld);
+}
+
+void mif_add_timer(struct timer_list *timer, unsigned long expire,
+		void (*function)(unsigned long), unsigned long data)
+{
+	init_timer(timer);
+	timer->expires = get_jiffies_64() + expire;
+	timer->function = function;
+	timer->data = data;
+	add_timer(timer);
+}
+
+void mif_print_data(char *buf, int len)
+{
+	int words = len >> 4;
+	int residue = len - (words << 4);
+	int i;
+	char *b;
+	char last[80];
+	char tb[8];
+
+	/* Make the last line, if ((len % 16) > 0) */
+	if (residue > 0) {
+		memset(last, 0, sizeof(last));
+		memset(tb, 0, sizeof(tb));
+		b = buf + (words << 4);
+
+		sprintf(last, "%04X: ", (words << 4));
+		for (i = 0; i < residue; i++) {
+			sprintf(tb, "%02x ", b[i]);
+			strcat(last, tb);
+			if ((i & 0x3) == 0x3) {
+				sprintf(tb, " ");
+				strcat(last, tb);
+			}
+		}
+	}
+
+	for (i = 0; i < words; i++) {
+		b = buf + (i << 4);
+		mif_err("%04X: "
+			"%02x %02x %02x %02x  %02x %02x %02x %02x  "
+			"%02x %02x %02x %02x  %02x %02x %02x %02x\n",
+			(i << 4),
+			b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7],
+			b[8], b[9], b[10], b[11], b[12], b[13], b[14], b[15]);
+	}
+
+	/* Print the last line */
+	if (residue > 0)
+		mif_err("%s\n", last);
+}
+
+void print_sipc4_hdlc_fmt_frame(const u8 *psrc)
+{
+	u8 *frm;			/* HDLC Frame	*/
+	struct fmt_hdr *hh;		/* HDLC Header	*/
+	struct sipc_fmt_hdr *fh;	/* IPC Header	*/
+	u16 hh_len = sizeof(struct fmt_hdr);
+	u16 fh_len = sizeof(struct sipc_fmt_hdr);
+	u8 *data;
+	int dlen;
+
+	/* Actual HDLC header starts from after START flag (0x7F) */
+	frm = (u8 *)(psrc + 1);
+
+	/* Point HDLC header and IPC header */
+	hh = (struct fmt_hdr *)(frm);
+	fh = (struct sipc_fmt_hdr *)(frm + hh_len);
+
+	/* Point IPC data */
+	data = frm + (hh_len + fh_len);
+	dlen = hh->len - (hh_len + fh_len);
+
+	mif_err("--------------------HDLC & FMT HEADER----------------------\n");
+
+	mif_err("HDLC: length %d, control 0x%02x\n", hh->len, hh->control);
+
+	mif_err("(M)0x%02X, (S)0x%02X, (T)0x%02X, mseq %d, aseq %d, len %d\n",
+		fh->main_cmd, fh->sub_cmd, fh->cmd_type,
+		fh->msg_seq, fh->ack_seq, fh->len);
+
+	mif_err("-----------------------IPC FMT DATA------------------------\n");
+
+	if (dlen > 0) {
+		if (dlen > 64)
+			dlen = 64;
+		mif_print_data(data, dlen);
+	}
+
+	mif_err("-----------------------------------------------------------\n");
+}
+
+void print_sipc4_fmt_frame(const u8 *psrc)
+{
+	struct sipc_fmt_hdr *fh = (struct sipc_fmt_hdr *)psrc;
+	u16 fh_len = sizeof(struct sipc_fmt_hdr);
+	u8 *data;
+	int dlen;
+
+	/* Point IPC data */
+	data = (u8 *)(psrc + fh_len);
+	dlen = fh->len - fh_len;
+
+	mif_err("----------------------IPC FMT HEADER-----------------------\n");
+
+	mif_err("(M)0x%02X, (S)0x%02X, (T)0x%02X, mseq:%d, aseq:%d, len:%d\n",
+		fh->main_cmd, fh->sub_cmd, fh->cmd_type,
+		fh->msg_seq, fh->ack_seq, fh->len);
+
+	mif_err("-----------------------IPC FMT DATA------------------------\n");
+
+	if (dlen > 0)
+		mif_print_data(data, dlen);
+
+	mif_err("-----------------------------------------------------------\n");
+}
+
+void print_sipc5_link_fmt_frame(const u8 *psrc)
+{
+	u8 *lf;				/* Link Frame	*/
+	struct sipc5_link_hdr *lh;	/* Link Header	*/
+	struct sipc_fmt_hdr *fh;	/* IPC Header	*/
+	u16 lh_len;
+	u16 fh_len;
+	u8 *data;
+	int dlen;
+
+	lf = (u8 *)psrc;
+
+	/* Point HDLC header and IPC header */
+	lh = (struct sipc5_link_hdr *)lf;
+	if (lh->cfg & SIPC5_CTL_FIELD_EXIST)
+		lh_len = SIPC5_HEADER_SIZE_WITH_CTL_FLD;
+	else
+		lh_len = SIPC5_MIN_HEADER_SIZE;
+	fh = (struct sipc_fmt_hdr *)(lf + lh_len);
+	fh_len = sizeof(struct sipc_fmt_hdr);
+
+	/* Point IPC data */
+	data = lf + (lh_len + fh_len);
+	dlen = lh->len - (lh_len + fh_len);
+
+	mif_err("--------------------LINK & FMT HEADER----------------------\n");
+
+	mif_err("LINK: cfg 0x%02X, ch %d, len %d\n", lh->cfg, lh->ch, lh->len);
+
+	mif_err("(M)0x%02X, (S)0x%02X, (T)0x%02X, mseq:%d, aseq:%d, len:%d\n",
+		fh->main_cmd, fh->sub_cmd, fh->cmd_type,
+		fh->msg_seq, fh->ack_seq, fh->len);
+
+	mif_err("-----------------------IPC FMT DATA------------------------\n");
+
+	if (dlen > 0) {
+		if (dlen > 64)
+			dlen = 64;
+		mif_print_data(data, dlen);
+	}
+
+	mif_err("-----------------------------------------------------------\n");
+}
+
+static void print_tcp_header(u8 *pkt)
+{
+	int i;
+	char tcp_flags[32];
+	struct tcphdr *tcph = (struct tcphdr *)pkt;
+	u8 *opt = pkt + TCP_HDR_SIZE;
+	unsigned opt_len = (tcph->doff << 2) - TCP_HDR_SIZE;
+
+/*-------------------------------------------------------------------------
+
+				TCP Header Format
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|          Source Port          |       Destination Port        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                        Sequence Number                        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                    Acknowledgment Number                      |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|  Data |       |C|E|U|A|P|R|S|F|                               |
+	| Offset| Rsvd  |W|C|R|C|S|S|Y|I|            Window             |
+	|       |       |R|E|G|K|H|T|N|N|                               |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|           Checksum            |         Urgent Pointer        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                    Options                    |    Padding    |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                             data                              |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+-------------------------------------------------------------------------*/
+
+	memset(tcp_flags, 0, sizeof(tcp_flags));
+	if (tcph->cwr)
+		strcat(tcp_flags, "CWR ");
+	if (tcph->ece)
+		strcat(tcp_flags, "EC");
+	if (tcph->urg)
+		strcat(tcp_flags, "URG ");
+	if (tcph->ack)
+		strcat(tcp_flags, "ACK ");
+	if (tcph->psh)
+		strcat(tcp_flags, "PSH ");
+	if (tcph->rst)
+		strcat(tcp_flags, "RST ");
+	if (tcph->syn)
+		strcat(tcp_flags, "SYN ");
+	if (tcph->fin)
+		strcat(tcp_flags, "FIN ");
+
+	mif_err("TCP:: Src.Port %u, Dst.Port %u\n",
+		ntohs(tcph->source), ntohs(tcph->dest));
+	mif_err("TCP:: SEQ 0x%08X(%u), ACK 0x%08X(%u)\n",
+		ntohs(tcph->seq), ntohs(tcph->seq),
+		ntohs(tcph->ack_seq), ntohs(tcph->ack_seq));
+	mif_err("TCP:: Flags {%s}\n", tcp_flags);
+	mif_err("TCP:: Window %u, Checksum 0x%04X, Urg Pointer %u\n",
+		ntohs(tcph->window), ntohs(tcph->check), ntohs(tcph->urg_ptr));
+
+	if (opt_len > 0) {
+		mif_err("TCP:: Options {");
+		for (i = 0; i < opt_len; i++)
+			mif_err("%02X ", opt[i]);
+		mif_err("}\n");
+	}
+}
+
+static void print_udp_header(u8 *pkt)
+{
+	struct udphdr *udph = (struct udphdr *)pkt;
+
+/*-------------------------------------------------------------------------
+
+				UDP Header Format
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|          Source Port          |       Destination Port        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|            Length             |           Checksum            |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                             data                              |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+-------------------------------------------------------------------------*/
+
+	mif_err("UDP:: Src.Port %u, Dst.Port %u\n",
+		ntohs(udph->source), ntohs(udph->dest));
+	mif_err("UDP:: Length %u, Checksum 0x%04X\n",
+		ntohs(udph->len), ntohs(udph->check));
+
+	if (ntohs(udph->dest) == 53)
+		mif_err("UDP:: DNS query!!!\n");
+
+	if (ntohs(udph->source) == 53)
+		mif_err("UDP:: DNS response!!!\n");
+}
+
+void print_ip4_packet(u8 *ip_pkt)
+{
+	char ip_flags[16];
+	struct iphdr *iph = (struct iphdr *)ip_pkt;
+	u8 *pkt = ip_pkt + (iph->ihl << 2);
+	u16 flags = (ntohs(iph->frag_off) & 0xE000);
+	u16 frag_off = (ntohs(iph->frag_off) & 0x1FFF);
+
+	mif_err("-----------------------------------------------------------\n");
+
+/*---------------------------------------------------------------------------
+
+				IPv4 Header Format
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|Version|  IHL  |Type of Service|          Total Length         |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|         Identification        |C|D|M|     Fragment Offset     |
+	|                               |E|F|F|                         |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|  Time to Live |    Protocol   |         Header Checksum       |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                       Source Address                          |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                    Destination Address                        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                    Options                    |    Padding    |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+	IHL - Header Length
+	Flags - Consist of 3 bits
+		The 1st bit is "Congestion" bit.
+		The 2nd bit is "Dont Fragment" bit.
+		The 3rd bit is "More Fragments" bit.
+
+---------------------------------------------------------------------------*/
+
+	memset(ip_flags, 0, sizeof(ip_flags));
+	if (flags & IP_CE)
+		strcat(ip_flags, "C");
+	if (flags & IP_DF)
+		strcat(ip_flags, "D");
+	if (flags & IP_MF)
+		strcat(ip_flags, "M");
+
+	mif_err("IP4:: Version %u, Header Length %u, TOS %u, Length %u\n",
+		iph->version, (iph->ihl << 2), iph->tos, ntohs(iph->tot_len));
+	mif_err("IP4:: I%u, Fragment Offset %u\n",
+		ntohs(iph->id), frag_off);
+	mif_err("IP4:: Flags {%s}\n", ip_flags);
+	mif_err("IP4:: TTL %u, Protocol %u, Header Checksum 0x%04X\n",
+		iph->ttl, iph->protocol, ntohs(iph->check));
+	mif_err("IP4:: Src.IP %u.%u.%u.%u, Dst.IP %u.%u.%u.%u\n",
+		ip_pkt[12], ip_pkt[13], ip_pkt[14], ip_pkt[15],
+		ip_pkt[16], ip_pkt[17], ip_pkt[18], ip_pkt[19]);
+
+	switch (iph->protocol) {
+	case 6:
+		/* TCP */
+		print_tcp_header(pkt);
+		break;
+
+	case 17:
+		/* UDP */
+		print_udp_header(pkt);
+		break;
+
+	default:
+		break;
+	}
+
+	mif_err("-----------------------------------------------------------\n");
+}
+
diff --git a/drivers/misc/modem_if/modem_utils.h b/drivers/misc/modem_if/modem_utils.h
new file mode 100644
index 0000000..60e4820
--- /dev/null
+++ b/drivers/misc/modem_if/modem_utils.h
@@ -0,0 +1,186 @@
+/*
+ * Copyright (C) 2011 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#ifndef __MODEM_UTILS_H__
+#define __MODEM_UTILS_H__
+
+#include <linux/rbtree.h>
+
+#define IS_CONNECTED(iod, ld) ((iod)->link_types & LINKTYPE((ld)->link_type))
+
+/** find_linkdev - find a link device
+ * @commons:	struct mif_common
+ */
+static inline struct link_device *find_linkdev(struct mif_common *commons,
+		enum modem_link link_type)
+{
+	struct link_device *ld;
+	list_for_each_entry(ld, &commons->link_dev_list, list) {
+		if (ld->link_type == link_type)
+			return ld;
+	}
+	return NULL;
+}
+
+/** countbits - count number of 1 bits as fastest way
+ * @n: number
+ */
+static inline unsigned int countbits(unsigned int n)
+{
+	unsigned int i;
+	for (i = 0; n != 0; i++)
+		n &= (n - 1);
+	return i;
+}
+
+/* print buffer as hex string */
+int pr_buffer(const char *tag, const char *data, size_t data_len,
+							size_t max_len);
+
+/* print a sk_buff as hex string */
+#define pr_skb(tag, skb) \
+	pr_buffer(tag, (char *)((skb)->data), (size_t)((skb)->len), (size_t)16)
+
+/* print a urb as hex string */
+#define pr_urb(tag, urb) \
+	pr_buffer(tag, (char *)((urb)->transfer_buffer), \
+			(size_t)((urb)->actual_length), (size_t)16)
+
+/* flow control CMD from CP, it use in serial devices */
+int link_rx_flowctl_cmd(struct link_device *ld, const char *data, size_t len);
+
+
+/* get iod from tree functions */
+
+struct io_device *get_iod_with_format(struct mif_common *commons,
+					enum dev_format format);
+struct io_device *get_iod_with_channel(struct mif_common *commons,
+					unsigned channel);
+
+static inline struct io_device *link_get_iod_with_format(
+			struct link_device *ld, enum dev_format format)
+{
+	struct io_device *iod = get_iod_with_format(&ld->mc->commons, format);
+	return (iod && IS_CONNECTED(iod, ld)) ? iod : NULL;
+}
+
+static inline struct io_device *link_get_iod_with_channel(
+			struct link_device *ld, unsigned channel)
+{
+	struct io_device *iod = get_iod_with_channel(&ld->mc->commons, channel);
+	return (iod && IS_CONNECTED(iod, ld)) ? iod : NULL;
+}
+
+/* insert iod to tree functions */
+struct io_device *insert_iod_with_format(struct mif_common *commons,
+			enum dev_format format, struct io_device *iod);
+struct io_device *insert_iod_with_channel(struct mif_common *commons,
+			unsigned channel, struct io_device *iod);
+
+/* iodev for each */
+typedef void (*action_fn)(struct io_device *iod, void *args);
+void iodevs_for_each(struct mif_common *commons, action_fn action, void *args);
+
+/* netif wake/stop queue of iod */
+void iodev_netif_wake(struct io_device *iod, void *args);
+void iodev_netif_stop(struct io_device *iod, void *args);
+
+/* change tx_link of raw devices */
+void rawdevs_set_tx_link(struct mif_common *commons, enum modem_link link_type);
+
+void mif_add_timer(struct timer_list *timer, unsigned long expire,
+		void (*function)(unsigned long), unsigned long data);
+
+/* debug helper functions for sipc4, sipc5 */
+void mif_print_data(char *buf, int len);
+void print_sipc4_hdlc_fmt_frame(const u8 *psrc);
+void print_sipc4_fmt_frame(const u8 *psrc);
+void print_sipc5_link_fmt_frame(const u8 *psrc);
+
+
+/*---------------------------------------------------------------------------
+
+				IPv4 Header Format
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|Version|  IHL  |Type of Service|          Total Length         |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|         Identification        |C|D|M|     Fragment Offset     |
+	|                               |E|F|F|                         |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|  Time to Live |    Protocol   |         Header Checksum       |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                       Source Address                          |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                    Destination Address                        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                    Options                    |    Padding    |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+	IHL - Header Length
+	Flags - Consist of 3 bits
+		The 1st bit is "Congestion" bit.
+		The 2nd bit is "Dont Fragment" bit.
+		The 3rd bit is "More Fragments" bit.
+
+---------------------------------------------------------------------------*/
+#define IPV4_HDR_SIZE	20
+
+/*-------------------------------------------------------------------------
+
+				TCP Header Format
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|          Source Port          |       Destination Port        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                        Sequence Number                        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                    Acknowledgment Number                      |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|  Data |       |C|E|U|A|P|R|S|F|                               |
+	| Offset| Rsvd  |W|C|R|C|S|S|Y|I|            Window             |
+	|       |       |R|E|G|K|H|T|N|N|                               |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|           Checksum            |         Urgent Pointer        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                    Options                    |    Padding    |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                             data                              |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+-------------------------------------------------------------------------*/
+#define TCP_HDR_SIZE	20
+
+/*-------------------------------------------------------------------------
+
+				UDP Header Format
+
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|          Source Port          |       Destination Port        |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|            Length             |           Checksum            |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	|                             data                              |
+	+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+-------------------------------------------------------------------------*/
+#define UDP_HDR_SIZE	8
+
+void print_ip4_packet(u8 *ip_pkt);
+
+#endif/*__MODEM_UTILS_H__*/
diff --git a/drivers/misc/modem_if/modem_variation.h b/drivers/misc/modem_if/modem_variation.h
new file mode 100644
index 0000000..596abd1
--- /dev/null
+++ b/drivers/misc/modem_if/modem_variation.h
@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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.
+ *
+ */
+
+#ifndef __MODEM_VARIATION_H__
+#define __MODEM_VARIATION_H__
+
+#include <linux/platform_data/modem.h>
+
+#define DECLARE_MODEM_INIT(type)	\
+	int type ## _init_modemctl_device(struct modem_ctl *mc,	\
+				struct modem_data *pdata)
+#define DECLARE_MODEM_INIT_DUMMY(type)	\
+	static DECLARE_MODEM_INIT(type) { return 0; }
+
+#define DECLARE_LINK_INIT(type)	\
+		struct link_device *type ## _create_link_device(	\
+		struct platform_device *pdev)
+#define DECLARE_LINK_INIT_DUMMY(type)	\
+	static DECLARE_LINK_INIT(type) { return NULL; }
+
+#define MODEM_INIT_CALL(type)	type ## _init_modemctl_device
+#define LINK_INIT_CALL(type)	type ## _create_link_device
+
+/* add declaration of modem & link type */
+/* modem device support */
+DECLARE_MODEM_INIT_DUMMY(dummy)
+
+#ifdef CONFIG_UMTS_MODEM_XMM6260
+DECLARE_MODEM_INIT(xmm6260);
+#else
+DECLARE_MODEM_INIT_DUMMY(xmm6260)
+#endif
+
+#ifdef CONFIG_UMTS_MODEM_XMM6262
+DECLARE_MODEM_INIT(xmm6262);
+#else
+DECLARE_MODEM_INIT_DUMMY(xmm6262)
+#endif
+
+#ifdef CONFIG_CDMA_MODEM_CBP71
+DECLARE_MODEM_INIT(cbp71);
+#else
+DECLARE_MODEM_INIT_DUMMY(cbp71)
+#endif
+
+#ifdef CONFIG_CDMA_MODEM_CBP72
+DECLARE_MODEM_INIT(cbp72);
+#else
+DECLARE_MODEM_INIT_DUMMY(cbp72)
+#endif
+
+#ifdef CONFIG_LTE_MODEM_CMC221
+DECLARE_MODEM_INIT(cmc221);
+#else
+DECLARE_MODEM_INIT_DUMMY(cmc221)
+#endif
+
+#ifdef CONFIG_CDMA_MODEM_MDM6600
+DECLARE_MODEM_INIT(mdm6600);
+#else
+DECLARE_MODEM_INIT_DUMMY(mdm6600)
+#endif
+
+/* link device support */
+DECLARE_LINK_INIT_DUMMY(undefined)
+
+#ifdef CONFIG_LINK_DEVICE_MIPI
+DECLARE_LINK_INIT(mipi);
+#else
+DECLARE_LINK_INIT_DUMMY(mipi)
+#endif
+
+#ifdef CONFIG_LINK_DEVICE_DPRAM
+DECLARE_LINK_INIT(dpram);
+#else
+DECLARE_LINK_INIT_DUMMY(dpram)
+#endif
+
+#ifdef CONFIG_LINK_DEVICE_SPI
+DECLARE_LINK_INIT(spi);
+#else
+DECLARE_LINK_INIT_DUMMY(spi)
+#endif
+
+#ifdef CONFIG_LINK_DEVICE_USB
+DECLARE_LINK_INIT(usb);
+#else
+DECLARE_LINK_INIT_DUMMY(usb)
+#endif
+
+#ifdef CONFIG_LINK_DEVICE_HSIC
+DECLARE_LINK_INIT(hsic);
+#else
+DECLARE_LINK_INIT_DUMMY(hsic)
+#endif
+
+#ifdef CONFIG_LINK_DEVICE_C2C
+DECLARE_LINK_INIT(c2c);
+#else
+DECLARE_LINK_INIT_DUMMY(c2c)
+#endif
+
+typedef int (*modem_init_call)(struct modem_ctl *, struct modem_data *);
+static modem_init_call modem_init_func[] = {
+	MODEM_INIT_CALL(xmm6260),
+	MODEM_INIT_CALL(xmm6262),
+	MODEM_INIT_CALL(cbp71),
+	MODEM_INIT_CALL(cbp72),
+	MODEM_INIT_CALL(cmc221),
+	MODEM_INIT_CALL(mdm6600),
+	MODEM_INIT_CALL(dummy),
+};
+
+typedef struct link_device *(*link_init_call)(struct platform_device *);
+static link_init_call link_init_func[] = {
+	LINK_INIT_CALL(undefined),
+	LINK_INIT_CALL(mipi),
+	LINK_INIT_CALL(dpram),
+	LINK_INIT_CALL(spi),
+	LINK_INIT_CALL(usb),
+	LINK_INIT_CALL(hsic),
+	LINK_INIT_CALL(c2c),
+};
+
+static int call_modem_init_func(struct modem_ctl *mc, struct modem_data *pdata)
+{
+	if (modem_init_func[pdata->modem_type])
+		return modem_init_func[pdata->modem_type](mc, pdata);
+	else
+		return -ENOTSUPP;
+}
+
+static struct link_device *call_link_init_func(struct platform_device *pdev,
+			enum modem_link link_type)
+{
+	if (link_init_func[link_type])
+		return link_init_func[link_type](pdev);
+	else
+		return NULL;
+}
+
+#endif
diff --git a/drivers/misc/modem_if/sipc4_io_device.c b/drivers/misc/modem_if/sipc4_io_device.c
new file mode 100644
index 0000000..94cd85b
--- /dev/null
+++ b/drivers/misc/modem_if/sipc4_io_device.c
@@ -0,0 +1,1540 @@
+/* /linux/drivers/misc/modem_if/modem_io_device.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+#include <linux/sched.h>
+#include <linux/fs.h>
+#include <linux/poll.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/if_arp.h>
+#include <linux/ip.h>
+#include <linux/if_ether.h>
+#include <linux/etherdevice.h>
+#include <linux/device.h>
+
+#include <linux/platform_data/modem.h>
+#ifdef CONFIG_LINK_DEVICE_C2C
+#include <linux/platform_data/c2c.h>
+#endif
+#include "modem_prj.h"
+#include "modem_utils.h"
+
+/*
+ * MAX_RXDATA_SIZE is used at making skb, when it called with page size
+ * it need more bytes to allocate itself (Ex, cache byte, shared info,
+ * padding...)
+ * So, give restriction to allocation size below 1 page to prevent
+ * big pages broken.
+ */
+#define MAX_RXDATA_SIZE		0x0E00	/* 4 * 1024 - 512 */
+#define MAX_MULTI_FMT_SIZE	0x4000	/* 16 * 1024 */
+
+static const char hdlc_start[1] = { HDLC_START };
+static const char hdlc_end[1] = { HDLC_END };
+
+static int rx_iodev_skb(struct sk_buff *skb);
+
+static ssize_t show_waketime(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	unsigned int msec;
+	char *p = buf;
+	struct miscdevice *miscdev = dev_get_drvdata(dev);
+	struct io_device *iod = container_of(miscdev, struct io_device,
+			miscdev);
+
+	msec = jiffies_to_msecs(iod->waketime);
+
+	p += sprintf(buf, "raw waketime : %ums\n", msec);
+
+	return p - buf;
+}
+
+static ssize_t store_waketime(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned long msec;
+	int ret;
+	struct miscdevice *miscdev = dev_get_drvdata(dev);
+	struct io_device *iod = container_of(miscdev, struct io_device,
+			miscdev);
+
+	ret = strict_strtoul(buf, 10, &msec);
+	if (ret)
+		return count;
+
+	iod->waketime = msecs_to_jiffies(msec);
+
+	return count;
+}
+
+static struct device_attribute attr_waketime =
+	__ATTR(waketime, S_IRUGO | S_IWUSR, show_waketime, store_waketime);
+
+static int get_header_size(struct io_device *iod)
+{
+	switch (iod->format) {
+	case IPC_FMT:
+		return sizeof(struct fmt_hdr);
+
+	case IPC_RAW:
+	case IPC_MULTI_RAW:
+		return sizeof(struct raw_hdr);
+
+	case IPC_RFS:
+		return sizeof(struct rfs_hdr);
+
+	case IPC_BOOT:
+		/* minimum size for transaction align */
+		return 4;
+
+	case IPC_RAMDUMP:
+	default:
+		return 0;
+	}
+}
+
+static int get_hdlc_size(struct io_device *iod, char *buf)
+{
+	struct fmt_hdr *fmt_header;
+	struct raw_hdr *raw_header;
+	struct rfs_hdr *rfs_header;
+
+	mif_debug("buf : %02x %02x %02x (%d)\n", *buf, *(buf + 1),
+				*(buf + 2), __LINE__);
+
+	switch (iod->format) {
+	case IPC_FMT:
+		fmt_header = (struct fmt_hdr *)buf;
+		if (iod->mc->mdm_data->ipc_version == SIPC_VER_42)
+			return fmt_header->len & 0x3FFF;
+		else
+			return fmt_header->len;
+	case IPC_RAW:
+	case IPC_MULTI_RAW:
+		raw_header = (struct raw_hdr *)buf;
+		return raw_header->len;
+	case IPC_RFS:
+		rfs_header = (struct rfs_hdr *)buf;
+		return rfs_header->len;
+	default:
+		break;
+	}
+	return 0;
+}
+
+static void *get_header(struct io_device *iod, size_t count,
+			char *frame_header_buf)
+{
+	struct fmt_hdr *fmt_h;
+	struct raw_hdr *raw_h;
+	struct rfs_hdr *rfs_h;
+
+	switch (iod->format) {
+	case IPC_FMT:
+		fmt_h = (struct fmt_hdr *)frame_header_buf;
+
+		fmt_h->len = count + sizeof(struct fmt_hdr);
+		fmt_h->control = 0;
+
+		return (void *)frame_header_buf;
+
+	case IPC_RAW:
+	case IPC_MULTI_RAW:
+		raw_h = (struct raw_hdr *)frame_header_buf;
+
+		raw_h->len = count + sizeof(struct raw_hdr);
+		raw_h->channel = iod->id & 0x1F;
+		raw_h->control = 0;
+
+		return (void *)frame_header_buf;
+
+	case IPC_RFS:
+		rfs_h = (struct rfs_hdr *)frame_header_buf;
+
+		rfs_h->len = count + sizeof(struct raw_hdr);
+		rfs_h->id = iod->id;
+
+		return (void *)frame_header_buf;
+
+	default:
+		return 0;
+	}
+}
+
+static inline int calc_padding_size(struct io_device *iod,
+	struct link_device *ld, unsigned len)
+{
+	if (ld->aligned)
+		return (4 - (len & 0x3)) & 0x3;
+	else
+		return 0;
+}
+
+static inline int rx_hdlc_head_start_check(char *buf)
+{
+	/* check hdlc head and return size of start byte */
+	return (buf[0] == HDLC_START) ? SIZE_OF_HDLC_START : -EBADMSG;
+}
+
+static inline int rx_hdlc_tail_check(char *buf)
+{
+	/* check hdlc tail and return size of tail byte */
+	return (buf[0] == HDLC_END) ? SIZE_OF_HDLC_END : -EBADMSG;
+}
+
+/* remove hdlc header and store IPC header */
+static int rx_hdlc_head_check(struct io_device *iod, struct link_device *ld,
+						char *buf, unsigned rest)
+{
+	struct header_data *hdr = &fragdata(iod, ld)->h_data;
+	int head_size = get_header_size(iod);
+	int done_len = 0;
+	int len = 0;
+
+	/* first frame, remove start header 7F */
+	if (!hdr->start) {
+		len = rx_hdlc_head_start_check(buf);
+		if (len < 0) {
+			mif_err("Wrong HDLC start: 0x%x\n", *buf);
+			return len; /*Wrong hdlc start*/
+		}
+
+		mif_debug("check len : %d, rest : %d (%d)\n", len,
+					rest, __LINE__);
+
+		/* set the start flag of current packet */
+		hdr->start = HDLC_START;
+		hdr->len = 0;
+
+		/* debug print */
+		switch (iod->format) {
+		case IPC_FMT:
+		case IPC_RAW:
+		case IPC_MULTI_RAW:
+		case IPC_RFS:
+			/* TODO: print buf...  */
+			break;
+
+		case IPC_CMD:
+		case IPC_BOOT:
+		case IPC_RAMDUMP:
+		default:
+			break;
+		}
+
+		buf += len;
+		done_len += len;
+		rest -= len; /* rest, call by value */
+	}
+
+	mif_debug("check len : %d, rest : %d (%d)\n",
+			len, rest, __LINE__);
+
+	/* store the HDLC header to iod priv */
+	if (hdr->len < head_size) {
+		len = min(rest, head_size - hdr->len);
+		memcpy(hdr->hdr + hdr->len, buf, len);
+		hdr->len += len;
+		done_len += len;
+	}
+
+	mif_debug("check done_len : %d, rest : %d (%d)\n", done_len,
+				rest, __LINE__);
+	return done_len;
+}
+
+/* alloc skb and copy data to skb */
+static int rx_hdlc_data_check(struct io_device *iod, struct link_device *ld,
+						char *buf, unsigned rest)
+{
+	struct header_data *hdr = &fragdata(iod, ld)->h_data;
+	struct sk_buff *skb = fragdata(iod, ld)->skb_recv;
+	int head_size = get_header_size(iod);
+	int data_size = get_hdlc_size(iod, hdr->hdr) - head_size;
+	int alloc_size;
+	int len = 0;
+	int done_len = 0;
+	int rest_len = data_size - hdr->frag_len;
+	int continue_len = fragdata(iod, ld)->realloc_offset;
+
+	mif_debug("head_size : %d, data_size : %d (%d)\n", head_size,
+				data_size, __LINE__);
+
+	if (continue_len) {
+		/* check the HDLC header*/
+		if (rx_hdlc_head_start_check(buf) == SIZE_OF_HDLC_START) {
+			rest_len -= (head_size + SIZE_OF_HDLC_START);
+			continue_len += (head_size + SIZE_OF_HDLC_START);
+		}
+
+		buf += continue_len;
+		rest -= continue_len;
+		done_len += continue_len;
+		fragdata(iod, ld)->realloc_offset = 0;
+
+		mif_debug("realloc_offset = %d\n", continue_len);
+	}
+
+	/* first payload data - alloc skb */
+	if (!skb) {
+		/* make skb data size under MAX_RXDATA_SIZE */
+		alloc_size = min(data_size, MAX_RXDATA_SIZE);
+		alloc_size = min(alloc_size, rest_len);
+
+		/* exceptional case for RFS channel
+		 * make skb for header info first
+		 */
+		if (iod->format == IPC_RFS && !hdr->frag_len) {
+			skb = rx_alloc_skb(head_size, GFP_ATOMIC, iod, ld);
+			if (unlikely(!skb))
+				return -ENOMEM;
+			memcpy(skb_put(skb, head_size), hdr->hdr, head_size);
+			rx_iodev_skb(skb);
+		}
+
+		/* allocate first packet for data, when its size exceed
+		 * MAX_RXDATA_SIZE, this packet will split to
+		 * multiple packets
+		 */
+		if (iod->use_handover)
+			alloc_size += sizeof(struct ethhdr);
+
+		skb = rx_alloc_skb(alloc_size, GFP_ATOMIC, iod, ld);
+		if (unlikely(!skb)) {
+			fragdata(iod, ld)->realloc_offset = continue_len;
+			return -ENOMEM;
+		}
+
+		if (iod->use_handover)
+			skb_reserve(skb, sizeof(struct ethhdr));
+		fragdata(iod, ld)->skb_recv = skb;
+	}
+
+	while (rest) {
+		/* copy length cannot exceed rest_len */
+		len = min_t(int, rest_len, rest);
+		/* copy length should be under skb tailroom size */
+		len = min(len, skb_tailroom(skb));
+		/* when skb tailroom is bigger than MAX_RXDATA_SIZE
+		 * restrict its size to MAX_RXDATA_SIZE just for convinience */
+		len = min(len, MAX_RXDATA_SIZE);
+
+		/* copy bytes to skb */
+		memcpy(skb_put(skb, len), buf, len);
+
+		/* adjusting variables */
+		buf += len;
+		rest -= len;
+		done_len += len;
+		rest_len -= len;
+		hdr->frag_len += len;
+
+		/* check if it is final for this packet sequence */
+		if (!rest_len || !rest)
+			break;
+
+		/* more bytes are remain for this packet sequence
+		 * pass fully loaded skb to rx queue
+		 * and allocate another skb for continues data recv chain
+		 */
+		rx_iodev_skb(skb);
+		fragdata(iod, ld)->skb_recv =  NULL;
+
+		alloc_size = min(rest_len, MAX_RXDATA_SIZE);
+
+		skb = rx_alloc_skb(alloc_size, GFP_ATOMIC, iod, ld);
+		if (unlikely(!skb)) {
+			fragdata(iod, ld)->realloc_offset = done_len;
+			return -ENOMEM;
+		}
+		fragdata(iod, ld)->skb_recv = skb;
+	}
+
+	mif_debug("rest : %d, alloc_size : %d , len : %d (%d)\n",
+				rest, alloc_size, skb->len, __LINE__);
+
+	return done_len;
+}
+
+static int rx_multi_fmt_frame(struct sk_buff *rx_skb)
+{
+	struct io_device *iod = skbpriv(rx_skb)->iod;
+	struct link_device *ld = skbpriv(rx_skb)->ld;
+	struct fmt_hdr *fh =
+		(struct fmt_hdr *)fragdata(iod, ld)->h_data.hdr;
+	unsigned int id = fh->control & 0x7F;
+	struct sk_buff *skb = iod->skb[id];
+	unsigned char *data = fragdata(iod, ld)->skb_recv->data;
+	unsigned int rcvd = fragdata(iod, ld)->skb_recv->len;
+
+	if (!skb) {
+		/* If there has been no multiple frame with this ID */
+		if (!(fh->control & 0x80)) {
+			/* It is a single frame because the "more" bit is 0. */
+#if 0
+			mif_err("\n<%s> Rx FMT frame (len %d)\n",
+				iod->name, rcvd);
+			print_sipc4_fmt_frame(data);
+			mif_err("\n");
+#endif
+			skb_queue_tail(&iod->sk_rx_q,
+					fragdata(iod, ld)->skb_recv);
+			mif_debug("wake up wq of %s\n", iod->name);
+			wake_up(&iod->wq);
+			return 0;
+		} else {
+			struct fmt_hdr *fh = NULL;
+			skb = rx_alloc_skb(MAX_MULTI_FMT_SIZE, GFP_ATOMIC,
+					iod, ld);
+			if (!skb) {
+				mif_err("<%d> alloc_skb fail\n",
+					__LINE__);
+				return -ENOMEM;
+			}
+			iod->skb[id] = skb;
+
+			fh = (struct fmt_hdr *)data;
+			mif_info("Start multi-frame (ID %d, len %d)",
+				id, fh->len);
+		}
+	}
+
+	/* Start multi-frame processing */
+
+	memcpy(skb_put(skb, rcvd), data, rcvd);
+	dev_kfree_skb_any(fragdata(iod, ld)->skb_recv);
+
+	if (fh->control & 0x80) {
+		/* The last frame has not arrived yet. */
+		mif_info("Receiving (ID %d, %d bytes)\n",
+			id, skb->len);
+	} else {
+		/* It is the last frame because the "more" bit is 0. */
+		mif_info("The Last (ID %d, %d bytes received)\n",
+			id, skb->len);
+#if 0
+		mif_err("\n<%s> Rx FMT frame (len %d)\n",
+			iod->name, skb->len);
+		print_sipc4_fmt_frame(skb->data);
+		mif_err("\n");
+#endif
+		skb_queue_tail(&iod->sk_rx_q, skb);
+		iod->skb[id] = NULL;
+		mif_info("wake up wq of %s\n", iod->name);
+		wake_up(&iod->wq);
+	}
+
+	return 0;
+}
+
+static int rx_multi_fmt_frame_sipc42(struct sk_buff *rx_skb)
+{
+	struct io_device *iod = skbpriv(rx_skb)->iod;
+	struct link_device *ld = skbpriv(rx_skb)->ld;
+	struct fmt_hdr *fh =
+		(struct fmt_hdr *)fragdata(iod, ld)->h_data.hdr;
+	unsigned int    id = fh->control & 0x7F;
+	struct sk_buff *skb = iod->skb[id];
+	unsigned char  *data = fragdata(iod, ld)->skb_recv->data;
+	unsigned int    rcvd = fragdata(iod, ld)->skb_recv->len;
+
+	u8 ch;
+	struct io_device *real_iod = NULL;
+
+	ch = (fh->len & 0xC000) >> 14;
+	fh->len = fh->len & 0x3FFF;
+	real_iod = ld->fmt_iods[ch];
+	if (!real_iod) {
+		mif_err("wrong channel %d\n", ch);
+		return -1;
+	}
+	skbpriv(rx_skb)->real_iod = real_iod;
+
+	if (!skb) {
+		/* If there has been no multiple frame with this ID */
+		if (!(fh->control & 0x80)) {
+			/* It is a single frame because the "more" bit is 0. */
+#if 0
+			mif_err("\n<%s> Rx FMT frame (len %d)\n",
+				iod->name, rcvd);
+			print_sipc4_fmt_frame(data);
+			mif_err("\n");
+#endif
+			skb_queue_tail(&real_iod->sk_rx_q,
+					fragdata(iod, ld)->skb_recv);
+			mif_debug("wake up wq of %s\n", iod->name);
+			wake_up(&real_iod->wq);
+			return 0;
+		} else {
+			struct fmt_hdr *fh = NULL;
+			skb = rx_alloc_skb(MAX_MULTI_FMT_SIZE, GFP_ATOMIC,
+					real_iod, ld);
+			if (!skb) {
+				mif_err("alloc_skb fail\n");
+				return -ENOMEM;
+			}
+			real_iod->skb[id] = skb;
+
+			fh = (struct fmt_hdr *)data;
+			mif_err("Start multi-frame (ID %d, len %d)",
+				id, fh->len);
+		}
+	}
+
+	/* Start multi-frame processing */
+
+	memcpy(skb_put(skb, rcvd), data, rcvd);
+	dev_kfree_skb_any(fragdata(real_iod, ld)->skb_recv);
+
+	if (fh->control & 0x80) {
+		/* The last frame has not arrived yet. */
+		mif_err("Receiving (ID %d, %d bytes)\n",
+			id, skb->len);
+	} else {
+		/* It is the last frame because the "more" bit is 0. */
+		mif_err("The Last (ID %d, %d bytes received)\n",
+			id, skb->len);
+#if 0
+		mif_err("\n<%s> Rx FMT frame (len %d)\n",
+			iod->name, skb->len);
+		print_sipc4_fmt_frame(skb->data);
+		mif_err("\n");
+#endif
+		skb_queue_tail(&real_iod->sk_rx_q, skb);
+		real_iod->skb[id] = NULL;
+		mif_info("wake up wq of %s\n", real_iod->name);
+		wake_up(&real_iod->wq);
+	}
+
+	return 0;
+}
+
+static int rx_iodev_skb_raw(struct sk_buff *skb)
+{
+	int err = 0;
+	struct io_device *iod = skbpriv(skb)->real_iod;
+	struct net_device *ndev = NULL;
+	struct iphdr *ip_header = NULL;
+	struct ethhdr *ehdr = NULL;
+	const char source[ETH_ALEN] = SOURCE_MAC_ADDR;
+
+	/* check the real_iod is open? */
+	/*
+	if (atomic_read(&iod->opened) == 0) {
+		mif_err("<%s> is not opened.\n",
+			iod->name);
+		pr_skb("drop packet", skb);
+		return -ENOENT;
+	}
+	*/
+
+	switch (iod->io_typ) {
+	case IODEV_MISC:
+		mif_debug("<%s> sk_rx_q.qlen = %d\n",
+			iod->name, iod->sk_rx_q.qlen);
+		skb_queue_tail(&iod->sk_rx_q, skb);
+		wake_up(&iod->wq);
+		return 0;
+
+	case IODEV_NET:
+		ndev = iod->ndev;
+		if (!ndev) {
+			mif_err("<%s> ndev == NULL",
+				iod->name);
+			return -EINVAL;
+		}
+
+		skb->dev = ndev;
+		ndev->stats.rx_packets++;
+		ndev->stats.rx_bytes += skb->len;
+
+		/* check the version of IP */
+		ip_header = (struct iphdr *)skb->data;
+		if (ip_header->version == IP6VERSION)
+			skb->protocol = htons(ETH_P_IPV6);
+		else
+			skb->protocol = htons(ETH_P_IP);
+
+		if (iod->use_handover) {
+			skb_push(skb, sizeof(struct ethhdr));
+			ehdr = (void *)skb->data;
+			memcpy(ehdr->h_dest, ndev->dev_addr, ETH_ALEN);
+			memcpy(ehdr->h_source, source, ETH_ALEN);
+			ehdr->h_proto = skb->protocol;
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+			skb_reset_mac_header(skb);
+
+			skb_pull(skb, sizeof(struct ethhdr));
+		}
+
+		if (in_irq())
+			err = netif_rx(skb);
+		else
+			err = netif_rx_ni(skb);
+
+		if (err != NET_RX_SUCCESS)
+			dev_err(&ndev->dev, "rx error: %d\n", err);
+
+		return err;
+
+	default:
+		mif_err("wrong io_type : %d\n", iod->io_typ);
+		return -EINVAL;
+	}
+}
+
+static void rx_iodev_work(struct work_struct *work)
+{
+	int ret = 0;
+	struct sk_buff *skb = NULL;
+	struct io_device *iod = container_of(work, struct io_device,
+				rx_work.work);
+
+	while ((skb = skb_dequeue(&iod->sk_rx_q)) != NULL) {
+		ret = rx_iodev_skb_raw(skb);
+		if (ret < 0) {
+			mif_err("<%s> rx_iodev_skb_raw err = %d",
+				iod->name, ret);
+			dev_kfree_skb_any(skb);
+		} else if (ret == NET_RX_DROP) {
+			mif_err("<%s> ret == NET_RX_DROP\n",
+				iod->name);
+			schedule_delayed_work(&iod->rx_work,
+						msecs_to_jiffies(100));
+			break;
+		}
+	}
+}
+
+static int rx_multipdp(struct sk_buff *skb)
+{
+	u8 ch;
+	struct io_device *iod = skbpriv(skb)->iod;
+	struct link_device *ld = skbpriv(skb)->ld;
+	struct raw_hdr *raw_header =
+		(struct raw_hdr *)fragdata(iod, ld)->h_data.hdr;
+	struct io_device *real_iod = NULL;
+
+	ch = raw_header->channel;
+	real_iod = link_get_iod_with_channel(ld, 0x20 | ch);
+	if (!real_iod) {
+		mif_err("wrong channel %d\n", ch);
+		return -1;
+	}
+
+	skbpriv(skb)->real_iod = real_iod;
+	skb_queue_tail(&iod->sk_rx_q, skb);
+	mif_debug("sk_rx_qlen:%d\n", iod->sk_rx_q.qlen);
+
+	schedule_delayed_work(&iod->rx_work, 0);
+	return 0;
+}
+
+/* de-mux function draft */
+static int rx_iodev_skb(struct sk_buff *skb)
+{
+	struct io_device *iod = skbpriv(skb)->iod;
+
+	switch (iod->format) {
+	case IPC_MULTI_RAW:
+		return rx_multipdp(skb);
+
+	case IPC_FMT:
+		if (iod->mc->mdm_data->ipc_version == SIPC_VER_42)
+			return rx_multi_fmt_frame_sipc42(skb);
+		else
+			return rx_multi_fmt_frame(skb);
+
+	case IPC_RFS:
+	default:
+		skb_queue_tail(&iod->sk_rx_q, skb);
+		mif_debug("wake up wq of %s\n", iod->name);
+		wake_up(&iod->wq);
+		return 0;
+	}
+}
+
+static int rx_hdlc_packet(struct io_device *iod, struct link_device *ld,
+		const char *data, unsigned recv_size)
+{
+	int rest = (int)recv_size;
+	char *buf = (char *)data;
+	int err = 0;
+	int len = 0;
+	unsigned rcvd = 0;
+
+	if (rest <= 0)
+		goto exit;
+
+	mif_debug("RX_SIZE = %d, ld: %s\n", rest, ld->name);
+
+	if (fragdata(iod, ld)->h_data.frag_len) {
+		/*
+		  If the fragdata(iod, ld)->h_data.frag_len field is
+		  not zero, there is a HDLC frame that is waiting for more data
+		  or HDLC_END in the skb (fragdata(iod, ld)->skb_recv).
+		  In this case, rx_hdlc_head_check() must be skipped.
+		*/
+		goto data_check;
+	}
+
+next_frame:
+	err = len = rx_hdlc_head_check(iod, ld, buf, rest);
+	if (err < 0)
+		goto exit;
+	mif_debug("check len : %d, rest : %d (%d)\n", len, rest,
+				__LINE__);
+
+	buf += len;
+	rest -= len;
+	if (rest <= 0)
+		goto exit;
+
+data_check:
+	/*
+	  If the return value of rx_hdlc_data_check() is zero, there remains
+	  only HDLC_END that will be received.
+	*/
+	err = len = rx_hdlc_data_check(iod, ld, buf, rest);
+	if (err < 0)
+		goto exit;
+	mif_debug("check len : %d, rest : %d (%d)\n", len, rest,
+				__LINE__);
+
+	buf += len;
+	rest -= len;
+
+	if (!rest && fragdata(iod, ld)->h_data.frag_len) {
+		/*
+		  Data is being received and more data or HDLC_END does not
+		  arrive yet, but there is no more data in the buffer. More
+		  data may come within the next frame from the link device.
+		*/
+		return 0;
+	} else if (rest <= 0)
+		goto exit;
+
+	/* At this point, one HDLC frame except HDLC_END has been received. */
+
+	err = len = rx_hdlc_tail_check(buf);
+	if (err < 0) {
+		mif_err("Wrong HDLC end: 0x%02X\n", *buf);
+		goto exit;
+	}
+	mif_debug("check len : %d, rest : %d (%d)\n", len, rest,
+				__LINE__);
+	buf += len;
+	rest -= len;
+
+	/* At this point, one complete HDLC frame has been received. */
+
+	/*
+	  The padding size is applied for the next HDLC frame. Zero will be
+	  returned by calc_padding_size() if the link device does not require
+	  4-byte aligned access.
+	*/
+	rcvd = get_hdlc_size(iod, fragdata(iod, ld)->h_data.hdr) +
+	       (SIZE_OF_HDLC_START + SIZE_OF_HDLC_END);
+	len = calc_padding_size(iod, ld, rcvd);
+	buf += len;
+	rest -= len;
+	if (rest < 0)
+		goto exit;
+
+	err = rx_iodev_skb(fragdata(iod, ld)->skb_recv);
+	if (err < 0)
+		goto exit;
+
+	/* initialize header & skb */
+	fragdata(iod, ld)->skb_recv = NULL;
+	memset(&fragdata(iod, ld)->h_data, 0x00,
+			sizeof(struct header_data));
+	fragdata(iod, ld)->realloc_offset = 0;
+
+	if (rest)
+		goto next_frame;
+
+exit:
+	/* free buffers. mipi-hsi re-use recv buf */
+
+	if (rest < 0)
+		err = -ERANGE;
+
+	if (err == -ENOMEM) {
+		if (!(fragdata(iod, ld)->h_data.frag_len))
+			memset(&fragdata(iod, ld)->h_data, 0x00,
+				sizeof(struct header_data));
+		return err;
+	}
+
+	if (err < 0 && fragdata(iod, ld)->skb_recv) {
+		dev_kfree_skb_any(fragdata(iod, ld)->skb_recv);
+		fragdata(iod, ld)->skb_recv = NULL;
+
+		/* clear headers */
+		memset(&fragdata(iod, ld)->h_data, 0x00,
+				sizeof(struct header_data));
+		fragdata(iod, ld)->realloc_offset = 0;
+	}
+
+	return err;
+}
+
+static int rx_rfs_packet(struct io_device *iod, struct link_device *ld,
+					const char *data, unsigned size)
+{
+	int err = 0;
+	int pad = 0;
+	int rcvd = 0;
+	struct sk_buff *skb;
+
+	if (data[0] != HDLC_START) {
+		mif_err("Dropping RFS packet ... "
+		       "size = %d, start = %02X %02X %02X %02X\n",
+			size,
+			data[0], data[1], data[2], data[3]);
+		return -EINVAL;
+	}
+
+	if (data[size-1] != HDLC_END) {
+		for (pad = 1; pad < 4; pad++)
+			if (data[(size-1)-pad] == HDLC_END)
+				break;
+
+		if (pad >= 4) {
+			char *b = (char *)data;
+			unsigned sz = size;
+			mif_err("size %d, No END_FLAG!!!\n", size);
+			mif_err("end = %02X %02X %02X %02X\n",
+				b[sz-4], b[sz-3], b[sz-2], b[sz-1]);
+			return -EINVAL;
+		} else {
+			mif_info("padding = %d\n", pad);
+		}
+	}
+
+	skb = rx_alloc_skb(size, GFP_ATOMIC, iod, ld);
+	if (unlikely(!skb)) {
+		mif_err("alloc_skb fail\n");
+		return -ENOMEM;
+	}
+
+	/* copy the RFS haeder to skb->data */
+	rcvd = size - sizeof(hdlc_start) - sizeof(hdlc_end) - pad;
+	memcpy(skb_put(skb, rcvd), ((char *)data + sizeof(hdlc_start)), rcvd);
+
+	fragdata(iod, ld)->skb_recv = skb;
+	err = rx_iodev_skb(fragdata(iod, ld)->skb_recv);
+
+	return err;
+}
+
+/* called from link device when a packet arrives for this io device */
+static int io_dev_recv_data_from_link_dev(struct io_device *iod,
+		struct link_device *ld, const char *data, unsigned int len)
+{
+	struct sk_buff *skb;
+	int err;
+
+	/* check the iod(except IODEV_DUMMY) is open?
+	 * if the iod is MULTIPDP, check this data on rx_iodev_skb_raw()
+	 * because, we cannot know the channel no in here.
+	 */
+	/*
+	if (iod->io_typ != IODEV_DUMMY && atomic_read(&iod->opened) == 0) {
+		mif_err("<%s> is not opened.\n", iod->name);
+		pr_buffer("drop packet", data, len, 16u);
+		return -ENOENT;
+	}
+	*/
+
+	switch (iod->format) {
+	case IPC_RFS:
+#ifdef CONFIG_IPC_CMC22x_OLD_RFS
+		err = rx_rfs_packet(iod, ld, data, len);
+		return err;
+#endif
+
+	case IPC_FMT:
+	case IPC_RAW:
+	case IPC_MULTI_RAW:
+		if (iod->waketime)
+			wake_lock_timeout(&iod->wakelock, iod->waketime);
+		err = rx_hdlc_packet(iod, ld, data, len);
+		if (err < 0)
+			mif_err("fail process HDLC frame\n");
+		return err;
+
+	case IPC_CMD:
+		/* TODO- handle flow control command from CP */
+		return 0;
+
+	case IPC_BOOT:
+	case IPC_RAMDUMP:
+		/* save packet to sk_buff */
+		skb = rx_alloc_skb(len, GFP_ATOMIC, iod, ld);
+		if (!skb) {
+			mif_err("fail alloc skb (%d)\n", __LINE__);
+			return -ENOMEM;
+		}
+
+		mif_debug("boot len : %d\n", len);
+
+		memcpy(skb_put(skb, len), data, len);
+		skb_queue_tail(&iod->sk_rx_q, skb);
+		mif_debug("skb len : %d\n", skb->len);
+
+		wake_up(&iod->wq);
+		return len;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+/* inform the IO device that the modem is now online or offline or
+ * crashing or whatever...
+ */
+static void io_dev_modem_state_changed(struct io_device *iod,
+			enum modem_state state)
+{
+	iod->mc->phone_state = state;
+	mif_err("modem state changed. (iod: %s, state: %d)\n",
+		iod->name, state);
+
+	if ((state == STATE_CRASH_RESET) || (state == STATE_CRASH_EXIT)
+		|| (state == STATE_NV_REBUILDING))
+		wake_up(&iod->wq);
+}
+
+/**
+ * io_dev_sim_state_changed
+ * @iod:	IPC's io_device
+ * @sim_online: SIM is online?
+ */
+static void io_dev_sim_state_changed(struct io_device *iod, bool sim_online)
+{
+	if (atomic_read(&iod->opened) == 0) {
+		mif_err("iod is not opened: %s\n",
+				iod->name);
+	} else if (iod->mc->sim_state.online == sim_online) {
+		mif_err("sim state not changed.\n");
+	} else {
+		iod->mc->sim_state.online = sim_online;
+		iod->mc->sim_state.changed = true;
+		wake_lock_timeout(&iod->mc->bootd->wakelock,
+						iod->mc->bootd->waketime);
+		mif_err("sim state changed. (iod: %s, state: "
+				"[online=%d, changed=%d])\n",
+				iod->name, iod->mc->sim_state.online,
+				iod->mc->sim_state.changed);
+		wake_up(&iod->wq);
+	}
+}
+
+static int misc_open(struct inode *inode, struct file *filp)
+{
+	struct io_device *iod = to_io_device(filp->private_data);
+	struct mif_common *commons = &iod->mc->commons;
+	struct link_device *ld;
+	int ret;
+	filp->private_data = (void *)iod;
+
+	mif_err("iod = %s\n", iod->name);
+	atomic_inc(&iod->opened);
+
+	list_for_each_entry(ld, &commons->link_dev_list, list) {
+		if (IS_CONNECTED(iod, ld) && ld->init_comm) {
+			ret = ld->init_comm(ld, iod);
+			if (ret < 0) {
+				mif_err("%s: init_comm error: %d\n",
+						ld->name, ret);
+				return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int misc_release(struct inode *inode, struct file *filp)
+{
+	struct io_device *iod = (struct io_device *)filp->private_data;
+	struct mif_common *commons = &iod->mc->commons;
+	struct link_device *ld;
+
+	mif_err("iod = %s\n", iod->name);
+	atomic_dec(&iod->opened);
+	skb_queue_purge(&iod->sk_rx_q);
+
+	list_for_each_entry(ld, &commons->link_dev_list, list) {
+		if (IS_CONNECTED(iod, ld) && ld->terminate_comm)
+			ld->terminate_comm(ld, iod);
+	}
+
+	return 0;
+}
+
+static unsigned int misc_poll(struct file *filp, struct poll_table_struct *wait)
+{
+	struct io_device *iod = (struct io_device *)filp->private_data;
+
+	poll_wait(filp, &iod->wq, wait);
+
+	if ((!skb_queue_empty(&iod->sk_rx_q)) &&
+	    (iod->mc->phone_state != STATE_OFFLINE)) {
+		return POLLIN | POLLRDNORM;
+	} else if ((iod->mc->phone_state == STATE_CRASH_RESET) ||
+			(iod->mc->phone_state == STATE_CRASH_EXIT) ||
+			(iod->mc->phone_state == STATE_NV_REBUILDING) ||
+			(iod->mc->sim_state.changed)) {
+		if (iod->format == IPC_RAW) {
+			msleep(20);
+			return 0;
+		}
+		return POLLHUP;
+	} else {
+		return 0;
+	}
+}
+
+static long misc_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	int p_state;
+	struct io_device *iod = (struct io_device *)filp->private_data;
+	struct link_device *ld = get_current_link(iod);
+	char cpinfo_buf[530] = "CP Crash ";
+
+	mif_debug("cmd = 0x%x\n", cmd);
+
+	switch (cmd) {
+	case IOCTL_MODEM_ON:
+		mif_debug("misc_ioctl : IOCTL_MODEM_ON\n");
+		return iod->mc->ops.modem_on(iod->mc);
+
+	case IOCTL_MODEM_OFF:
+		mif_debug("misc_ioctl : IOCTL_MODEM_OFF\n");
+		return iod->mc->ops.modem_off(iod->mc);
+
+	case IOCTL_MODEM_RESET:
+		mif_debug("misc_ioctl : IOCTL_MODEM_RESET\n");
+		return iod->mc->ops.modem_reset(iod->mc);
+
+	case IOCTL_MODEM_BOOT_ON:
+		mif_debug("misc_ioctl : IOCTL_MODEM_BOOT_ON\n");
+		return iod->mc->ops.modem_boot_on(iod->mc);
+
+	case IOCTL_MODEM_BOOT_OFF:
+		mif_debug("misc_ioctl : IOCTL_MODEM_BOOT_OFF\n");
+		return iod->mc->ops.modem_boot_off(iod->mc);
+
+	/* TODO - will remove this command after ril updated */
+	case IOCTL_MODEM_START:
+		mif_debug("misc_ioctl : IOCTL_MODEM_START\n");
+		return 0;
+
+	case IOCTL_MODEM_STATUS:
+		mif_debug("misc_ioctl : IOCTL_MODEM_STATUS\n");
+
+		p_state = iod->mc->phone_state;
+		if ((p_state == STATE_CRASH_RESET) ||
+		    (p_state == STATE_CRASH_EXIT)) {
+			mif_err("<%s> send err state : %d\n",
+				iod->name, p_state);
+		} else if (iod->mc->sim_state.changed) {
+			int s_state = iod->mc->sim_state.online ?
+					STATE_SIM_ATTACH : STATE_SIM_DETACH;
+			iod->mc->sim_state.changed = false;
+			return s_state;
+		} else if (p_state == STATE_NV_REBUILDING) {
+			mif_info("send nv rebuild state : %d\n",
+				p_state);
+			iod->mc->phone_state = STATE_ONLINE;
+		}
+		return p_state;
+
+	case IOCTL_MODEM_PROTOCOL_SUSPEND:
+		mif_info("misc_ioctl : IOCTL_MODEM_PROTOCOL_SUSPEND\n");
+
+		if (iod->format != IPC_MULTI_RAW)
+			return -EINVAL;
+
+		iodevs_for_each(&iod->mc->commons, iodev_netif_stop, 0);
+		return 0;
+
+	case IOCTL_MODEM_PROTOCOL_RESUME:
+		mif_info("misc_ioctl : IOCTL_MODEM_PROTOCOL_RESUME\n");
+
+		if (iod->format != IPC_MULTI_RAW)
+			return -EINVAL;
+
+		iodevs_for_each(&iod->mc->commons, iodev_netif_wake, 0);
+		return 0;
+
+	case IOCTL_MODEM_DUMP_START:
+		mif_err("misc_ioctl : IOCTL_MODEM_DUMP_START\n");
+		return ld->dump_start(ld, iod);
+
+	case IOCTL_MODEM_DUMP_UPDATE:
+		mif_debug("misc_ioctl : IOCTL_MODEM_DUMP_UPDATE\n");
+		return ld->dump_update(ld, iod, arg);
+
+	case IOCTL_MODEM_FORCE_CRASH_EXIT:
+		mif_debug("misc_ioctl : IOCTL_MODEM_FORCE_CRASH_EXIT\n");
+		if (iod->mc->ops.modem_force_crash_exit)
+			return iod->mc->ops.modem_force_crash_exit(iod->mc);
+		return -EINVAL;
+
+	case IOCTL_MODEM_CP_UPLOAD:
+		mif_err("misc_ioctl : IOCTL_MODEM_CP_UPLOAD\n");
+		if (copy_from_user(cpinfo_buf + strlen(cpinfo_buf),
+			(void __user *)arg, MAX_CPINFO_SIZE) != 0)
+			panic("CP Crash");
+		else
+			panic(cpinfo_buf);
+		return 0;
+
+	case IOCTL_MODEM_DUMP_RESET:
+		mif_err("misc_ioctl : IOCTL_MODEM_DUMP_RESET\n");
+		return iod->mc->ops.modem_dump_reset(iod->mc);
+
+	default:
+		 /* If you need to handle the ioctl for specific link device,
+		  * then assign the link ioctl handler to ld->ioctl
+		  * It will be call for specific link ioctl */
+		if (ld->ioctl)
+			return ld->ioctl(ld, iod, cmd, arg);
+
+		mif_err("misc_ioctl : ioctl 0x%X is not defined.\n", cmd);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static ssize_t misc_write(struct file *filp, const char __user *buf,
+			size_t count, loff_t *ppos)
+{
+	struct io_device *iod = (struct io_device *)filp->private_data;
+	struct link_device *ld = get_current_link(iod);
+	int frame_len = 0;
+	char frame_header_buf[sizeof(struct raw_hdr)];
+	struct sk_buff *skb;
+	int err;
+	size_t tx_size;
+
+	/* TODO - check here flow control for only raw data */
+
+	frame_len = SIZE_OF_HDLC_START +
+		    get_header_size(iod) +
+		    count +
+		    SIZE_OF_HDLC_END;
+	if (ld->aligned)
+		frame_len += MAX_LINK_PADDING_SIZE;
+
+	skb = alloc_skb(frame_len, GFP_KERNEL);
+	if (!skb) {
+		mif_err("fail alloc skb (%d)\n", __LINE__);
+		return -ENOMEM;
+	}
+
+	switch (iod->format) {
+	case IPC_BOOT:
+	case IPC_RAMDUMP:
+		if (copy_from_user(skb_put(skb, count), buf, count) != 0) {
+			dev_kfree_skb_any(skb);
+			return -EFAULT;
+		}
+		break;
+
+	case IPC_RFS:
+		memcpy(skb_put(skb, SIZE_OF_HDLC_START), hdlc_start,
+				SIZE_OF_HDLC_START);
+		if (copy_from_user(skb_put(skb, count), buf, count) != 0) {
+			dev_kfree_skb_any(skb);
+			return -EFAULT;
+		}
+		memcpy(skb_put(skb, SIZE_OF_HDLC_END), hdlc_end,
+					SIZE_OF_HDLC_END);
+		break;
+
+	default:
+		memcpy(skb_put(skb, SIZE_OF_HDLC_START), hdlc_start,
+				SIZE_OF_HDLC_START);
+		memcpy(skb_put(skb, get_header_size(iod)),
+			get_header(iod, count, frame_header_buf),
+			get_header_size(iod));
+		if (copy_from_user(skb_put(skb, count), buf, count) != 0) {
+			dev_kfree_skb_any(skb);
+			return -EFAULT;
+		}
+		memcpy(skb_put(skb, SIZE_OF_HDLC_END), hdlc_end,
+					SIZE_OF_HDLC_END);
+		break;
+	}
+
+	skb_put(skb, calc_padding_size(iod, ld, skb->len));
+
+#if 0
+	if (iod->format == IPC_FMT) {
+		mif_err("\n<%s> Tx HDLC FMT frame (len %d)\n",
+			iod->name, skb->len);
+		print_sipc4_hdlc_fmt_frame(skb->data);
+		mif_err("\n");
+	}
+#endif
+#if 0
+	if (iod->format == IPC_RAW) {
+		mif_err("\n<%s> Tx HDLC RAW frame (len %d)\n",
+			iod->name, skb->len);
+		mif_print_data(skb->data, (skb->len < 64 ? skb->len : 64));
+		mif_err("\n");
+	}
+#endif
+#if 0
+	if (iod->format == IPC_RFS) {
+		mif_err("\n<%s> Tx HDLC RFS frame (len %d)\n",
+			iod->name, skb->len);
+		mif_print_data(skb->data, (skb->len < 64 ? skb->len : 64));
+		mif_err("\n");
+	}
+#endif
+
+	/* send data with sk_buff, link device will put sk_buff
+	 * into the specific sk_buff_q and run work-q to send data
+	 */
+	tx_size = skb->len;
+
+	skbpriv(skb)->iod = iod;
+	skbpriv(skb)->ld = ld;
+
+	err = ld->send(ld, iod, skb);
+	if (err < 0) {
+		dev_kfree_skb_any(skb);
+		return err;
+	}
+
+	if (err != tx_size)
+		mif_err("WARNNING: wrong tx size: %s, format=%d "
+			"count=%d, tx_size=%d, return_size=%d",
+			iod->name, iod->format, count, tx_size, err);
+
+	/* Temporaly enable t he RFS log for debugging IPC RX pedding issue */
+	if (iod->format == IPC_RFS)
+		mif_info("write rfs size = %d\n", count);
+
+	return count;
+}
+
+static ssize_t misc_read(struct file *filp, char *buf, size_t count,
+			loff_t *f_pos)
+{
+	struct io_device *iod = (struct io_device *)filp->private_data;
+	struct sk_buff *skb = NULL;
+	int pktsize = 0;
+
+	skb = skb_dequeue(&iod->sk_rx_q);
+	if (!skb) {
+		mif_err("<%s> no data from sk_rx_q\n", iod->name);
+		return 0;
+	}
+	mif_debug("<%s> skb->len : %d\n", iod->name, skb->len);
+
+	if (skb->len > count) {
+		/* BOOT device receviced rx data as serial stream, return data
+		 by User requested size */
+		if (iod->format == IPC_BOOT) {
+			mif_err("skb->len %d > count %d\n", skb->len,
+				count);
+			pr_skb("BOOT-wRX", skb);
+			if (copy_to_user(buf, skb->data, count) != 0) {
+				dev_kfree_skb_any(skb);
+				return -EFAULT;
+			}
+			skb_pull(skb, count);
+			if (skb->len) {
+				mif_info("queue-head, skb->len = %d\n",
+					skb->len);
+				skb_queue_head(&iod->sk_rx_q, skb);
+			}
+
+			return count;
+		} else {
+			mif_err("<%s> skb->len %d > count %d\n",
+					iod->name, skb->len, count);
+			dev_kfree_skb_any(skb);
+			return -EFAULT;
+		}
+	}
+
+	pktsize = skb->len;
+	if (copy_to_user(buf, skb->data, pktsize) != 0) {
+		dev_kfree_skb_any(skb);
+		return -EFAULT;
+	}
+	if (iod->format == IPC_FMT)
+		mif_debug("copied %d bytes to user\n", pktsize);
+
+	dev_kfree_skb_any(skb);
+
+	return pktsize;
+}
+
+#ifdef CONFIG_LINK_DEVICE_C2C
+static int misc_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	int r = 0;
+	unsigned long size = 0;
+	unsigned long pfn = 0;
+	unsigned long offset = 0;
+	struct io_device *iod = (struct io_device *)filp->private_data;
+
+	if (!vma)
+		return -EFAULT;
+
+	size = vma->vm_end - vma->vm_start;
+	offset = vma->vm_pgoff << PAGE_SHIFT;
+	if (offset + size > (C2C_CP_RGN_SIZE + C2C_SH_RGN_SIZE)) {
+		mif_err("offset + size > C2C_CP_RGN_SIZE\n");
+		return -EINVAL;
+	}
+
+	/* Set the noncacheable property to the region */
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_flags |= VM_RESERVED | VM_IO;
+
+	pfn = __phys_to_pfn(C2C_CP_RGN_ADDR + offset);
+	r = remap_pfn_range(vma, vma->vm_start, pfn, size, vma->vm_page_prot);
+	if (r) {
+		mif_err("Failed in remap_pfn_range()!!!\n");
+		return -EAGAIN;
+	}
+
+	mif_err("VA = 0x%08lx, offset = 0x%lx, size = %lu\n",
+		vma->vm_start, offset, size);
+
+	return 0;
+}
+#endif
+
+static const struct file_operations misc_io_fops = {
+	.owner = THIS_MODULE,
+	.open = misc_open,
+	.release = misc_release,
+	.poll = misc_poll,
+	.unlocked_ioctl = misc_ioctl,
+	.write = misc_write,
+	.read = misc_read,
+#ifdef CONFIG_LINK_DEVICE_C2C
+	.mmap = misc_mmap,
+#endif
+};
+
+static int vnet_open(struct net_device *ndev)
+{
+	struct vnet *vnet = netdev_priv(ndev);
+	netif_start_queue(ndev);
+	atomic_inc(&vnet->iod->opened);
+	return 0;
+}
+
+static int vnet_stop(struct net_device *ndev)
+{
+	struct vnet *vnet = netdev_priv(ndev);
+	atomic_dec(&vnet->iod->opened);
+	netif_stop_queue(ndev);
+	return 0;
+}
+
+static int vnet_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+	int ret = 0;
+	int headroom = 0;
+	int tailroom = 0;
+	struct sk_buff *skb_new = NULL;
+	struct vnet *vnet = netdev_priv(ndev);
+	struct io_device *iod = vnet->iod;
+	struct link_device *ld = get_current_link(iod);
+	struct raw_hdr hd;
+
+	/* When use `handover' with Network Bridge,
+	 * user -> TCP/IP(kernel) -> bridge device -> TCP/IP(kernel) -> this.
+	 *
+	 * We remove the one ethernet header of skb before using skb->len,
+	 * because the skb has two ethernet headers.
+	 */
+	if (iod->use_handover) {
+		if (iod->id >= PSD_DATA_CHID_BEGIN &&
+			iod->id <= PSD_DATA_CHID_END)
+			skb_pull(skb, sizeof(struct ethhdr));
+	}
+
+	hd.len = skb->len + sizeof(hd);
+	hd.control = 0;
+	hd.channel = iod->id & 0x1F;
+
+	headroom = sizeof(hd) + sizeof(hdlc_start);
+	tailroom = sizeof(hdlc_end);
+	if (ld->aligned)
+		tailroom += MAX_LINK_PADDING_SIZE;
+	if (skb_headroom(skb) < headroom || skb_tailroom(skb) < tailroom) {
+		skb_new = skb_copy_expand(skb, headroom, tailroom, GFP_ATOMIC);
+		/* skb_copy_expand success or not, free old skb from caller */
+		dev_kfree_skb_any(skb);
+		if (!skb_new)
+			return -ENOMEM;
+	} else
+		skb_new = skb;
+
+	memcpy(skb_push(skb_new, sizeof(hd)), &hd, sizeof(hd));
+	memcpy(skb_push(skb_new, sizeof(hdlc_start)), hdlc_start,
+				sizeof(hdlc_start));
+	memcpy(skb_put(skb_new, sizeof(hdlc_end)), hdlc_end, sizeof(hdlc_end));
+	skb_put(skb_new, calc_padding_size(iod, ld,  skb_new->len));
+
+	skbpriv(skb_new)->iod = iod;
+	skbpriv(skb_new)->ld = ld;
+
+	ret = ld->send(ld, iod, skb_new);
+	if (ret < 0) {
+		netif_stop_queue(ndev);
+		dev_kfree_skb_any(skb_new);
+		return NETDEV_TX_BUSY;
+	}
+
+	ndev->stats.tx_packets++;
+	ndev->stats.tx_bytes += skb->len;
+
+	return NETDEV_TX_OK;
+}
+
+static struct net_device_ops vnet_ops = {
+	.ndo_open = vnet_open,
+	.ndo_stop = vnet_stop,
+	.ndo_start_xmit = vnet_xmit,
+};
+
+static void vnet_setup(struct net_device *ndev)
+{
+	ndev->netdev_ops = &vnet_ops;
+	ndev->type = ARPHRD_PPP;
+	ndev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
+	ndev->addr_len = 0;
+	ndev->hard_header_len = 0;
+	ndev->tx_queue_len = 1000;
+	ndev->mtu = ETH_DATA_LEN;
+	ndev->watchdog_timeo = 5 * HZ;
+}
+
+static void vnet_setup_ether(struct net_device *ndev)
+{
+	ndev->netdev_ops = &vnet_ops;
+	ndev->type = ARPHRD_ETHER;
+	ndev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST | IFF_SLAVE;
+	ndev->addr_len = ETH_ALEN;
+	random_ether_addr(ndev->dev_addr);
+	ndev->hard_header_len = 0;
+	ndev->tx_queue_len = 1000;
+	ndev->mtu = ETH_DATA_LEN;
+	ndev->watchdog_timeo = 5 * HZ;
+}
+
+int sipc4_init_io_device(struct io_device *iod)
+{
+	int ret = 0;
+	struct vnet *vnet;
+
+	/* Get modem state from modem control device */
+	iod->modem_state_changed = io_dev_modem_state_changed;
+
+	iod->sim_state_changed = io_dev_sim_state_changed;
+
+	/* Get data from link device */
+	iod->recv = io_dev_recv_data_from_link_dev;
+
+	/* Register misc or net device */
+	switch (iod->io_typ) {
+	case IODEV_MISC:
+		init_waitqueue_head(&iod->wq);
+		skb_queue_head_init(&iod->sk_rx_q);
+		INIT_DELAYED_WORK(&iod->rx_work, rx_iodev_work);
+
+		iod->miscdev.minor = MISC_DYNAMIC_MINOR;
+		iod->miscdev.name = iod->name;
+		iod->miscdev.fops = &misc_io_fops;
+
+		ret = misc_register(&iod->miscdev);
+		if (ret)
+			mif_err("failed to register misc io device : %s\n",
+						iod->name);
+
+		break;
+
+	case IODEV_NET:
+		skb_queue_head_init(&iod->sk_rx_q);
+		if (iod->use_handover)
+			iod->ndev = alloc_netdev(0, iod->name,
+						vnet_setup_ether);
+		else
+			iod->ndev = alloc_netdev(0, iod->name, vnet_setup);
+
+		if (!iod->ndev) {
+			mif_err("failed to alloc netdev\n");
+			return -ENOMEM;
+		}
+
+		ret = register_netdev(iod->ndev);
+		if (ret)
+			free_netdev(iod->ndev);
+
+		mif_debug("(iod:0x%p)\n", iod);
+		vnet = netdev_priv(iod->ndev);
+		mif_debug("(vnet:0x%p)\n", vnet);
+		vnet->iod = iod;
+
+		break;
+
+	case IODEV_DUMMY:
+		skb_queue_head_init(&iod->sk_rx_q);
+		INIT_DELAYED_WORK(&iod->rx_work, rx_iodev_work);
+
+		iod->miscdev.minor = MISC_DYNAMIC_MINOR;
+		iod->miscdev.name = iod->name;
+		iod->miscdev.fops = &misc_io_fops;
+
+		ret = misc_register(&iod->miscdev);
+		if (ret)
+			mif_err("failed to register misc io device : %s\n",
+						iod->name);
+		ret = device_create_file(iod->miscdev.this_device,
+				&attr_waketime);
+		if (ret)
+			mif_err("failed to create sysfs file : %s\n",
+					iod->name);
+
+		break;
+
+	default:
+		mif_err("wrong io_type : %d\n", iod->io_typ);
+		return -EINVAL;
+	}
+
+	mif_debug("%s(%d) : init_io_device() done : %d\n",
+				iod->name, iod->io_typ, ret);
+	return ret;
+}
+
diff --git a/drivers/misc/modem_if/sipc4_modem.c b/drivers/misc/modem_if/sipc4_modem.c
new file mode 100644
index 0000000..1f9ee7c
--- /dev/null
+++ b/drivers/misc/modem_if/sipc4_modem.c
@@ -0,0 +1,324 @@
+/* linux/drivers/modem/modem.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/wakelock.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_variation.h"
+#include "modem_utils.h"
+
+#define FMT_WAKE_TIME   (HZ/2)
+#define RFS_WAKE_TIME   (HZ*3)
+#define RAW_WAKE_TIME   (HZ*6)
+
+static struct modem_ctl *create_modemctl_device(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct modem_data *pdata;
+	struct modem_ctl *modemctl;
+	struct device *dev = &pdev->dev;
+
+	/* create modem control device */
+	modemctl = kzalloc(sizeof(struct modem_ctl), GFP_KERNEL);
+	if (!modemctl)
+		return NULL;
+
+	modemctl->dev = dev;
+	modemctl->phone_state = STATE_OFFLINE;
+
+	pdata = pdev->dev.platform_data;
+	modemctl->mdm_data = pdata;
+	modemctl->name = pdata->name;
+
+	/* initialize link device list */
+	INIT_LIST_HEAD(&modemctl->commons.link_dev_list);
+
+	/* initialize tree of io devices */
+	modemctl->commons.iodevs_tree_chan = RB_ROOT;
+	modemctl->commons.iodevs_tree_fmt = RB_ROOT;
+
+	/* init modemctl device for getting modemctl operations */
+	ret = call_modem_init_func(modemctl, pdata);
+	if (ret) {
+		kfree(modemctl);
+		return NULL;
+	}
+
+	modemctl->use_mif_log = pdata->use_mif_log;
+	if (pdata->use_mif_log) {
+		mif_err("<%s> IPC logger can be used.\n",
+			pdata->name);
+	}
+
+	ret = mif_init_log(modemctl);
+	if (ret < 0) {
+		kfree(modemctl);
+		return NULL;
+	}
+
+	mif_info("%s is created!!!\n", pdata->name);
+	return modemctl;
+}
+
+static struct io_device *create_io_device(struct modem_io_t *io_t,
+		struct modem_ctl *modemctl, struct modem_data *pdata)
+{
+	int ret = 0;
+	struct io_device *iod = NULL;
+
+	iod = kzalloc(sizeof(struct io_device), GFP_KERNEL);
+	if (!iod) {
+		mif_err("iod == NULL\n");
+		return NULL;
+	}
+
+	rb_init_node(&iod->node_chan);
+	rb_init_node(&iod->node_fmt);
+
+	iod->name = io_t->name;
+	iod->id = io_t->id;
+	iod->format = io_t->format;
+	iod->io_typ = io_t->io_type;
+	iod->link_types = io_t->links;
+	iod->net_typ = pdata->modem_net;
+	iod->use_handover = pdata->use_handover;
+	iod->ipc_version = pdata->ipc_version;
+	atomic_set(&iod->opened, 0);
+
+	/* link between io device and modem control */
+	iod->mc = modemctl;
+	if (iod->format == IPC_FMT)
+		modemctl->iod = iod;
+	if (iod->format == IPC_BOOT) {
+		modemctl->bootd = iod;
+		mif_info("Bood device = %s\n", iod->name);
+	}
+
+	/* register misc device or net device */
+	ret = sipc4_init_io_device(iod);
+	if (ret) {
+		kfree(iod);
+		mif_err("sipc4_init_io_device fail (%d)\n", ret);
+		return NULL;
+	}
+
+	mif_debug("%s is created!!!\n", iod->name);
+	return iod;
+}
+
+static int attach_devices(struct modem_ctl *mc, struct io_device *iod,
+		enum modem_link tx_link)
+{
+	struct link_device *ld;
+
+	/* add iod to rb_tree */
+	if (iod->format != IPC_RAW)
+		insert_iod_with_format(&mc->commons, iod->format, iod);
+
+	if (sipc4_is_not_reserved_channel(iod->id))
+		insert_iod_with_channel(&mc->commons, iod->id, iod);
+
+	/* find link type for this io device */
+	list_for_each_entry(ld, &mc->commons.link_dev_list, list) {
+		if (IS_CONNECTED(iod, ld)) {
+			/* The count 1 bits of iod->link_types is count
+			 * of link devices of this iod.
+			 * If use one link device,
+			 * or, 2+ link devices and this link is tx_link,
+			 * set iod's link device with ld
+			 */
+			if ((countbits(iod->link_types) <= 1) ||
+					(tx_link == ld->link_type)) {
+				mif_debug("set %s->%s\n", iod->name, ld->name);
+
+				set_current_link(iod, ld);
+
+				if (iod->ipc_version == SIPC_VER_42) {
+					if (iod->format == IPC_FMT) {
+						int ch = iod->id & 0x03;
+						ld->fmt_iods[ch] = iod;
+					}
+				}
+			}
+		}
+	}
+
+	/* if use rx dynamic switch, set tx_link at modem_io_t of
+	 * board-*-modems.c
+	 */
+	if (!get_current_link(iod)) {
+		mif_err("%s->link == NULL\n", iod->name);
+		BUG();
+	}
+
+	switch (iod->format) {
+	case IPC_FMT:
+		wake_lock_init(&iod->wakelock, WAKE_LOCK_SUSPEND, iod->name);
+		iod->waketime = FMT_WAKE_TIME;
+		break;
+
+	case IPC_RFS:
+		wake_lock_init(&iod->wakelock, WAKE_LOCK_SUSPEND, iod->name);
+		iod->waketime = RFS_WAKE_TIME;
+		break;
+
+	case IPC_MULTI_RAW:
+		wake_lock_init(&iod->wakelock, WAKE_LOCK_SUSPEND, iod->name);
+		iod->waketime = RAW_WAKE_TIME;
+		break;
+	case IPC_BOOT:
+		wake_lock_init(&iod->wakelock, WAKE_LOCK_SUSPEND, iod->name);
+		iod->waketime = 3 * HZ;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int __devinit modem_probe(struct platform_device *pdev)
+{
+	int i;
+	struct modem_data *pdata = pdev->dev.platform_data;
+	struct modem_ctl *modemctl;
+	struct io_device *iod[pdata->num_iodevs];
+	struct link_device *ld;
+
+	mif_err("%s\n", pdev->name);
+	memset(iod, 0, sizeof(iod));
+
+	modemctl = create_modemctl_device(pdev);
+	if (!modemctl) {
+		mif_err("modemctl == NULL\n");
+		goto err_free_modemctl;
+	}
+
+	/* create link device */
+	/* support multi-link device */
+	for (i = 0; i < LINKDEV_MAX ; i++) {
+		/* find matching link type */
+		if (pdata->link_types & LINKTYPE(i)) {
+			ld = call_link_init_func(pdev, i);
+			if (!ld)
+				goto err_free_modemctl;
+
+			mif_err("link created: %s\n", ld->name);
+			ld->link_type = i;
+			ld->mc = modemctl;
+			list_add(&ld->list, &modemctl->commons.link_dev_list);
+		}
+	}
+
+	/* create io deivces and connect to modemctl device */
+	for (i = 0; i < pdata->num_iodevs; i++) {
+		iod[i] = create_io_device(&pdata->iodevs[i], modemctl, pdata);
+		if (!iod[i]) {
+			mif_err("iod[%d] == NULL\n", i);
+			goto err_free_modemctl;
+		}
+
+		attach_devices(modemctl, iod[i],
+				pdata->iodevs[i].tx_link);
+	}
+
+	platform_set_drvdata(pdev, modemctl);
+
+	mif_info("Complete!!!\n");
+	return 0;
+
+err_free_modemctl:
+	for (i = 0; i < pdata->num_iodevs; i++)
+		if (iod[i] != NULL)
+			kfree(iod[i]);
+
+	if (modemctl != NULL)
+		kfree(modemctl);
+
+	return -ENOMEM;
+}
+
+static void modem_shutdown(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct modem_ctl *mc = dev_get_drvdata(dev);
+	mc->ops.modem_off(mc);
+	mc->phone_state = STATE_OFFLINE;
+}
+
+static int modem_suspend(struct device *pdev)
+{
+#ifndef CONFIG_LINK_DEVICE_HSIC
+	struct modem_ctl *mc = dev_get_drvdata(pdev);
+
+	if (mc->gpio_pda_active)
+		gpio_set_value(mc->gpio_pda_active, 0);
+#endif
+
+	return 0;
+}
+
+static int modem_resume(struct device *pdev)
+{
+#ifndef CONFIG_LINK_DEVICE_HSIC
+	struct modem_ctl *mc = dev_get_drvdata(pdev);
+
+	if (mc->gpio_pda_active)
+		gpio_set_value(mc->gpio_pda_active, 1);
+#endif
+
+	return 0;
+}
+
+static const struct dev_pm_ops modem_pm_ops = {
+	.suspend    = modem_suspend,
+	.resume     = modem_resume,
+};
+
+static struct platform_driver modem_driver = {
+	.probe = modem_probe,
+	.shutdown = modem_shutdown,
+	.driver = {
+		.name = "modem_if",
+		.pm   = &modem_pm_ops,
+	},
+};
+
+static int __init modem_init(void)
+{
+	return platform_driver_register(&modem_driver);
+}
+
+module_init(modem_init);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Samsung Modem Interface Driver");
diff --git a/drivers/misc/modem_if/sipc5_io_device.c b/drivers/misc/modem_if/sipc5_io_device.c
new file mode 100644
index 0000000..05578f4
--- /dev/null
+++ b/drivers/misc/modem_if/sipc5_io_device.c
@@ -0,0 +1,1402 @@
+/* /linux/drivers/misc/modem_if/sipc5_io_device.c
+ *
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+#include <linux/sched.h>
+#include <linux/fs.h>
+#include <linux/poll.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/if_arp.h>
+#include <linux/ip.h>
+#include <linux/if_ether.h>
+#include <linux/etherdevice.h>
+#include <linux/device.h>
+
+#include <linux/platform_data/modem.h>
+#ifdef CONFIG_LINK_DEVICE_C2C
+#include <linux/platform_data/c2c.h>
+#endif
+#include "modem_prj.h"
+#include "modem_utils.h"
+
+static ssize_t show_waketime(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	unsigned int msec;
+	char *p = buf;
+	struct miscdevice *miscdev = dev_get_drvdata(dev);
+	struct io_device *iod = container_of(miscdev, struct io_device,
+			miscdev);
+
+	msec = jiffies_to_msecs(iod->waketime);
+
+	p += sprintf(buf, "raw waketime : %ums\n", msec);
+
+	return p - buf;
+}
+
+static ssize_t store_waketime(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned long msec;
+	int ret;
+	struct miscdevice *miscdev = dev_get_drvdata(dev);
+	struct io_device *iod = container_of(miscdev, struct io_device,
+			miscdev);
+
+	ret = strict_strtoul(buf, 10, &msec);
+	if (ret)
+		return count;
+
+	iod->waketime = msecs_to_jiffies(msec);
+
+	return count;
+}
+
+static struct device_attribute attr_waketime =
+	__ATTR(waketime, S_IRUGO | S_IWUSR, show_waketime, store_waketime);
+
+static inline int sipc5_check_frame_cfg(u8 *buff, struct sipc5_frame_data *frm)
+{
+	u8 config = buff[0];
+
+	if ((config & SIPC5_START_MASK) != SIPC5_START_MASK)
+		return -EBADMSG;
+
+	frm->config = config;
+	frm->hdr_len = SIPC5_MIN_HEADER_SIZE;
+
+	if (config & SIPC5_PADDING_EXIST)
+		frm->padding = true;
+
+	if (unlikely(config & SIPC5_EXT_FIELD_EXIST)) {
+		frm->ext_fld = true;
+		if (config & SIPC5_CTL_FIELD_EXIST) {
+			frm->ctl_fld = true;
+			frm->hdr_len = SIPC5_HEADER_SIZE_WITH_CTL_FLD;
+		} else {
+			frm->ext_len = true;
+			frm->hdr_len = SIPC5_HEADER_SIZE_WITH_EXT_LEN;
+		}
+	}
+
+	return SIPC5_CONFIG_SIZE;
+}
+
+static inline void sipc5_build_rx_frame_data(struct link_device *ld,
+		struct sipc5_frame_data *frm)
+{
+	u16 *sz16 = (u16 *)(frm->hdr + SIPC5_LEN_OFFSET);
+	u32 *sz32 = (u32 *)(frm->hdr + SIPC5_LEN_OFFSET);
+
+	frm->ch_id = frm->hdr[SIPC5_CH_ID_OFFSET];
+	frm->len = *sz16;
+
+	if (unlikely(frm->ext_fld)) {
+		if (frm->ctl_fld)
+			frm->control = frm->hdr[SIPC5_CTL_OFFSET];
+		else
+			frm->len = *sz32;
+	}
+
+	frm->data_len = frm->len - frm->hdr_len;
+
+	mif_debug("%s: FRM ch:%d len:%d ctl:%02X data.len:%d\n",
+		ld->name, frm->ch_id, frm->len, frm->control, frm->data_len);
+}
+
+static inline struct sk_buff *sipc5_prepare_rx_skb(struct io_device *iod,
+		struct link_device *ld, unsigned len)
+{
+	struct sk_buff *skb;
+
+	if (iod->format == IPC_MULTI_RAW && iod->use_handover) {
+		int alloc = len + sizeof(struct ethhdr);
+		skb = rx_alloc_skb(alloc, GFP_ATOMIC, iod, ld);
+		if (unlikely(!skb))
+			return NULL;
+		skb_reserve(skb, sizeof(struct ethhdr));
+	} else {
+		skb = rx_alloc_skb(len, GFP_ATOMIC, iod, ld);
+	}
+
+	return skb;
+}
+
+/* Check and store link layer header, then alloc an skb */
+static int sipc5_recv_header(struct io_device *iod, struct link_device *ld,
+		u8 *buff, unsigned size, struct sipc5_frame_data *frm)
+{
+	int len = 0;
+
+	mif_debug("%s: size %d\n", ld->name, size);
+
+	if (likely(!frm->config)) {
+		len = sipc5_check_frame_cfg(buff, frm);
+		if (len < 0) {
+			mif_info("%s: ERR! wrong start (0x%02x)\n",
+				ld->name, buff[0]);
+			return len;
+		}
+
+		/* Copy the link layer header to the header buffer */
+		len = min(frm->hdr_len, size);
+		memcpy(frm->hdr, buff, len);
+	} else {
+		/* Copy the link layer header to the header buffer */
+		len = min((frm->hdr_len - frm->hdr_rcvd), size);
+		memcpy((frm->hdr + frm->hdr_rcvd), buff, len);
+	}
+
+	frm->hdr_rcvd += len;
+
+	mif_debug("%s: FRM hdr.len:%d hdr.rcvd:%d\n",
+		ld->name, frm->hdr_len, frm->hdr_rcvd);
+
+	if (frm->hdr_rcvd >= frm->hdr_len) {
+		struct sk_buff *skb;
+		sipc5_build_rx_frame_data(ld, frm);
+		skb = sipc5_prepare_rx_skb(iod, ld, frm->data_len);
+		fragdata(iod, ld)->skb_recv = skb;
+	}
+
+	return len;
+}
+
+/* copy data to skb */
+static int sipc5_recv_payload(struct io_device *iod, struct link_device *ld,
+		u8 *buff, unsigned size, struct sipc5_frame_data *frm)
+{
+	struct sk_buff *skb = fragdata(iod, ld)->skb_recv;
+	unsigned rest = frm->data_len - frm->data_rcvd;
+	unsigned len;
+
+	/*
+	** rest == frm->data_len - frm->data_rcvd == tailroom of skb or mifb
+	*/
+	rest = frm->data_len - frm->data_rcvd;
+	mif_debug("%s: FRM data.len:%d data.rcvd:%d rest:%d size:%d\n",
+		ld->name, frm->data_len, frm->data_rcvd, rest, size);
+
+	/* If there is no skb, data must be dropped. */
+	len = min(rest, size);
+	if (skb)
+		memcpy(skb_put(skb, len), buff, len);
+
+	frm->data_rcvd += len;
+
+	mif_debug("%s: FRM data.len:%d data.rcvd:%d\n",
+		ld->name, frm->data_len, frm->data_rcvd);
+
+	return len;
+}
+
+static int sipc5_recv_fmt(struct sk_buff *skb, struct sipc5_frame_data *frm)
+{
+	struct io_device *iod = skbpriv(skb)->iod;
+	struct link_device *ld = skbpriv(skb)->ld;
+	struct sk_buff_head *rxq;
+	struct sipc_fmt_hdr *fh;
+	struct sk_buff *rx_skb;
+	unsigned id;
+
+	rxq = &iod->sk_rx_q;
+	if (!rxq) {
+		mif_debug("%s: no sk_rx_q\n", iod->name);
+		return -EINVAL;
+	}
+
+	id = frm->control & 0x7F;
+
+	if (iod->skb[id] == NULL) {
+		/*
+		** There has been no multiple frame with this ID.
+		*/
+		if ((frm->control & 0x80) == 0) {
+			/*
+			** It is a single frame because the "more" bit is 0.
+			*/
+			skb_queue_tail(rxq, skb);
+			if (unlikely(rxq->qlen > 2048)) {
+				struct sk_buff *victim;
+				mif_info("%s: WARNING! rxq->qlen %d > 2048\n",
+					iod->name, rxq->qlen);
+				victim = skb_dequeue(rxq);
+				dev_kfree_skb_any(victim);
+			} else {
+				mif_debug("%s: rxq->qlen = %d\n",
+					iod->name, rxq->qlen);
+			}
+
+			wake_up(&iod->wq);
+			return 0;
+		}
+
+		/*
+		** The start of multiple frames
+		*/
+		fh = (struct sipc_fmt_hdr *)skb->data;
+		mif_debug("%s: start multi-frame (ID:%d len:%d)\n",
+			iod->name, id, fh->len);
+
+		rx_skb = rx_alloc_skb(fh->len, GFP_ATOMIC, iod, ld);
+		if (!rx_skb) {
+			mif_info("%s: ERR! rx_alloc_skb fail\n", iod->name);
+			return -ENOMEM;
+		}
+
+		iod->skb[id] = rx_skb;
+	} else {
+		rx_skb = iod->skb[id];
+	}
+
+	/*
+	** Start multi-frame processing
+	*/
+	memcpy(skb_put(rx_skb, skb->len), skb->data, skb->len);
+	dev_kfree_skb_any(skb);
+
+	if (frm->control & 0x80) {
+		/* The last frame has not arrived yet. */
+		mif_debug("%s: recv multi-frame (ID:%d rcvd:%d)\n",
+			iod->name, id, rx_skb->len);
+	} else {
+		/* It is the last frame because the "more" bit is 0. */
+		mif_debug("%s: end multi-frame (ID:%d rcvd:%d)\n",
+			iod->name, id, rx_skb->len);
+		skb_queue_tail(rxq, rx_skb);
+		if (unlikely(rxq->qlen > 2048)) {
+			struct sk_buff *victim;
+			mif_info("%s: WARNING! rxq->qlen %d > 2048\n",
+				iod->name, rxq->qlen);
+			victim = skb_dequeue(rxq);
+			dev_kfree_skb_any(victim);
+		} else {
+			mif_debug("%s: rxq->qlen = %d\n", iod->name, rxq->qlen);
+		}
+
+		iod->skb[id] = NULL;
+		wake_up(&iod->wq);
+	}
+
+	return 0;
+}
+
+static int sipc5_recv_rfs(struct sk_buff *skb)
+{
+	struct io_device *iod = skbpriv(skb)->iod;
+	struct sk_buff_head *rxq = &iod->sk_rx_q;
+
+	skb_queue_tail(rxq, skb);
+	if (unlikely(rxq->qlen > 2048)) {
+		struct sk_buff *victim;
+		mif_debug("%s: ERR! rxq->qlen %d > 2048\n",
+			iod->name, rxq->qlen);
+		victim = skb_dequeue(rxq);
+		dev_kfree_skb_any(victim);
+	} else {
+		mif_debug("%s: rxq->qlen %d\n", iod->name, rxq->qlen);
+	}
+
+	wake_up(&iod->wq);
+
+	return 0;
+}
+
+static int sipc5_recv_misc(struct sk_buff *skb)
+{
+	struct io_device *iod = skbpriv(skb)->iod; /* same with real_iod */
+	struct sk_buff_head *rxq = &iod->sk_rx_q;
+
+	skb_queue_tail(rxq, skb);
+	if (unlikely(rxq->qlen > 2048)) {
+		struct sk_buff *victim;
+		mif_debug("%s: ERR! rxq->qlen %d > 2048\n",
+			iod->name, rxq->qlen);
+		victim = skb_dequeue(rxq);
+		dev_kfree_skb_any(victim);
+	} else {
+		mif_debug("%s: rxq->qlen %d\n", iod->name, rxq->qlen);
+	}
+
+	wake_up(&iod->wq);
+
+	return 0;
+}
+
+static int sipc5_recv_pdp(struct sk_buff *skb)
+{
+	struct io_device *iod = skbpriv(skb)->iod; /* same with real_iod */
+	struct net_device *ndev;
+	struct iphdr *iphdr;
+	struct ethhdr *ehdr;
+	int ret;
+	const char source[ETH_ALEN] = SOURCE_MAC_ADDR;
+
+	ndev = iod->ndev;
+	if (!ndev) {
+		mif_info("%s: ERR! no iod->ndev\n", iod->name);
+		return -ENODEV;
+	}
+
+	skb->dev = ndev;
+	ndev->stats.rx_packets++;
+	ndev->stats.rx_bytes += skb->len;
+
+	/* check the version of IP */
+	iphdr = (struct iphdr *)skb->data;
+	if (iphdr->version == IP6VERSION)
+		skb->protocol = htons(ETH_P_IPV6);
+	else
+		skb->protocol = htons(ETH_P_IP);
+
+	if (iod->use_handover) {
+		skb_push(skb, sizeof(struct ethhdr));
+		ehdr = (void *)skb->data;
+		memcpy(ehdr->h_dest, ndev->dev_addr, ETH_ALEN);
+		memcpy(ehdr->h_source, source, ETH_ALEN);
+		ehdr->h_proto = skb->protocol;
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+		skb_reset_mac_header(skb);
+
+		skb_pull(skb, sizeof(struct ethhdr));
+	}
+
+	if (in_interrupt())
+		ret = netif_rx(skb);
+	else
+		ret = netif_rx_ni(skb);
+
+	if (ret != NET_RX_SUCCESS)
+		mif_info("%s: ERR! netif_rx fail (err %d)\n", iod->name, ret);
+
+	return ret;
+}
+
+/** rx_iodev_work - rx workqueue for raw data
+ *
+ * @work: workqueue
+ *
+ * If you throw packets to Network layer directly in interrupt context,
+ * sometimes, you'll meet backlog buffer full of Network layer.
+ * Applications need some time to get packets from Network layer.
+ * And, we need to retry logic when NET_RX_DROP occured. this work ensure
+ * retry when netif_rx failed.
+ */
+static void rx_iodev_work(struct work_struct *work)
+{
+	int ret = 0;
+	struct sk_buff *skb = NULL;
+	struct io_device *iod = container_of(work, struct io_device,
+				rx_work.work);
+
+	while ((skb = skb_dequeue(&iod->sk_rx_q)) != NULL) {
+		ret = sipc5_recv_pdp(skb);
+		if (ret < 0) {
+			mif_err("%s: sipc5_recv_pdp fail (err %d)",
+				iod->name, ret);
+			dev_kfree_skb_any(skb);
+		} else if (ret == NET_RX_DROP) {
+			mif_err("%s: ret == NET_RX_DROP. retry later.\n",
+					iod->name);
+			schedule_delayed_work(&iod->rx_work,
+						msecs_to_jiffies(100));
+			return;
+		}
+	}
+}
+
+static int rx_multipdp(struct sk_buff *skb)
+{
+	/* in sipc5, this iod == real_iod. not MULTIPDP's iod */
+	struct io_device *iod = skbpriv(skb)->iod;
+
+	if (iod->io_typ != IODEV_NET) {
+		mif_info("%s: ERR! wrong io_type %d\n", iod->name, iod->io_typ);
+		return -EINVAL;
+	}
+
+	skb_queue_tail(&iod->sk_rx_q, skb);
+	mif_debug("%s: sk_rx_qlen %d\n", iod->name, iod->sk_rx_q.qlen);
+
+	schedule_delayed_work(&iod->rx_work, 0);
+	return 0;
+}
+
+static int sipc5_recv_demux(struct link_device *ld, struct sk_buff *skb,
+		struct sipc5_frame_data *frm)
+{
+	struct io_device *iod = NULL;
+
+	if (unlikely(frm->ch_id >= SIPC5_CH_ID_MAX || frm->ch_id == 0)) {
+		mif_info("%s: ERR! invalid channel %d\n", ld->name, frm->ch_id);
+		return -ENODEV;
+	}
+
+	iod = link_get_iod_with_channel(ld, frm->ch_id);
+	if (unlikely(!iod)) {
+		mif_info("%s: ERR! no iod for ch# %d\n", ld->name, frm->ch_id);
+		return -ENODEV;
+	}
+
+	skbpriv(skb)->ld = ld;
+	skbpriv(skb)->iod = iod;
+	skbpriv(skb)->real_iod = iod;
+
+	if (atomic_read(&iod->opened) <= 0) {
+		mif_info("%s: ERR! %s is not opened\n", ld->name, iod->name);
+		return -ENODEV;
+	}
+
+	if (frm->ch_id >= SIPC5_CH_ID_RFS_0)
+		return sipc5_recv_rfs(skb);
+	else if (frm->ch_id >= SIPC5_CH_ID_FMT_0)
+		return sipc5_recv_fmt(skb, frm);
+	else if (iod->io_typ == IODEV_MISC)
+		return sipc5_recv_misc(skb);
+	else
+		return rx_multipdp(skb);
+}
+
+static int sipc5_recv_ipc_from_serial(struct io_device *iod,
+		struct link_device *ld, const char *data, unsigned size)
+{
+	struct sipc5_frame_data *frm = &fragdata(iod, ld)->f_data;
+	struct sk_buff *skb;
+	u8 *buff = (u8 *)data;
+	int rest = (int)size;
+	int err = 0;
+	int done = 0;
+
+	mif_debug("%s: size = %d\n", ld->name, size);
+
+	if (frm->hdr_rcvd >= frm->hdr_len && frm->data_rcvd < frm->data_len) {
+		/*
+		  There may be an skb or mifb (fragdata(iod, ld)->skb_recv) that
+		  is waiting for more IPC frame. In this case, sipc5_recv_header
+		  function must be skipped.
+		*/
+		mif_debug("%s: FRM data.len:%d data.rcvd:%d -> recv_data\n",
+			ld->name, frm->data_len, frm->data_rcvd);
+		goto recv_data;
+	}
+
+next_frame:
+	/* Receive and analyze header, then prepare an akb */
+	err = done = sipc5_recv_header(iod, ld, buff, rest, frm);
+	if (err < 0)
+		goto err_exit;
+
+	buff += done;
+	rest -= done;
+	mif_debug("%s: sipc5_recv_header() -> done:%d rest:%d\n",
+		ld->name, done, rest);
+	if (rest < 0)
+		goto err_range;
+
+	if (rest == 0)
+		return size;
+
+recv_data:
+	err = 0;
+
+	mif_debug("%s: done:%d rest:%d -> recv_payload()\n",
+		ld->name, done, rest);
+
+	done = sipc5_recv_payload(iod, ld, buff, rest, frm);
+	buff += done;
+	rest -= done;
+
+	mif_debug("%s: recv_payload() -> done:%d rest:%d\n",
+		ld->name, done, rest);
+
+	if (rest == 0 && frm->data_rcvd < frm->data_len) {
+		/*
+		  Data is being received and more data will come within the next
+		  frame from the link device.
+		*/
+		return size;
+	}
+
+	/* At this point, one complete link layer frame has been received. */
+
+	/* A padding size is applied to access the next IPC frame. */
+	if (frm->padding) {
+		done = sipc5_calc_padding_size(frm->len);
+		if (done > rest) {
+			mif_info("%s: ERR! padding %d > rest %d\n",
+				ld->name, done, rest);
+			goto err_exit;
+		}
+
+		buff += done;
+		rest -= done;
+
+		mif_debug("%s: padding:%d -> rest:%d\n", ld->name, done, rest);
+
+		if (rest < 0)
+			goto err_range;
+
+	}
+
+	skb = fragdata(iod, ld)->skb_recv;
+	if (likely(skb)) {
+		mif_debug("%s: len %d -> recv_demux()\n", ld->name, skb->len);
+		err = sipc5_recv_demux(ld, skb, frm);
+		if (err < 0)
+			dev_kfree_skb_any(skb);
+	} else {
+		mif_debug("%s: len:%d -> drop\n", ld->name, skb->len);
+	}
+
+	/* initialize the skb_recv and the frame_data buffer */
+	fragdata(iod, ld)->skb_recv = NULL;
+	memset(frm, 0, sizeof(struct sipc5_frame_data));
+
+	if (rest > 0)
+		goto next_frame;
+
+	if (rest <= 0)
+		return size;
+
+err_exit:
+	if (fragdata(iod, ld)->skb_recv &&
+	    frm->hdr_rcvd >= frm->hdr_len && frm->data_rcvd >= frm->data_len) {
+		dev_kfree_skb_any(fragdata(iod, ld)->skb_recv);
+		memset(frm, 0, sizeof(struct sipc5_frame_data));
+		fragdata(iod, ld)->skb_recv = NULL;
+		mif_info("%s: ERR! clear frag\n", ld->name);
+	}
+	return err;
+
+err_range:
+	mif_info("%s: ERR! size:%d vs. rest:%d\n", ld->name, size, rest);
+	return size;
+}
+
+/* Check and store link layer header */
+static int sipc5_recv_header_from_dpram(struct link_device *ld, u8 *buff,
+		struct sipc5_frame_data *frm)
+{
+	int len = sipc5_check_frame_cfg(buff, frm);
+
+	if (len < 0) {
+		mif_info("%s: ERR! wrong start 0x%02x\n",
+			ld->name, buff[0]);
+		return len;
+	} else if (len > SIPC5_MAX_HEADER_SIZE) {
+		mif_info("%s: ERR! wrong header length %d\n",
+			ld->name, len);
+		return -EBADMSG;
+	}
+
+	/* Copy the link layer header to the header buffer */
+	len = frm->hdr_len;
+	memcpy(frm->hdr, buff, len);
+	frm->hdr_rcvd = frm->hdr_len;
+
+	sipc5_build_rx_frame_data(ld, frm);
+
+	return len;
+}
+
+/* copy data to skb */
+static int sipc5_recv_payload_from_dpram(struct sk_buff *skb, u8 *buff,
+		unsigned len)
+{
+	/* If there is no skb, data must be dropped. */
+	if (skb)
+		memcpy(skb_put(skb, len), buff, len);
+	return len;
+}
+
+static int sipc5_recv_ipc_from_dpram(struct io_device *iod,
+		struct link_device *ld, const char *data, unsigned size)
+{
+	struct sipc5_frame_data *frm = &fragdata(iod, ld)->f_data;
+	struct sk_buff *skb;
+	u8 *buff = (u8 *)data;
+	int rest = (int)size;
+	int len;
+	int done;
+
+	mif_debug("%s: size = %d\n", ld->name, size);
+
+	while (rest > 0) {
+		/* Initialize the frame data buffer */
+		memset(frm, 0, sizeof(struct sipc5_frame_data));
+
+		/* Receive and analyze link layer header */
+		done = sipc5_recv_header_from_dpram(ld, buff, frm);
+		if (unlikely(done < 0))
+			return -EBADMSG;
+
+		rest -= done;
+		if (rest < 0)
+			return -ERANGE;
+		buff += done;
+
+		/* Prepare an akb */
+		len = frm->data_len;
+		skb = sipc5_prepare_rx_skb(iod, ld, len);
+
+		/* Receive payload */
+		mif_debug("%s: done:%d rest:%d len:%d -> recv_payload()\n",
+			ld->name, done, rest, len);
+		done = sipc5_recv_payload_from_dpram(skb, buff, len);
+		rest -= done;
+		if (rest < 0)
+			return -ERANGE;
+		buff += done;
+
+		/* A padding size is applied to access the next IPC frame. */
+		if (frm->padding) {
+			done = sipc5_calc_padding_size(frm->len);
+			rest -= done;
+			if (rest < 0)
+				return -ERANGE;
+			buff += done;
+		}
+
+		if (likely(skb)) {
+			mif_debug("%s: len:%d -> demux\n", ld->name, skb->len);
+			if (sipc5_recv_demux(ld, skb, frm) < 0)
+				dev_kfree_skb_any(skb);
+		} else {
+			mif_debug("%s: len:%d -> drop\n", ld->name, skb->len);
+		}
+	}
+
+	return 0;
+}
+
+/* called from link device when a packet arrives for this io device */
+static int io_dev_recv_data_from_link_dev(struct io_device *iod,
+		struct link_device *ld, const char *data, unsigned int len)
+{
+	struct sk_buff_head *rxq = &iod->sk_rx_q;
+	struct sk_buff *skb;
+	int err;
+
+	if (!ld) {
+		mif_info("ERR: !ld\n");
+		return -EINVAL;
+	}
+
+	if (!iod) {
+		mif_info("%s: ERR! !iod\n", ld->name);
+		return -EINVAL;
+	}
+
+	if (!data) {
+		mif_info("%s: ERR! !data\n", ld->name);
+		return -EINVAL;
+	}
+
+	if (len <= 0) {
+		mif_info("%s: ERR! len = %d <= 0\n",
+			ld->name, len);
+		return -EINVAL;
+	}
+
+	switch (iod->format) {
+	case IPC_FMT:
+	case IPC_RAW:
+	case IPC_RFS:
+	case IPC_MULTI_RAW:
+		if (iod->waketime)
+			wake_lock_timeout(&iod->wakelock, iod->waketime);
+
+		if (ld->link_type == LINKDEV_DPRAM && ld->aligned)
+			err = sipc5_recv_ipc_from_dpram(iod, ld, data, len);
+		else
+			err = sipc5_recv_ipc_from_serial(iod, ld, data, len);
+
+		if (err < 0)
+			mif_info("%s: ERR! sipc5_recv_ipc_from_link fail "
+				"(err %d)\n", ld->name, err);
+
+		return err;
+
+	case IPC_CMD:
+	case IPC_BOOT:
+	case IPC_RAMDUMP:
+		/* save packet to sk_buff */
+		skb = rx_alloc_skb(len, GFP_ATOMIC, iod, ld);
+		if (!skb) {
+			mif_info("%s: ERR! rx_alloc_skb fail\n", ld->name);
+			return -ENOMEM;
+		}
+
+		mif_debug("%s: len:%d -> iod:%s\n", ld->name, len, iod->name);
+
+		memcpy(skb_put(skb, len), data, len);
+		skb_queue_tail(rxq, skb);
+		if (unlikely(rxq->qlen > 2048)) {
+			struct sk_buff *victim;
+			mif_info("%s: ERR! rxq->qlen %d > 2048\n",
+				iod->name, rxq->qlen);
+			victim = skb_dequeue(rxq);
+			dev_kfree_skb_any(victim);
+		}
+		wake_up(&iod->wq);
+
+		return len;
+
+	default:
+		mif_info("%s: ERR! unknown format %d\n", ld->name, iod->format);
+		return -EINVAL;
+	}
+}
+
+static unsigned sipc5_build_tx_link_header(struct sipc5_frame_data *frm,
+		struct io_device *iod, struct link_device *ld, ssize_t count)
+{
+	u8 *buff = frm->hdr;
+	u16 *sz16 = (u16 *)(buff + SIPC5_LEN_OFFSET);
+	u32 *sz32 = (u32 *)(buff + SIPC5_LEN_OFFSET);
+
+	memset(frm, 0, sizeof(struct sipc5_frame_data));
+
+	if (iod->format == IPC_CMD ||
+	    iod->format == IPC_BOOT ||
+	    iod->format == IPC_RAMDUMP) {
+		frm->len = count;
+		return 0;
+	}
+
+	frm->config = SIPC5_START_MASK;
+
+	if (iod->format == IPC_FMT && count > 2048) {
+		frm->ext_fld = true;
+		frm->ctl_fld = true;
+
+		frm->config |= SIPC5_EXT_FIELD_EXIST;
+		frm->config |= SIPC5_CTL_FIELD_EXIST;
+	}
+
+	if (iod->id >= SIPC5_CH_ID_RFS_0 && count > 0xFFFF) {
+		frm->ext_fld = true;
+		frm->ext_len = true;
+
+		frm->config |= SIPC5_EXT_FIELD_EXIST;
+	}
+
+	if (ld->aligned)
+		frm->config |= SIPC5_PADDING_EXIST;
+
+	frm->ch_id = iod->id;
+
+	frm->hdr_len = sipc5_get_hdr_size(frm->config);
+	frm->data_len = count;
+	frm->len = frm->hdr_len + frm->data_len;
+
+	buff[SIPC5_CONFIG_OFFSET] = frm->config;
+	buff[SIPC5_CH_ID_OFFSET] = frm->ch_id;
+
+	if (frm->ext_fld) {
+		if (frm->ctl_fld) {
+			*sz16 = (u16)frm->len;
+			buff[SIPC5_CTL_OFFSET] = frm->control;
+		} else {
+			*sz32 = (u32)frm->len;
+		}
+	} else {
+		*sz16 = (u16)frm->len;
+	}
+
+	return frm->hdr_len;
+}
+
+/* inform the IO device that the modem is now online or offline or
+ * crashing or whatever...
+ */
+static void io_dev_modem_state_changed(struct io_device *iod,
+			enum modem_state state)
+{
+	mif_info("%s: %s state changed (state %d)\n",
+		iod->name, iod->mc->name, state);
+
+	iod->mc->phone_state = state;
+
+	if (state == STATE_CRASH_RESET || state == STATE_CRASH_EXIT ||
+	    state == STATE_NV_REBUILDING)
+		wake_up(&iod->wq);
+}
+
+/**
+ * io_dev_sim_state_changed
+ * @iod:	IPC's io_device
+ * @sim_online: SIM is online?
+ */
+static void io_dev_sim_state_changed(struct io_device *iod, bool sim_online)
+{
+	if (atomic_read(&iod->opened) == 0) {
+		mif_info("%s: ERR! not opened\n", iod->name);
+	} else if (iod->mc->sim_state.online == sim_online) {
+		mif_info("%s: SIM state not changed\n", iod->name);
+	} else {
+		iod->mc->sim_state.online = sim_online;
+		iod->mc->sim_state.changed = true;
+		mif_info("%s: SIM state changed {online %d, changed %d}\n",
+			iod->name, iod->mc->sim_state.online,
+			iod->mc->sim_state.changed);
+		wake_up(&iod->wq);
+	}
+}
+
+static int misc_open(struct inode *inode, struct file *filp)
+{
+	struct io_device *iod = to_io_device(filp->private_data);
+	struct mif_common *commons = &iod->mc->commons;
+	struct link_device *ld;
+	int ret;
+	filp->private_data = (void *)iod;
+
+	mif_info("%s\n", iod->name);
+	atomic_inc(&iod->opened);
+
+	list_for_each_entry(ld, &commons->link_dev_list, list) {
+		if (IS_CONNECTED(iod, ld) && ld->init_comm) {
+			ret = ld->init_comm(ld, iod);
+			if (ret < 0) {
+				mif_info("%s: init_comm fail(%d)\n",
+					ld->name, ret);
+				return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int misc_release(struct inode *inode, struct file *filp)
+{
+	struct io_device *iod = (struct io_device *)filp->private_data;
+	struct mif_common *commons = &iod->mc->commons;
+	struct link_device *ld;
+
+	mif_info("%s\n", iod->name);
+	atomic_dec(&iod->opened);
+	skb_queue_purge(&iod->sk_rx_q);
+
+	list_for_each_entry(ld, &commons->link_dev_list, list) {
+		if (IS_CONNECTED(iod, ld) && ld->terminate_comm)
+			ld->terminate_comm(ld, iod);
+	}
+
+	return 0;
+}
+
+static unsigned int misc_poll(struct file *filp, struct poll_table_struct *wait)
+{
+	struct io_device *iod = (struct io_device *)filp->private_data;
+
+	poll_wait(filp, &iod->wq, wait);
+
+	if (!skb_queue_empty(&iod->sk_rx_q) &&
+	    iod->mc->phone_state != STATE_OFFLINE) {
+		return POLLIN | POLLRDNORM;
+	} else if ((iod->mc->phone_state == STATE_CRASH_RESET) ||
+		   (iod->mc->phone_state == STATE_CRASH_EXIT) ||
+		   (iod->mc->phone_state == STATE_NV_REBUILDING) ||
+		   (iod->mc->sim_state.changed)) {
+		if (iod->format == IPC_RAW) {
+			msleep(20);
+			return 0;
+		}
+		return POLLHUP;
+	} else {
+		return 0;
+	}
+}
+
+static long misc_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	int p_state;
+	struct io_device *iod = (struct io_device *)filp->private_data;
+	struct link_device *ld = get_current_link(iod);
+	char cpinfo_buf[530] = "CP Crash ";
+
+	switch (cmd) {
+	case IOCTL_MODEM_ON:
+		mif_info("%s: IOCTL_MODEM_ON\n", iod->name);
+		return iod->mc->ops.modem_on(iod->mc);
+
+	case IOCTL_MODEM_OFF:
+		mif_info("%s: IOCTL_MODEM_OFF\n", iod->name);
+		return iod->mc->ops.modem_off(iod->mc);
+
+	case IOCTL_MODEM_RESET:
+		mif_info("%s: IOCTL_MODEM_RESET\n", iod->name);
+		return iod->mc->ops.modem_reset(iod->mc);
+
+	case IOCTL_MODEM_BOOT_ON:
+		mif_info("%s: IOCTL_MODEM_BOOT_ON\n", iod->name);
+		return iod->mc->ops.modem_boot_on(iod->mc);
+
+	case IOCTL_MODEM_BOOT_OFF:
+		mif_info("%s: IOCTL_MODEM_BOOT_OFF\n", iod->name);
+		return iod->mc->ops.modem_boot_off(iod->mc);
+
+	case IOCTL_MODEM_START:
+		mif_info("%s: IOCTL_MODEM_START\n", iod->name);
+		return 0;
+
+	case IOCTL_MODEM_STATUS:
+		mif_debug("%s: IOCTL_MODEM_STATUS\n", iod->name);
+
+		p_state = iod->mc->phone_state;
+		if ((p_state == STATE_CRASH_RESET) ||
+		    (p_state == STATE_CRASH_EXIT)) {
+			mif_info("%s: IOCTL_MODEM_STATUS (state %d)\n",
+				iod->name, p_state);
+		} else if (iod->mc->sim_state.changed) {
+			int s_state = iod->mc->sim_state.online ?
+					STATE_SIM_ATTACH : STATE_SIM_DETACH;
+			iod->mc->sim_state.changed = false;
+			return s_state;
+		} else if (p_state == STATE_NV_REBUILDING) {
+			mif_info("%s: IOCTL_MODEM_STATUS (state %d)\n",
+				iod->name, p_state);
+			iod->mc->phone_state = STATE_ONLINE;
+		}
+		return p_state;
+
+	case IOCTL_MODEM_PROTOCOL_SUSPEND:
+		mif_debug("%s: IOCTL_MODEM_PROTOCOL_SUSPEND\n",
+			iod->name);
+
+		if (iod->format != IPC_MULTI_RAW)
+			return -EINVAL;
+
+		iodevs_for_each(&iod->mc->commons, iodev_netif_stop, 0);
+		return 0;
+
+	case IOCTL_MODEM_PROTOCOL_RESUME:
+		mif_info("%s: IOCTL_MODEM_PROTOCOL_RESUME\n",
+			iod->name);
+
+		if (iod->format != IPC_MULTI_RAW)
+			return -EINVAL;
+
+		iodevs_for_each(&iod->mc->commons, iodev_netif_wake, 0);
+		return 0;
+
+	case IOCTL_MODEM_DUMP_START:
+		mif_info("%s: IOCTL_MODEM_DUMP_START\n", iod->name);
+		return ld->dump_start(ld, iod);
+
+	case IOCTL_MODEM_DUMP_UPDATE:
+		mif_debug("%s: IOCTL_MODEM_DUMP_UPDATE\n", iod->name);
+		return ld->dump_update(ld, iod, arg);
+
+	case IOCTL_MODEM_FORCE_CRASH_EXIT:
+		mif_info("%s: IOCTL_MODEM_FORCE_CRASH_EXIT\n", iod->name);
+		if (iod->mc->ops.modem_force_crash_exit)
+			return iod->mc->ops.modem_force_crash_exit(iod->mc);
+		return -EINVAL;
+
+	case IOCTL_MODEM_CP_UPLOAD:
+		mif_info("%s: IOCTL_MODEM_CP_UPLOAD\n", iod->name);
+		if (copy_from_user(cpinfo_buf + strlen(cpinfo_buf),
+			(void __user *)arg, MAX_CPINFO_SIZE) != 0)
+			return -EFAULT;
+		panic(cpinfo_buf);
+		return 0;
+
+	case IOCTL_MODEM_DUMP_RESET:
+		mif_info("%s: IOCTL_MODEM_DUMP_RESET\n", iod->name);
+		return iod->mc->ops.modem_dump_reset(iod->mc);
+
+	default:
+		 /* If you need to handle the ioctl for specific link device,
+		  * then assign the link ioctl handler to ld->ioctl
+		  * It will be call for specific link ioctl */
+		if (ld->ioctl)
+			return ld->ioctl(ld, iod, cmd, arg);
+
+		mif_info("%s: ERR! cmd 0x%X not defined.\n", iod->name, cmd);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static ssize_t misc_write(struct file *filp, const char __user *data,
+			size_t count, loff_t *fpos)
+{
+	struct io_device *iod = (struct io_device *)filp->private_data;
+	struct link_device *ld = get_current_link(iod);
+	struct sipc5_frame_data *frm = &iod->meta_frame;
+	struct sk_buff *skb;
+	u8 *buff;
+	unsigned headroom;
+	unsigned tailroom = 0;
+	size_t tx_size;
+	int ret;
+
+	if (iod->format <= IPC_RFS && iod->id == 0)
+		return -EINVAL;
+
+	headroom = sipc5_build_tx_link_header(frm, iod, ld, count);
+
+	if (ld->aligned)
+		tailroom = sipc5_calc_padding_size(headroom + count);
+
+	tx_size = headroom + count + tailroom;
+
+	skb = alloc_skb(tx_size, GFP_KERNEL);
+	if (!skb) {
+		mif_info("%s: ERR! alloc_skb fail (tx_size:%d)\n",
+			iod->name, tx_size);
+		return -ENOMEM;
+	}
+
+	/* store IPC link header*/
+	buff = skb_put(skb, headroom);
+	memcpy(buff, frm->hdr, headroom);
+
+	/* store IPC message */
+	buff = skb_put(skb, count);
+	if (copy_from_user(buff, data, count) != 0) {
+		if (skb)
+			dev_kfree_skb_any(skb);
+		return -EFAULT;
+	}
+
+	if (iod->id == SIPC5_CH_ID_FMT_0) {
+		mif_ipc_log(iod->mc, MIF_IOD_TX_EVT, iod, ld, buff, count);
+		mif_flush_logs(ld->mc);
+	}
+
+	/* store padding */
+	if (tailroom)
+		skb_put(skb, tailroom);
+
+	/* send data with sk_buff, link device will put sk_buff
+	 * into the specific sk_buff_q and run work-q to send data
+	 */
+	skbpriv(skb)->iod = iod;
+	skbpriv(skb)->ld = ld;
+
+	ret = ld->send(ld, iod, skb);
+	if (ret < 0) {
+		mif_info("%s: ERR! ld->send fail (err %d)\n", iod->name, ret);
+		return ret;
+	}
+
+	if (ret != tx_size)
+		mif_info("%s: wrong tx size (count:%d tx_size:%d ret:%d)\n",
+			iod->name, count, tx_size, ret);
+
+	return count;
+}
+
+static ssize_t misc_read(struct file *filp, char *buf, size_t count,
+			loff_t *fpos)
+{
+	struct io_device *iod = (struct io_device *)filp->private_data;
+	struct sk_buff_head *rxq = &iod->sk_rx_q;
+	struct sk_buff *skb;
+	int copied = 0;
+
+	skb = skb_dequeue(rxq);
+	if (!skb) {
+		mif_info("%s: ERR! no data in rxq\n", iod->name);
+		return 0;
+	}
+
+	if (iod->id == SIPC5_CH_ID_FMT_0) {
+		mif_ipc_log(iod->mc, MIF_IOD_RX_EVT, iod, NULL, skb->data,
+				skb->len);
+		mif_flush_logs(iod->mc);
+	}
+
+	copied = skb->len > count ? count : skb->len;
+
+	if (copy_to_user(buf, skb->data, copied)) {
+		mif_info("%s: ERR! copy_to_user fail\n", iod->name);
+		dev_kfree_skb_any(skb);
+		return -EFAULT;
+	}
+
+	mif_debug("%s: data:%d copied:%d qlen:%d\n",
+		iod->name, skb->len, copied, rxq->qlen);
+
+	if (skb->len > count) {
+		skb_pull(skb, count);
+		skb_queue_head(rxq, skb);
+	} else {
+		dev_kfree_skb_any(skb);
+	}
+
+	return copied;
+}
+
+#ifdef CONFIG_LINK_DEVICE_C2C
+static int misc_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	int r = 0;
+	unsigned long size = 0;
+	unsigned long pfn = 0;
+	unsigned long offset = 0;
+	struct io_device *iod = (struct io_device *)filp->private_data;
+
+	if (!vma)
+		return -EFAULT;
+
+	size = vma->vm_end - vma->vm_start;
+	offset = vma->vm_pgoff << PAGE_SHIFT;
+	if (offset + size > (C2C_CP_RGN_SIZE + C2C_SH_RGN_SIZE)) {
+		mif_info("ERR: offset + size > C2C_CP_RGN_SIZE\n");
+		return -EINVAL;
+	}
+
+	/* Set the noncacheable property to the region */
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_flags |= VM_RESERVED | VM_IO;
+
+	pfn = __phys_to_pfn(C2C_CP_RGN_ADDR + offset);
+	r = remap_pfn_range(vma, vma->vm_start, pfn, size, vma->vm_page_prot);
+	if (r) {
+		mif_info("ERR: Failed in remap_pfn_range()!!!\n");
+		return -EAGAIN;
+	}
+
+	mif_info("%s: VA = 0x%08lx, offset = 0x%lx, size = %lu\n",
+		iod->name, vma->vm_start, offset, size);
+
+	return 0;
+}
+#endif
+
+static const struct file_operations misc_io_fops = {
+	.owner = THIS_MODULE,
+	.open = misc_open,
+	.release = misc_release,
+	.poll = misc_poll,
+	.unlocked_ioctl = misc_ioctl,
+	.write = misc_write,
+	.read = misc_read,
+#ifdef CONFIG_LINK_DEVICE_C2C
+	.mmap = misc_mmap,
+#endif
+};
+
+static int vnet_open(struct net_device *ndev)
+{
+	struct vnet *vnet = netdev_priv(ndev);
+
+	mif_info("%s\n", vnet->iod->name);
+
+	netif_start_queue(ndev);
+	atomic_inc(&vnet->iod->opened);
+	return 0;
+}
+
+static int vnet_stop(struct net_device *ndev)
+{
+	struct vnet *vnet = netdev_priv(ndev);
+
+	mif_info("%s\n", vnet->iod->name);
+
+	atomic_dec(&vnet->iod->opened);
+	netif_stop_queue(ndev);
+	return 0;
+}
+
+static int vnet_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+	struct vnet *vnet = netdev_priv(ndev);
+	struct io_device *iod = vnet->iod;
+	struct link_device *ld = get_current_link(iod);
+	struct sipc5_frame_data *frm = &iod->meta_frame;
+	struct sk_buff *skb_new;
+	unsigned headroom;
+	unsigned tailroom = 0;
+	int ret;
+
+#if 0
+	mif_ipc_log(iod->mc, MIF_IOD_TX_EVT, iod, ld, skb->data, skb->len);
+	mif_flush_logs(ld->mc);
+#endif
+
+	/* When use `handover' with Network Bridge,
+	 * user -> TCP/IP(kernel) -> bridge device -> TCP/IP(kernel) -> this.
+	 *
+	 * We remove the one ethernet header of skb before using skb->len,
+	 * because the skb has two ethernet headers.
+	 */
+	if (iod->use_handover) {
+		if (iod->id >= PS_DATA_CH_0 && iod->id <= PS_DATA_CH_LAST)
+			skb_pull(skb, sizeof(struct ethhdr));
+	}
+
+	headroom = sipc5_build_tx_link_header(frm, iod, ld, skb->len);
+
+	if (ld->aligned)
+		tailroom = sipc5_calc_padding_size(frm->len);
+
+	if (skb_headroom(skb) < headroom || skb_tailroom(skb) < tailroom) {
+		mif_debug("%s: skb_copy_expand needed\n", iod->name);
+		skb_new = skb_copy_expand(skb, headroom, tailroom, GFP_ATOMIC);
+		/* skb_copy_expand success or not, free old skb from caller */
+		dev_kfree_skb_any(skb);
+		if (!skb_new) {
+			mif_info("%s: ERR! skb_copy_expand fail\n", iod->name);
+			return NETDEV_TX_BUSY;
+		}
+	} else {
+		skb_new = skb;
+	}
+
+	memcpy(skb_push(skb_new, headroom), frm->hdr, headroom);
+	if (tailroom)
+		skb_put(skb_new, tailroom);
+
+	skbpriv(skb_new)->iod = iod;
+	skbpriv(skb_new)->ld = ld;
+
+	ret = ld->send(ld, iod, skb_new);
+	if (ret < 0) {
+		netif_stop_queue(ndev);
+		mif_info("%s: ERR! ld->send fail (err %d)\n", iod->name, ret);
+		return NETDEV_TX_BUSY;
+	}
+
+	ndev->stats.tx_packets++;
+	ndev->stats.tx_bytes += skb->len;
+
+	return NETDEV_TX_OK;
+}
+
+static struct net_device_ops vnet_ops = {
+	.ndo_open = vnet_open,
+	.ndo_stop = vnet_stop,
+	.ndo_start_xmit = vnet_xmit,
+};
+
+static void vnet_setup(struct net_device *ndev)
+{
+	ndev->netdev_ops = &vnet_ops;
+	ndev->type = ARPHRD_PPP;
+	ndev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
+	ndev->addr_len = 0;
+	ndev->hard_header_len = 0;
+	ndev->tx_queue_len = 1000;
+	ndev->mtu = ETH_DATA_LEN;
+	ndev->watchdog_timeo = 5 * HZ;
+}
+
+static void vnet_setup_ether(struct net_device *ndev)
+{
+	ndev->netdev_ops = &vnet_ops;
+	ndev->type = ARPHRD_ETHER;
+	ndev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST | IFF_SLAVE;
+	ndev->addr_len = ETH_ALEN;
+	random_ether_addr(ndev->dev_addr);
+	ndev->hard_header_len = 0;
+	ndev->tx_queue_len = 1000;
+	ndev->mtu = ETH_DATA_LEN;
+	ndev->watchdog_timeo = 5 * HZ;
+}
+
+int sipc5_init_io_device(struct io_device *iod)
+{
+	int ret = 0;
+	struct vnet *vnet;
+
+	/* Get modem state from modem control device */
+	iod->modem_state_changed = io_dev_modem_state_changed;
+
+	iod->sim_state_changed = io_dev_sim_state_changed;
+
+	/* Get data from link device */
+	mif_debug("%s: SIPC version = %d\n", iod->name, iod->ipc_version);
+	iod->recv = io_dev_recv_data_from_link_dev;
+
+	/* Register misc or net device */
+	switch (iod->io_typ) {
+	case IODEV_MISC:
+		init_waitqueue_head(&iod->wq);
+		skb_queue_head_init(&iod->sk_rx_q);
+		INIT_DELAYED_WORK(&iod->rx_work, rx_iodev_work);
+
+		iod->miscdev.minor = MISC_DYNAMIC_MINOR;
+		iod->miscdev.name = iod->name;
+		iod->miscdev.fops = &misc_io_fops;
+
+		ret = misc_register(&iod->miscdev);
+		if (ret)
+			mif_info("%s: ERR! misc_register failed\n", iod->name);
+
+		break;
+
+	case IODEV_NET:
+		skb_queue_head_init(&iod->sk_rx_q);
+		INIT_DELAYED_WORK(&iod->rx_work, rx_iodev_work);
+
+		if (iod->use_handover)
+			iod->ndev = alloc_netdev(0, iod->name,
+						vnet_setup_ether);
+		else
+			iod->ndev = alloc_netdev(0, iod->name, vnet_setup);
+
+		if (!iod->ndev) {
+			mif_info("%s: ERR! alloc_netdev fail\n", iod->name);
+			return -ENOMEM;
+		}
+
+		ret = register_netdev(iod->ndev);
+		if (ret) {
+			mif_info("%s: ERR! register_netdev fail\n", iod->name);
+			free_netdev(iod->ndev);
+		}
+
+		mif_debug("iod 0x%p\n", iod);
+		vnet = netdev_priv(iod->ndev);
+		mif_debug("vnet 0x%p\n", vnet);
+		vnet->iod = iod;
+
+		break;
+
+	case IODEV_DUMMY:
+		skb_queue_head_init(&iod->sk_rx_q);
+		/* in sipc5, does not need rx_iodev_work on DUMMY */
+
+		iod->miscdev.minor = MISC_DYNAMIC_MINOR;
+		iod->miscdev.name = iod->name;
+		iod->miscdev.fops = &misc_io_fops;
+
+		ret = misc_register(&iod->miscdev);
+		if (ret)
+			mif_info("%s: ERR! misc_register fail\n", iod->name);
+		ret = device_create_file(iod->miscdev.this_device,
+					&attr_waketime);
+		if (ret)
+			mif_info("%s: ERR! device_create_file fail\n",
+				iod->name);
+
+		break;
+
+	default:
+		mif_info("%s: ERR! wrong io_type %d\n", iod->name, iod->io_typ);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
diff --git a/drivers/misc/modem_if/sipc5_modem.c b/drivers/misc/modem_if/sipc5_modem.c
new file mode 100644
index 0000000..a1287b5
--- /dev/null
+++ b/drivers/misc/modem_if/sipc5_modem.c
@@ -0,0 +1,325 @@
+/* linux/drivers/modem/modem.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 Samsung Electronics.
+ *
+ * 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/init.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+#include <linux/if_arp.h>
+
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/wakelock.h>
+
+#include <linux/platform_data/modem.h>
+#include "modem_prj.h"
+#include "modem_variation.h"
+#include "modem_utils.h"
+
+#define FMT_WAKE_TIME   (HZ/2)
+#define RAW_WAKE_TIME   (HZ*6)
+
+static struct modem_ctl *create_modemctl_device(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct modem_data *pdata;
+	struct modem_ctl *modemctl;
+	struct device *dev = &pdev->dev;
+
+	/* create modem control device */
+	modemctl = kzalloc(sizeof(struct modem_ctl), GFP_KERNEL);
+	if (!modemctl)
+		return NULL;
+
+	modemctl->dev = dev;
+	modemctl->phone_state = STATE_OFFLINE;
+
+	pdata = pdev->dev.platform_data;
+	modemctl->mdm_data = pdata;
+	modemctl->name = pdata->name;
+
+	/* initialize link device list */
+	INIT_LIST_HEAD(&modemctl->commons.link_dev_list);
+
+	/* initialize tree of io devices */
+	modemctl->commons.iodevs_tree_chan = RB_ROOT;
+	modemctl->commons.iodevs_tree_fmt = RB_ROOT;
+
+	/* init modemctl device for getting modemctl operations */
+	ret = call_modem_init_func(modemctl, pdata);
+	if (ret) {
+		kfree(modemctl);
+		return NULL;
+	}
+
+	modemctl->use_mif_log = pdata->use_mif_log;
+	if (pdata->use_mif_log)
+		mif_err("<%s> IPC logger can be used.\n", pdata->name);
+
+	ret = mif_init_log(modemctl);
+	if (ret < 0) {
+		kfree(modemctl);
+		return NULL;
+	}
+
+	mif_info("%s is created!!!\n", pdata->name);
+
+	return modemctl;
+}
+
+static struct io_device *create_io_device(struct modem_io_t *io_t,
+		struct modem_ctl *modemctl, struct modem_data *pdata)
+{
+	int ret = 0;
+	struct io_device *iod = NULL;
+
+	iod = kzalloc(sizeof(struct io_device), GFP_KERNEL);
+	if (!iod) {
+		mif_err("iod == NULL\n");
+		return NULL;
+	}
+
+	rb_init_node(&iod->node_chan);
+	rb_init_node(&iod->node_fmt);
+
+	iod->name = io_t->name;
+	iod->id = io_t->id;
+	iod->format = io_t->format;
+	iod->io_typ = io_t->io_type;
+	iod->link_types = io_t->links;
+	iod->net_typ = pdata->modem_net;
+	iod->use_handover = pdata->use_handover;
+	iod->ipc_version = pdata->ipc_version;
+	atomic_set(&iod->opened, 0);
+
+	/* link between io device and modem control */
+	iod->mc = modemctl;
+	if (iod->format == IPC_FMT)
+		modemctl->iod = iod;
+	if (iod->format == IPC_BOOT) {
+		modemctl->bootd = iod;
+		mif_info("Bood device = %s\n", iod->name);
+	}
+
+	/* register misc device or net device */
+	ret = sipc5_init_io_device(iod);
+	if (ret) {
+		kfree(iod);
+		mif_err("sipc5_init_io_device fail (%d)\n", ret);
+		return NULL;
+	}
+
+	mif_debug("%s is created!!!\n", iod->name);
+	return iod;
+}
+
+static int attach_devices(struct modem_ctl *mc, struct io_device *iod,
+		enum modem_link tx_link)
+{
+	struct link_device *ld;
+	unsigned ch;
+
+	/* add iod to rb_tree */
+	if (iod->format != IPC_RAW)
+		insert_iod_with_format(&mc->commons, iod->format, iod);
+
+	if (sipc5_is_not_reserved_channel(iod->id))
+		insert_iod_with_channel(&mc->commons, iod->id, iod);
+
+	/* find link type for this io device */
+	list_for_each_entry(ld, &mc->commons.link_dev_list, list) {
+		if (IS_CONNECTED(iod, ld)) {
+			/* The count 1 bits of iod->link_types is count
+			 * of link devices of this iod.
+			 * If use one link device,
+			 * or, 2+ link devices and this link is tx_link,
+			 * set iod's link device with ld
+			 */
+			if ((countbits(iod->link_types) <= 1) ||
+					(tx_link == ld->link_type)) {
+				mif_debug("set %s->%s\n", iod->name, ld->name);
+				set_current_link(iod, ld);
+			}
+		}
+	}
+
+	/* if use rx dynamic switch, set tx_link at modem_io_t of
+	 * board-*-modems.c
+	 */
+	if (!get_current_link(iod)) {
+		mif_err("%s->link == NULL\n", iod->name);
+		BUG();
+	}
+
+	switch (iod->format) {
+	case IPC_FMT:
+		wake_lock_init(&iod->wakelock, WAKE_LOCK_SUSPEND, iod->name);
+		iod->waketime = FMT_WAKE_TIME;
+		break;
+
+	case IPC_RAW:
+		wake_lock_init(&iod->wakelock, WAKE_LOCK_SUSPEND, iod->name);
+		iod->waketime = RAW_WAKE_TIME;
+		break;
+
+	case IPC_RFS:
+		wake_lock_init(&iod->wakelock, WAKE_LOCK_SUSPEND, iod->name);
+		iod->waketime = RAW_WAKE_TIME;
+		break;
+
+	case IPC_MULTI_RAW:
+		wake_lock_init(&iod->wakelock, WAKE_LOCK_SUSPEND, iod->name);
+		iod->waketime = RAW_WAKE_TIME;
+		break;
+
+	case IPC_BOOT:
+		wake_lock_init(&iod->wakelock, WAKE_LOCK_SUSPEND, iod->name);
+		iod->waketime = RAW_WAKE_TIME;
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int __devinit modem_probe(struct platform_device *pdev)
+{
+	int i;
+	struct modem_data *pdata = pdev->dev.platform_data;
+	struct modem_ctl *modemctl;
+	struct io_device *iod[pdata->num_iodevs];
+	struct link_device *ld;
+
+	mif_err("%s\n", pdev->name);
+	memset(iod, 0, sizeof(iod));
+
+	modemctl = create_modemctl_device(pdev);
+	if (!modemctl) {
+		mif_err("modemctl == NULL\n");
+		goto err_free_modemctl;
+	}
+
+	/* create link device */
+	/* support multi-link device */
+	for (i = 0; i < LINKDEV_MAX ; i++) {
+		/* find matching link type */
+		if (pdata->link_types & LINKTYPE(i)) {
+			ld = call_link_init_func(pdev, i);
+			if (!ld)
+				goto err_free_modemctl;
+
+			mif_err("link created: %s\n", ld->name);
+			ld->link_type = i;
+			ld->mc = modemctl;
+			list_add(&ld->list, &modemctl->commons.link_dev_list);
+		}
+	}
+
+	/* create io deivces and connect to modemctl device */
+	for (i = 0; i < pdata->num_iodevs; i++) {
+		iod[i] = create_io_device(&pdata->iodevs[i], modemctl, pdata);
+		if (!iod[i]) {
+			mif_err("iod[%d] == NULL\n", i);
+			goto err_free_modemctl;
+		}
+
+		attach_devices(modemctl, iod[i],
+				pdata->iodevs[i].tx_link);
+	}
+
+	platform_set_drvdata(pdev, modemctl);
+
+	mif_err("Complete!!!\n");
+
+	return 0;
+
+err_free_modemctl:
+	for (i = 0; i < pdata->num_iodevs; i++)
+		if (iod[i] != NULL)
+			kfree(iod[i]);
+
+	if (modemctl != NULL)
+		kfree(modemctl);
+
+	return -ENOMEM;
+}
+
+static void modem_shutdown(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct modem_ctl *mc = dev_get_drvdata(dev);
+	mc->ops.modem_off(mc);
+	mc->phone_state = STATE_OFFLINE;
+}
+
+static int modem_suspend(struct device *pdev)
+{
+#ifndef CONFIG_LINK_DEVICE_HSIC
+	struct modem_ctl *mc = dev_get_drvdata(pdev);
+
+	if (mc->gpio_pda_active)
+		gpio_set_value(mc->gpio_pda_active, 0);
+#endif
+
+	return 0;
+}
+
+static int modem_resume(struct device *pdev)
+{
+#ifndef CONFIG_LINK_DEVICE_HSIC
+	struct modem_ctl *mc = dev_get_drvdata(pdev);
+
+	if (mc->gpio_pda_active)
+		gpio_set_value(mc->gpio_pda_active, 1);
+#endif
+
+	return 0;
+}
+
+static const struct dev_pm_ops modem_pm_ops = {
+	.suspend    = modem_suspend,
+	.resume     = modem_resume,
+};
+
+static struct platform_driver modem_driver = {
+	.probe = modem_probe,
+	.shutdown = modem_shutdown,
+	.driver = {
+		.name = "mif_sipc5",
+		.pm   = &modem_pm_ops,
+	},
+};
+
+static int __init modem_init(void)
+{
+	return platform_driver_register(&modem_driver);
+}
+
+module_init(modem_init);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Samsung Modem Interface Driver");
diff --git a/drivers/misc/mpu3050/Kconfig b/drivers/misc/mpu3050/Kconfig
new file mode 100755
index 0000000..933aa33
--- /dev/null
+++ b/drivers/misc/mpu3050/Kconfig
@@ -0,0 +1,147 @@
+
+menu "Motion Sensors Support"
+
+config MPU_NONE
+    bool "None"
+
+config MPU_SENSORS_MPU6000
+    tristate "MPU6000"
+    depends on I2C
+
+choice
+    prompt "Accelerometer Type"
+    depends on MPU_SENSORS_MPU3050
+    default MPU_SENSORS_ACCELEROMETER_NONE
+
+config MPU_SENSORS_ACCELEROMETER_NONE
+    bool "NONE"
+    depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6000
+
+config MPU_SENSORS_ADXL346
+    bool "ADI adxl346"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_BMA150
+    bool "Bosch BMA150"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_BMA222
+    bool "Bosch BMA222"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_KXSD9
+    bool "Kionix KXSD9"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_KXUD9
+    bool "Kionix KXUD9"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_KXTF9
+    bool "Kionix KXTF9"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_LIS331DLH
+    bool "ST lis331dlh"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_LIS3DH
+    bool "ST lis3dh"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_LSM303DLHA
+    bool "ST lsm303dlh"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_MMA8450
+    bool "Freescale mma8450"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_MMA845X
+    bool "Freescale mma8451/8452/8453"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_KXTF9_LIS3DH
+    bool "Kionix KXTF9+ ST LIS3DH"
+    depends on MPU_SENSORS_MPU3050
+
+endchoice
+
+choice
+    prompt "Compass Type"
+    depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050
+    default MPU_SENSORS_COMPASS_NONE
+
+config MPU_SENSORS_COMPASS_NONE
+    bool "NONE"
+    depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_AK8975
+    bool "AKM ak8975"
+    depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_MMC314X
+    bool "MEMSIC mmc314x"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_MMC328X
+    bool "MEMSIC mmc328x"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_AMI30X
+    bool "Aichi Steel ami30X"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_HMC5883
+    bool "Honeywell hmc5883"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_LSM303DLHM
+    bool "ST lsm303dlh"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_MMC314X
+    bool "MEMSIC mmc314xMS"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_YAS529
+    bool "Yamaha yas529"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_HSCDTD002B
+    bool "Alps hscdtd002b"
+    depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_HSCDTD004A
+    bool "Alps hscdtd004a"
+    depends on MPU_SENSORS_MPU3050
+
+endchoice
+
+choice
+    prompt "Pressure Type"
+    depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050
+    default MPU_SENSORS_PRESSURE_NONE
+
+config MPU_SENSORS_PRESSURE_NONE
+    bool "NONE"
+    depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_BMA085
+    bool "Bosch BMA085"
+    depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050
+
+endchoice
+
+
+config MPU_SENSORS_CORE
+	tristate "Sensors core"
+
+config MPU_SENSORS_TIMERIRQ
+    tristate "Timer IRQ"
+
+config MPU_SENSORS_DEBUG
+    bool "MPU debug"
+    depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6000 || MPU_SENSORS_TIMERIRQ
+
+endmenu
diff --git a/drivers/misc/mpu3050/Makefile b/drivers/misc/mpu3050/Makefile
new file mode 100755
index 0000000..7432122
--- /dev/null
+++ b/drivers/misc/mpu3050/Makefile
@@ -0,0 +1,145 @@
+#

+# 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 \

+	mpu-accel.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_MACH_BOSE_ATT

+	mpu3050-objs += $(MLLITE_DIR)accel/kxud9.o

+	mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o

+else

+ifdef CONFIG_MPU_SENSORS_KXUD9

+mpu3050-objs += $(MLLITE_DIR)accel/kxud9.o

+endif

+

+ifdef CONFIG_MPU_SENSORS_KXTF9

+mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o

+endif

+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

+

+ifdef CONFIG_MPU_SENSORS_KXTF9_LIS3DH

+mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o

+mpu3050-objs += $(MLLITE_DIR)accel/lis3dh.o

+endif

+

+#

+# Compass options

+#

+ifdef CONFIG_MPU_SENSORS_AK8975

+mpu3050-objs += $(MLLITE_DIR)compass/mpuak8975.o

+endif

+

+ifdef CONFIG_MPU_SENSORS_AMI30X

+mpu3050-objs += $(MLLITE_DIR)compass/ami30x.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_MMC328X

+mpu3050-objs += $(MLLITE_DIR)compass/mmc328x.o

+endif

+

+ifdef CONFIG_MPU_SENSORS_YAS529

+mpu3050-objs += $(MLLITE_DIR)compass/yas529-kernel.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

+

+ccflags-y += -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_MPU6000

+ccflags-y += -DM_HW

+endif

+

+obj-$(CONFIG_MPU_SENSORS_CORE)			+= sensors_core.o

+obj-$(CONFIG_MPU_SENSORS_TIMERIRQ)+= timerirq.o

+

+ifdef CONFIG_MPU_SENSORS_DEBUG

+ccflags-y += -DDEBUG

+endif

+

diff --git a/drivers/misc/mpu3050/README b/drivers/misc/mpu3050/README
new file mode 100755
index 0000000..2734dc1
--- /dev/null
+++ b/drivers/misc/mpu3050/README
@@ -0,0 +1,250 @@
+Kernel driver mpu

+=====================

+

+Supported chips:

+  * InvenSense IMU3050

+    Prefix: 'mpu3050'

+    Datasheet:

+        PS-MPU-3000A-00.2.4b.pdf

+

+  * InvenSense IMU6000

+    Prefix: 'mpu6000'

+    Datasheet:

+        MPU-6000A-00 v1.0.pdf

+

+Author: InvenSense <http://invensense.com>

+

+Description

+-----------

+The mpu is a motion processor unit that controls the mpu3050 gyroscope, a slave

+accelerometer, a compass and a pressure sensor, or the mpu6000 and slave 

+compass.  This document describes how to install the driver into a Linux kernel

+and a small note about how to set up the file permissions in an android file

+system.

+

+Sysfs entries

+-------------

+/dev/mpu

+/dev/mpuirq

+/dev/accelirq

+/dev/compassirq

+/dev/pressureirq

+

+General Remarks MPU3050

+-----------------------

+* Valid addresses for the MPU3050 is 0x68.

+* Accelerometer must be on the secondary I2C bus for MPU3050, the 

+  magnetometer must be on the primary bus and pressure sensor must

+  be on the primary bus.

+

+General Remarks MPU6000

+-----------------------

+* Valid addresses for the MPU6000 is 0x68.

+* Magnetometer must be on the secondary I2C bus for the MPU6000.

+* Accelerometer slave address must be set to 0x68

+* Gyro and Accel orientation matrices should be the same

+

+Programming the chip using /dev/mpu

+----------------------------------

+Programming of MPU3050 or MPU6000 is done by first opening the /dev/mpu file and

+then performing a series of IOCTLS on the handle returned.  The IOCTL codes can

+be found in mpu.h.  Typically this is done by the mllite library in user

+space.

+

+Adding to a Kernel

+==================

+

+The mpu driver is designed to be inserted in the drivers/misc part of the 

+kernel.  Extracting the tarball from the root kernel dir will place the

+contents of the tarball here:

+

+    <kernel root dir>/drivers/misc/mpu3050

+    <kernel root dir>/include/linux/mpu.h

+    <kernel root dir>/include/linux/mpu3050.h

+    <kernel root dir>/include/linux/mpu6000.h

+

+After this is done the drivers/misc/Kconfig must be edited to add the line:

+

+    source "drivers/misc/mpu3050/Kconfig"

+

+Similarly drivers/misc/Makefile must be edited to add the line:

+

+    obj-y += mpu3050/

+

+Configuration can then be done as normal.

+

+NOTE: This driver depends on a kernel patch to drivers/char/char.c.  This patch

+started to be included in most 2.6.35 based kernels.

+drivers: misc: pass miscdevice pointer via file private data

+https://patchwork.kernel.org/patch/96412/

+

+---

+ drivers/char/misc.c |    1 +

+ 1 files changed, 1 insertions(+), 0 deletions(-)

+

+

+diff --git a/drivers/char/misc.c b/drivers/char/misc.c

+index 92ab03d..cd650ca 100644

+--- a/drivers/char/misc.c

++++ b/drivers/char/misc.c

+@@ -144,6 +144,7 @@ static int misc_open(struct inode * inode, struct file * file)

+ 	old_fops = file->f_op;

+ 	file->f_op = new_fops;

+ 	if (file->f_op->open) {

++		file->private_data = c;

+ 		err=file->f_op->open(inode,file);

+ 		if (err) {

+ 			fops_put(file->f_op);

+---

+

+Board and Platform Data

+-----------------------

+

+In order for the driver to work, board and platform data specific to the device

+needs to be added to the board file.  A mpu3050_platform_data structure must

+be created and populated and set in the i2c_board_info_structure.  For details

+of each structure member see mpu.h.  All values below are simply an example and

+should be modified for your platform.

+

+#include <linux/mpu.h>

+

+#if defined(CONFIG_SENSORS_MPU3050) || defined(CONFIG_SENSORS_MPU3050_MODULE)

+

+#define SENSOR_MPU_NAME "mpu3050"

+

+static struct mpu3050_platform_data mpu_data = {

+	.int_config  = 0x10,

+	.orientation = {  -1,  0,  0, 

+			   0,  1,  0, 

+			   0,  0, -1 },

+	/* accel */

+	.accel = {

+#ifdef CONFIG_SENSORS_MPU3050_MODULE

+		 .get_slave_descr = NULL,

+#else

+		 .get_slave_descr = get_accel_slave_descr,

+#endif

+		 .adapt_num   = 2,

+		 .bus         = EXT_SLAVE_BUS_SECONDARY,

+		 .address     = 0x0F,

+		 .orientation = {  -1,  0,  0, 

+				    0,  1,  0, 

+				    0,  0, -1 },

+	 },

+	/* compass */

+	.compass = {

+#ifdef CONFIG_SENSORS_MPU3050_MODULE

+		 .get_slave_descr = NULL,

+#else

+		 .get_slave_descr = get_compass_slave_descr,

+#endif

+		 .adapt_num   = 2,

+		 .bus         = EXT_SLAVE_BUS_PRIMARY,

+		 .address     = 0x0E,

+		 .orientation = { 1, 0, 0, 

+				  0, 1, 0, 

+				  0, 0, 1 },

+	 },

+	/* pressure */

+	.pressure = {

+#ifdef CONFIG_SENSORS_MPU3050_MODULE

+		 .get_slave_descr = NULL,

+#else

+		 .get_slave_descr = get_pressure_slave_descr,

+#endif

+		 .adapt_num   = 2,

+		 .bus         = EXT_SLAVE_BUS_PRIMARY,

+		 .address     = 0x77,

+		 .orientation = { 1, 0, 0, 

+				  0, 1, 0, 

+				  0, 0, 1 },

+	 },

+};

+#endif

+

+#if defined(CONFIG_SENSORS_MPU6000) || defined(CONFIG_SENSORS_MPU6000_MODULE)

+

+#define SENSOR_MPU_NAME "mpu6000"

+

+static struct mpu3050_platform_data mpu_data = {

+	.int_config  = 0x10,

+	.orientation = {  -1,  0,  0,

+			   0,  1,  0,

+			   0,  0, -1 },

+	/* accel */

+	.accel = {

+#ifdef CONFIG_SENSORS_MPU6000_MODULE

+		 .get_slave_descr = NULL,

+#else

+		 .get_slave_descr = get_accel_slave_descr,

+#endif

+		 .adapt_num   = 2,

+		 .bus         = EXT_SLAVE_BUS_PRIMARY,

+		 .address     = 0x68,

+		 .orientation = {  -1,  0,  0,

+				    0,  1,  0,

+				    0,  0, -1 },

+	 },

+	/* compass */

+	.compass = {

+#ifdef CONFIG_SENSORS_MPU6000_MODULE

+		 .get_slave_descr = NULL,

+#else

+		 .get_slave_descr = get_compass_slave_descr,

+#endif

+		 .adapt_num   = 2,

+		 .bus         = EXT_SLAVE_BUS_SECONDARY,

+		 .address     = 0x0E,

+		 .orientation = { 1, 0, 0,

+				  0, 1, 0,

+				  0, 0, 1 },

+	 },

+	/* pressure */

+	.pressure = {

+#ifdef CONFIG_SENSORS_MPU6000_MODULE

+		 .get_slave_descr = NULL,

+#else

+		 .get_slave_descr = get_pressure_slave_descr,

+#endif

+		 .adapt_num   = 2,

+		 .bus         = EXT_SLAVE_BUS_PRIMARY,

+		 .address     = 0x77,

+		 .orientation = { 1, 0, 0, 

+				  0, 1, 0, 

+				  0, 0, 1 },

+	 },

+

+};

+#endif

+

+static struct i2c_board_info __initdata beagle_i2c_2_boardinfo[] = {

+	{

+		I2C_BOARD_INFO(SENSOR_MPU_NAME, 0x68),

+		.irq = (IH_GPIO_BASE + MPU_GPIO_IRQ),

+		.platform_data = &mpu_data,

+	},

+};

+

+Typically the IRQ is a GPIO input pin and needs to be configured properly.  If

+in the above example GPIO 168 corresponds to IRQ 299, the following should be

+done as well:

+

+#define MPU_GPIO_IRQ 168

+

+    gpio_request(MPU_GPIO_IRQ,"MPUIRQ");

+    gpio_direction_input(MPU_GPIO_IRQ)

+

+

+Android File Permissions

+========================

+

+To set up the file permissions on an android system, the /dev/mpu and 

+/dev/mpuirq files needs to be added to the system/core/init/devices.c file 

+inside the perms_ structure.

+

+static struct perms_ devperms[] = {

+    { "/dev/mpu"           ,0640,   AID_SYSTEM,    AID_SYSTEM,     1 },

+};

+

+Sufficient file permissions need to be give to read and write it by the system.

+

diff --git a/drivers/misc/mpu3050/accel/adxl346.c b/drivers/misc/mpu3050/accel/adxl346.c
new file mode 100755
index 0000000..14cb38a
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/adxl346.c
@@ -0,0 +1,163 @@
+/*
+ $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   adxl346.c
+ *      @brief  Accelerometer setup and handling methods for AD adxl346.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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 ACCEL_ADI346_SLEEP_REG      (0x2D)
+#define ACCEL_ADI346_SLEEP_MASK     (0x04)
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+int adxl346_suspend(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address,
+			   ACCEL_ADI346_SLEEP_REG, 1, &reg);
+	ERROR_CHECK(result);
+	reg |= ACCEL_ADI346_SLEEP_MASK;
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_ADI346_SLEEP_REG, reg);
+	ERROR_CHECK(result);
+	return result;
+}
+
+/* full scale setting - register & mask */
+#define ACCEL_ADI346_CTRL_REG      (0x31)
+#define ACCEL_ADI346_CTRL_MASK     (0x03)
+
+int adxl346_resume(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+	unsigned char reg;
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address,
+			   ACCEL_ADI346_SLEEP_REG, 1, &reg);
+	ERROR_CHECK(result);
+	reg &= ~ACCEL_ADI346_SLEEP_MASK;
+	/*wake up if sleeping */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       ACCEL_ADI346_SLEEP_REG, reg);
+	ERROR_CHECK(result);
+	/*MLOSSleep(10) */
+
+	/* Full Scale */
+	reg = 0x04;
+	reg &= ~ACCEL_ADI346_CTRL_MASK;
+	if (slave->range.mantissa == 4)
+		reg |= 0x1;
+	else if (slave->range.mantissa == 8)
+		reg |= 0x2;
+	else if (slave->range.mantissa == 16)
+		reg |= 0x3;
+	else {
+		slave->range.mantissa = 2;
+		reg |= 0x0;
+	}
+	slave->range.fraction = 0;
+
+	/* DATA_FORMAT: full resolution of +/-2g; data is left justified */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x31, reg);
+	ERROR_CHECK(result);
+	/* BW_RATE: normal power operation with output data rate of 200Hz */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x2C, 0x0B);
+	ERROR_CHECK(result);
+	/* POWER_CTL: power on in measurement mode */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x2D, 0x28);
+	ERROR_CHECK(result);
+	/*--- after wake up, it takes at least [1/(data rate) + 1.1]ms ==>
+	  6.1ms to get valid sensor data ---*/
+	MLOSSleep(10);
+
+	return result;
+}
+
+int adxl346_read(void *mlsl_handle,
+		 struct ext_slave_descr *slave,
+		 struct ext_slave_platform_data *pdata,
+		 unsigned char *data)
+{
+	int result;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->len, data);
+	return result;
+}
+
+struct ext_slave_descr adxl346_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ adxl346_suspend,
+	/*.resume           = */ adxl346_resume,
+	/*.read             = */ adxl346_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "adx1346",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_ADI346,
+	/*.reg              = */ 0x32,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ {2, 0},
+};
+
+struct ext_slave_descr *adxl346_get_slave_descr(void)
+{
+	return &adxl346_descr;
+}
+EXPORT_SYMBOL(adxl346_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/accel/bma150.c b/drivers/misc/mpu3050/accel/bma150.c
new file mode 100755
index 0000000..30fed15
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/bma150.c
@@ -0,0 +1,149 @@
+/*
+ $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   bma150.c
+ *      @brief  Accelerometer setup and handling methods.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#ifdef __KERNEL__
+#include <linux/module.h>
+#endif
+
+#include "mpu.h"
+#include "mlos.h"
+#include "mlsl.h"
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*********************************************
+    Accelerometer Initialization Functions
+**********************************************/
+
+static int bma150_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result;
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0a, 0x01);
+	MLOSSleep(3); /* 3 ms powerup time maximum */
+	ERROR_CHECK(result);
+	return result;
+}
+
+/* full scale setting - register and mask */
+#define ACCEL_BOSCH_CTRL_REG       (0x14)
+#define ACCEL_BOSCH_CTRL_MASK      (0x18)
+
+static int bma150_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg = 0;
+
+	/* Soft reset */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0a, 0x02);
+	ERROR_CHECK(result);
+	MLOSSleep(3);
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, 0x14, 1, &reg);
+	ERROR_CHECK(result);
+
+	/* Bandwidth */
+	reg &= 0xc0;
+	reg |= 3;		/* 3=190 Hz */
+
+	/* Full Scale */
+	reg &= ~ACCEL_BOSCH_CTRL_MASK;
+	if (slave->range.mantissa == 4)
+		reg |= 0x08;
+	else if (slave->range.mantissa == 8)
+		reg |= 0x10;
+	else {
+		slave->range.mantissa = 2;
+		reg |= 0x00;
+	}
+	slave->range.fraction = 0;
+
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x14, reg);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+static int bma150_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->len, data);
+	return result;
+}
+
+static struct ext_slave_descr bma150_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ bma150_suspend,
+	/*.resume           = */ bma150_resume,
+	/*.read             = */ bma150_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "bma150",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_BMA150,
+	/*.reg              = */ 0x02,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ {2, 0},
+};
+
+struct ext_slave_descr *bma150_get_slave_descr(void)
+{
+	return &bma150_descr;
+}
+EXPORT_SYMBOL(bma150_get_slave_descr);
+
+#ifdef __KERNEL__
+MODULE_AUTHOR("Invensense");
+MODULE_DESCRIPTION("User space IRQ handler for MPU3xxx devices");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("bma");
+#endif
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/accel/bma222.c b/drivers/misc/mpu3050/accel/bma222.c
new file mode 100755
index 0000000..534a1e5
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/bma222.c
@@ -0,0 +1,142 @@
+/*
+ $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   bma222.c
+ *      @brief  Accelerometer setup and handling methods.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#ifdef __KERNEL__
+#include <linux/module.h>
+#endif
+
+#include "mpu.h"
+#include "mlos.h"
+#include "mlsl.h"
+
+#define ACCEL_BMA222_RANGE_REG          (0x0F)
+#define ACCEL_BMA222_BW_REG             (0x10)
+#define ACCEL_BMA222_SUSPEND_REG        (0x11)
+#define ACCEL_BMA222_SFT_RST_REG        (0x14)
+
+/*********************************************
+    Accelerometer Initialization Functions
+**********************************************/
+
+static int bma222_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result;
+
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_BMA222_SUSPEND_REG, 0x80);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+static int bma222_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg = 0;
+
+	/* Soft reset */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_BMA222_SFT_RST_REG, 0xB6);
+	ERROR_CHECK(result);
+	MLOSSleep(10);
+
+	/*Bandwidth */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_BMA222_BW_REG, 0x0C);
+	ERROR_CHECK(result);
+
+	/* Full Scale */
+	if (slave->range.mantissa == 4)
+		reg |= 0x05;
+	else if (slave->range.mantissa == 8)
+		reg |= 0x08;
+	else if (slave->range.mantissa == 16)
+		reg |= 0x0C;
+	else {
+		slave->range.mantissa = 2;
+		reg |= 0x03;
+	}
+	slave->range.fraction = 0;
+
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_BMA222_RANGE_REG, reg);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+static int bma222_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->len, data);
+	return result;
+}
+
+static struct ext_slave_descr bma222_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ bma222_suspend,
+	/*.resume           = */ bma222_resume,
+	/*.read             = */ bma222_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "bma222",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_BMA222,
+	/*.reg              = */ 0x02,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ {2, 0},
+};
+
+struct ext_slave_descr *bma222_get_slave_descr(void)
+{
+	return &bma222_descr;
+}
+EXPORT_SYMBOL(bma222_get_slave_descr);
+
+/*
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/accel/cma3000.c b/drivers/misc/mpu3050/accel/cma3000.c
new file mode 100755
index 0000000..0592595
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/cma3000.c
@@ -0,0 +1,109 @@
+/*
+ $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   cma3000.c
+ *      @brief  Accelerometer setup and handling methods for VTI CMA3000
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#ifdef __KERNEL__
+#include <linux/module.h>
+#endif
+#include "mpu.h"
+#include "mlsl.h"
+#include "mlos.h"
+#include "accel.h"
+
+#include <log.h>
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+int cma3000_suspend(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata)
+{
+	int result;
+	/* RAM reset */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1d, 0xcd);
+	return result;
+}
+
+int cma3000_resume(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+
+
+	return ML_SUCCESS;
+}
+
+int cma3000_read(void *mlsl_handle,
+		 struct ext_slave_descr *slave,
+		 struct ext_slave_platform_data *pdata,
+		 unsigned char *data)
+{
+	int result;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->len, data);
+	return result;
+}
+
+struct ext_slave_descr cma3000_descr = {
+	/*.suspend          = */ cma3000_suspend,
+	/*.resume           = */ cma3000_resume,
+	/*.read             = */ cma3000_read,
+	/*.name             = */ "cma3000",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ ID_INVALID,
+	/* fixme - id to added when support becomes available */
+	/*.reg              = */ 0x06,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ 65536,
+};
+
+struct ext_slave_descr *cma3000_get_slave_descr(void)
+{
+	return &cma3000_descr;
+}
+EXPORT_SYMBOL(cma3000_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/accel/kxsd9.c b/drivers/misc/mpu3050/accel/kxsd9.c
new file mode 100755
index 0000000..77bc52c
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/kxsd9.c
@@ -0,0 +1,145 @@
+/*
+ $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   kxsd9.c
+ *      @brief  Accelerometer setup and handling methods.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#ifdef __KERNEL__
+#include <linux/kernel.h>
+#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"
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+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 =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x0);
+	ERROR_CHECK(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 = ML_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 =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_KIONIX_CTRL_REG, reg);
+	ERROR_CHECK(result);
+	/* normal operation */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x40);
+	ERROR_CHECK(result);
+
+	return ML_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 = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->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_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_KXSD9,
+	/*.reg              = */ 0x00,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {2, 5006},
+};
+
+struct ext_slave_descr *kxsd9_get_slave_descr(void)
+{
+	return &kxsd9_descr;
+}
+EXPORT_SYMBOL(kxsd9_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/accel/kxtf9.c b/drivers/misc/mpu3050/accel/kxtf9.c
new file mode 100755
index 0000000..f438259
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/kxtf9.c
@@ -0,0 +1,617 @@
+/*
+ $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;
+};
+
+extern struct acc_data cal_data;
+/*****************************************
+    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;
+	}
+
+	if (odr != 0)
+		config->ctrl_reg1 |= 0x80;
+
+	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;
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       KXTF9_CTRL_REG1, 0x0);
+	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)
+{
+	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:
+		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 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;
+	int x, y, z;
+
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->len, data);
+
+	if (slave->len == 6) {
+		x = (s16) ((data[1] << 4) | (data[0] >> 4)) + cal_data.x;
+		y = (s16) ((data[3] << 4) | (data[2] >> 4)) + cal_data.y;
+		z = (s16) ((data[5] << 4) | (data[4] >> 4)) + cal_data.z;
+
+		data[0] = (x & 0xf) << 4;
+		data[1] = (x & 0xff0) >> 4;
+		data[2] = (y & 0xf) << 4;
+		data[3] = (y & 0xff0) >> 4;
+		data[4] = (z & 0xf) << 4;
+		data[5] = (z & 0xff0) >> 4;
+	}
+
+	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/accel/kxud9.c b/drivers/misc/mpu3050/accel/kxud9.c
new file mode 100755
index 0000000..651219e
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/kxud9.c
@@ -0,0 +1,145 @@
+/*
+ $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   kxud9.c
+ *      @brief  Accelerometer setup and handling methods.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#ifdef __KERNEL__
+#include <linux/kernel.h>
+#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"
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Accelerometer Initialization Functions
+ *****************************************/
+
+static int kxud9_suspend(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	/* CTRL_REGB: low-power standby mode */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x0);
+	ERROR_CHECK(result);
+	return result;
+}
+
+/* full scale setting - register and mask */
+#define ACCEL_KIONIX_CTRL_REG      (0x0C)
+#define ACCEL_KIONIX_CTRL_MASK     (0x3)
+
+static int kxud9_resume(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata)
+{
+	int result = ML_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 =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_KIONIX_CTRL_REG, reg);
+	ERROR_CHECK(result);
+	/* normal operation */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x40);
+	ERROR_CHECK(result);
+
+	return ML_SUCCESS;
+}
+
+static int kxud9_read(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata,
+		      unsigned char *data)
+{
+	int result;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->len, data);
+	return result;
+}
+
+static struct ext_slave_descr kxud9_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ kxud9_suspend,
+	/*.resume           = */ kxud9_resume,
+	/*.read             = */ kxud9_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "kxud9",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ /* ACCEL_ID_KXUD9, */ ACCEL_ID_KXSD9,
+	/*.reg              = */ 0x00,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {2, 5006},
+};
+
+struct ext_slave_descr *kxud9_get_slave_descr(void)
+{
+	return &kxud9_descr;
+}
+EXPORT_SYMBOL(kxud9_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/accel/lis331.c b/drivers/misc/mpu3050/accel/lis331.c
new file mode 100755
index 0000000..53c599b
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/lis331.c
@@ -0,0 +1,617 @@
+/*
+ $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   lis331.c
+ *      @brief  Accelerometer setup and handling methods for ST LIS331
+ */
+
+/* ------------------ */
+/* - 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"
+
+/* full scale setting - register & mask */
+#define LIS331_CTRL_REG1         (0x20)
+#define LIS331_CTRL_REG2         (0x21)
+#define LIS331_CTRL_REG3         (0x22)
+#define LIS331_CTRL_REG4         (0x23)
+#define LIS331_CTRL_REG5         (0x24)
+#define LIS331_HP_FILTER_RESET   (0x25)
+#define LIS331_REFERENCE         (0x26)
+#define LIS331_STATUS_REG        (0x27)
+#define LIS331_OUT_X_L           (0x28)
+#define LIS331_OUT_X_H           (0x29)
+#define LIS331_OUT_Y_L           (0x2a)
+#define LIS331_OUT_Y_H           (0x2b)
+#define LIS331_OUT_Z_L           (0x2b)
+#define LIS331_OUT_Z_H           (0x2d)
+
+#define LIS331_INT1_CFG          (0x30)
+#define LIS331_INT1_SRC          (0x31)
+#define LIS331_INT1_THS          (0x32)
+#define LIS331_INT1_DURATION     (0x33)
+
+#define LIS331_INT2_CFG          (0x34)
+#define LIS331_INT2_SRC          (0x35)
+#define LIS331_INT2_THS          (0x36)
+#define LIS331_INT2_DURATION     (0x37)
+
+#define LIS331_CTRL_MASK         (0x30)
+#define LIS331_SLEEP_MASK        (0x20)
+
+#define LIS331_MAX_DUR (0x7F)
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+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;
+};
+
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+static int lis331dlh_set_ths(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct lis331dlh_config *config,
+			int apply,
+			long ths)
+{
+	int result = ML_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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					LIS331_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 = ML_SUCCESS;
+	long reg_dur = (dur * config->odr) / 1000000L;
+	config->dur = dur;
+
+	if (reg_dur > LIS331_MAX_DUR)
+		reg_dur = LIS331_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,
+					LIS331_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 = ML_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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					LIS331_CTRL_REG3, reg1);
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					LIS331_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 = ML_SUCCESS;
+
+	if (odr > 400000) {
+		config->odr = 1000000;
+		bits = 0x38;
+	} else if (odr > 100000) {
+		config->odr = 400000;
+		bits = 0x30;
+	} else if (odr > 50000) {
+		config->odr = 100000;
+		bits = 0x28;
+	} else if (odr > 10000) {
+		config->odr = 50000;
+		bits = 0x20;
+	} else if (odr > 5000) {
+		config->odr = 10000;
+		bits = 0xC0;
+	} else if (odr > 2000) {
+		config->odr = 5000;
+		bits = 0xB0;
+	} 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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					LIS331_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 = ML_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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					LIS331_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 = ML_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lis331dlh_private_data *private_data = pdata->private_data;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_CTRL_REG1,
+				       private_data->suspend.ctrl_reg1);
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_CTRL_REG4, reg1);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_INT1_THS,
+				       private_data->suspend.reg_ths);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_CTRL_REG3, reg1);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_INT1_CFG, reg2);
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				LIS331_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 = ML_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lis331dlh_private_data *private_data = pdata->private_data;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_CTRL_REG1,
+				       private_data->resume.ctrl_reg1);
+	ERROR_CHECK(result);
+	MLOSSleep(6);
+
+	/* Full Scale */
+	reg1 = 0x40;
+	if (private_data->resume.fsr == 8192)
+		reg1 |= 0x30;
+	else if (private_data->resume.fsr == 4096)
+		reg1 |= 0x10;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_CTRL_REG4, reg1);
+	ERROR_CHECK(result);
+
+	/* Configure high pass filter */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_CTRL_REG2, 0x0F);
+	ERROR_CHECK(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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_CTRL_REG3, reg1);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_INT1_THS,
+				       private_data->resume.reg_ths);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_INT1_DURATION,
+				       private_data->resume.reg_dur);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS331_INT1_CFG, reg2);
+	ERROR_CHECK(result);
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				LIS331_HP_FILTER_RESET, 1, &reg1);
+	ERROR_CHECK(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 = ML_SUCCESS;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				LIS331_STATUS_REG, 1, data);
+	if (data[0] & 0x0F) {
+		result = MLSLSerialRead(mlsl_handle, pdata->address,
+					slave->reg, slave->len, data);
+		return result;
+	} else
+		return ML_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;
+	private_data = (struct lis331dlh_private_data *)
+		MLOSMalloc(sizeof(struct lis331dlh_private_data));
+
+	if (!private_data)
+		return ML_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);
+	lis331dlh_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+			FALSE, 2048);
+	lis331dlh_set_fsr(mlsl_handle, pdata, &private_data->resume,
+			FALSE, 2048);
+	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 ML_SUCCESS;
+}
+
+static int lis331dlh_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 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 ML_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:
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return ML_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 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 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_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_LIS331,
+	/*.reg              = */ (0x28 | 0x80), /* 0x80 for burst reads */
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {2, 480},
+};
+
+struct ext_slave_descr *lis331dlh_get_slave_descr(void)
+{
+	return &lis331dlh_descr;
+}
+EXPORT_SYMBOL(lis331dlh_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/accel/lis3dh.c b/drivers/misc/mpu3050/accel/lis3dh.c
new file mode 100755
index 0000000..594cd42
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/lis3dh.c
@@ -0,0 +1,625 @@
+/*
+ $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   lis3dh.c
+ *      @brief  Accelerometer setup and handling methods for ST LIS3DH
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0
+
+#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"
+
+/* 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           (0x2b)
+#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)
+
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+struct lis3dh_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 lis3dh_private_data {
+	struct lis3dh_config suspend;
+	struct lis3dh_config resume;
+};
+
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+static int lis3dh_set_ths(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct lis3dh_config *config,
+			int apply,
+			long ths)
+{
+	int result = ML_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 = MLSLSerialWriteSingle(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 = ML_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 = MLSLSerialWriteSingle(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 = ML_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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					LIS3DH_CTRL_REG3, reg1);
+		result = MLSLSerialWriteSingle(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 = ML_SUCCESS;
+
+	if (odr > 400000) {
+		config->odr = 1250000;
+		bits = 0x90;
+	} else if (odr > 200000) {
+		config->odr = 400000;
+		bits = 0x70;
+	} else if (odr > 100000) {
+		config->odr = 200000;
+		bits = 0x60;
+	} else if (odr > 50000) {
+		config->odr = 100000;
+		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: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+	if (apply)
+		result = MLSLSerialWriteSingle(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 = ML_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 = MLSLSerialWriteSingle(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 = ML_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lis3dh_private_data *private_data = pdata->private_data;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_CTRL_REG1,
+				       private_data->suspend.ctrl_reg1);
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_CTRL_REG2, 0x31);
+	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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_CTRL_REG4, reg1);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_INT1_THS,
+				       private_data->suspend.reg_ths);
+	result = MLSLSerialWriteSingle(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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_CTRL_REG3, reg1);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_INT1_CFG, reg2);
+	result = MLSLSerialRead(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)
+{
+	tMLError result;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lis3dh_private_data *private_data = pdata->private_data;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_CTRL_REG1,
+				       private_data->resume.ctrl_reg1);
+	ERROR_CHECK(result);
+	MLOSSleep(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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_CTRL_REG4, reg1);
+	ERROR_CHECK(result);
+
+	/* Configure high pass filter */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_CTRL_REG2, 0x31);
+	ERROR_CHECK(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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_CTRL_REG3, reg1);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_INT1_THS,
+				       private_data->resume.reg_ths);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_INT1_DURATION,
+				       private_data->resume.reg_dur);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_INT1_CFG, reg2);
+	ERROR_CHECK(result);
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				LIS3DH_CTRL_REG6, 1, &reg1);
+	ERROR_CHECK(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 = ML_SUCCESS;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				LIS3DH_STATUS_REG, 1, data);
+	if (data[0] & 0x0F) {
+		result = MLSLSerialRead(mlsl_handle, pdata->address,
+					slave->reg, slave->len, data);
+		return result;
+	} else
+		return ML_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+static int lis3dh_init(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	tMLError result;
+
+	struct lis3dh_private_data *private_data;
+	private_data = (struct lis3dh_private_data *)
+		MLOSMalloc(sizeof(struct lis3dh_private_data));
+
+	if (!private_data)
+		return ML_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, 200000);
+	lis3dh_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+			FALSE, 2048);
+	lis3dh_set_fsr(mlsl_handle, pdata, &private_data->resume,
+			FALSE, 2048);
+	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 = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       LIS3DH_CTRL_REG1, 0x07);
+	MLOSSleep(6);
+
+	return ML_SUCCESS;
+}
+
+static int lis3dh_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 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 ML_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 ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+	return ML_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 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 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_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_LIS3DH,
+	/*.reg              = */ 0x28 | 0x80, /* 0x80 for burst reads */
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {2, 480},
+};
+
+struct ext_slave_descr *lis3dh_get_slave_descr(void)
+{
+	return &lis3dh_descr;
+}
+EXPORT_SYMBOL(lis3dh_get_slave_descr);
+
+/*
+ *  @}
+*/
diff --git a/drivers/misc/mpu3050/accel/lsm303a.c b/drivers/misc/mpu3050/accel/lsm303a.c
new file mode 100755
index 0000000..b849496
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/lsm303a.c
@@ -0,0 +1,178 @@
+/*
+ $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   lsm303a.c
+ *      @brief  Accelerometer setup and handling methods for ST LSM303
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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 ACCEL_ST_SLEEP_REG          (0x20)
+#define ACCEL_ST_SLEEP_MASK         (0x20)
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+int lsm303dlha_suspend(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, ACCEL_ST_SLEEP_REG,
+			   1, &reg);
+	ERROR_CHECK(result);
+	reg &= ~(0x27);
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_ST_SLEEP_REG, reg);
+	ERROR_CHECK(result);
+	return result;
+}
+
+/* full scale setting - register & mask */
+#define ACCEL_ST_CTRL_REG          (0x23)
+#define ACCEL_ST_CTRL_MASK         (0x30)
+
+int lsm303dlha_resume(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+	unsigned char reg;
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, ACCEL_ST_SLEEP_REG,
+			   1, &reg);
+	ERROR_CHECK(result);
+	reg |= 0x27;
+	/*wake up if sleeping */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_ST_SLEEP_REG, reg);
+	ERROR_CHECK(result);
+
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x20, 0x37);
+	ERROR_CHECK(result);
+	MLOSSleep(500);
+
+	reg = 0x40;
+
+	/* Full Scale */
+	reg &= ~ACCEL_ST_CTRL_MASK;
+	if (slave->range.mantissa == 4) {
+		slave->range.fraction = 960;
+		reg |= 0x10;
+	} else if (slave->range.mantissa == 8) {
+		slave->range.fraction = 1920;
+		reg |= 0x30;
+	} else {
+		slave->range.mantissa = 2;
+		slave->range.fraction = 480;
+		reg |= 0x00;
+	}
+
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x23, reg);
+	ERROR_CHECK(result);
+
+	/* Configure high pass filter */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x21, 0x0F);
+	ERROR_CHECK(result);
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x32, 0x00);
+	ERROR_CHECK(result);
+	/* Configure INT1_DURATION */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x33, 0x7F);
+	ERROR_CHECK(result);
+	/* Configure INT1_CFG */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x30, 0x95);
+	ERROR_CHECK(result);
+	MLOSSleep(50);
+	return result;
+}
+
+int lsm303dlha_read(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata,
+		    unsigned char *data)
+{
+	int result;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->len, data);
+	return result;
+}
+
+struct ext_slave_descr lsm303dlha_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ lsm303dlha_suspend,
+	/*.resume           = */ lsm303dlha_resume,
+	/*.read             = */ lsm303dlha_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "lsm303dlha",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_LSM303,
+	/*.reg              = */ (0x28 | 0x80), /* 0x80 for burst reads */
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {2, 480},
+};
+
+struct ext_slave_descr *lsm303dlha_get_slave_descr(void)
+{
+	return &lsm303dlha_descr;
+}
+EXPORT_SYMBOL(lsm303dlha_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/accel/mantis.c b/drivers/misc/mpu3050/accel/mantis.c
new file mode 100755
index 0000000..1cb9847
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/mantis.c
@@ -0,0 +1,306 @@
+/*
+ $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   lis331.c
+ *      @brief  Accelerometer setup and handling methods for Invensense MANTIS
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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"
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+struct mantis_config {
+	unsigned int odr; /* output data rate 1/1000 Hz*/
+	unsigned int fsr; /* full scale range mg */
+	unsigned int ths; /* Motion no-motion thseshold mg */
+	unsigned int dur; /* Motion no-motion duration ms */
+};
+
+struct mantis_private_data {
+	struct mantis_config suspend;
+	struct mantis_config resume;
+};
+
+
+/*****************************************
+ *Accelerometer Initialization Functions
+ *****************************************/
+/**
+ * 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
+ */
+void mantis_set_odr(struct mantis_config *config,
+		long odr)
+{
+	config->odr = odr;
+}
+
+void mantis_set_ths(struct mantis_config *config,
+		long ths)
+{
+	if (ths < 0)
+		ths = 0;
+
+	config->ths = ths;
+	MPL_LOGV("THS: %d\n", config->ths);
+}
+
+void mantis_set_dur(struct mantis_config *config,
+		long dur)
+{
+	if (dur < 0)
+		dur = 0;
+
+	config->dur = dur;
+	MPL_LOGV("DUR: %d\n", config->dur);
+}
+
+static void mantis_set_fsr(
+	struct mantis_config *config,
+	long fsr)
+{
+	if (fsr <= 2000)
+		config->fsr = 2000;
+	else if (fsr <= 4000)
+		config->fsr = 4000;
+	else
+		config->fsr = 8000;
+
+	MPL_LOGV("FSR: %d\n", config->fsr);
+}
+
+static int mantis_init(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	struct mantis_private_data *private_data;
+	private_data = (struct mantis_private_data *)
+		MLOSMalloc(sizeof(struct mantis_private_data));
+
+	if (!private_data)
+		return ML_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	mantis_set_odr(&private_data->suspend, 0);
+	mantis_set_odr(&private_data->resume, 200000);
+	mantis_set_fsr(&private_data->suspend, 2000);
+	mantis_set_fsr(&private_data->resume, 2000);
+	mantis_set_ths(&private_data->suspend, 80);
+	mantis_set_ths(&private_data->resume, 40);
+	mantis_set_dur(&private_data->suspend, 1000);
+	mantis_set_dur(&private_data->resume,  2540);
+	return ML_SUCCESS;
+}
+
+static int mantis_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;
+}
+
+int mantis_suspend(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	unsigned char reg;
+	int result;
+
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				MPUREG_PWR_MGMT_2, 1, &reg);
+	ERROR_CHECK(result);
+	reg |= (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				MPUREG_PWR_MGMT_2, reg);
+	ERROR_CHECK(result);
+
+	return ML_SUCCESS;
+}
+
+int mantis_resume(void *mlsl_handle,
+		  struct ext_slave_descr *slave,
+		  struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+	unsigned char reg;
+	struct mantis_private_data *private_data;
+
+	private_data = (struct mantis_private_data *) pdata->private_data;
+
+	MLSLSerialRead(mlsl_handle, pdata->address,
+		MPUREG_PWR_MGMT_2, 1, &reg);
+
+	reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				MPUREG_PWR_MGMT_2, reg);
+	ERROR_CHECK(result);
+
+	if (slave->range.mantissa == 2)
+		reg = 0;
+	else if (slave->range.mantissa == 4)
+		reg = 1 << 3;
+	else if (slave->range.mantissa == 8)
+		reg = 2 << 3;
+	else if (slave->range.mantissa == 16)
+		reg = 3 << 3;
+	else
+		return ML_ERROR;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				       MPUREG_ACCEL_CONFIG, reg);
+	ERROR_CHECK(result);
+
+	reg = (unsigned char) private_data->suspend.ths / ACCEL_MOT_THR_LSB;
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				MPUREG_ACCEL_MOT_THR, reg);
+	ERROR_CHECK(result);
+
+	reg = (unsigned char)
+		ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				MPUREG_ACCEL_ZRMOT_THR, reg);
+	ERROR_CHECK(result);
+
+	reg = (unsigned char) private_data->suspend.ths / ACCEL_MOT_DUR_LSB;
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				MPUREG_ACCEL_MOT_DUR, reg);
+	ERROR_CHECK(result);
+
+	reg = (unsigned char) private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB;
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				MPUREG_ACCEL_ZRMOT_DUR, reg);
+	ERROR_CHECK(result);
+	return result;
+}
+
+int mantis_read(void *mlsl_handle,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+	int result;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->len, data);
+	return result;
+}
+
+static int mantis_config(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config *data)
+{
+	struct mantis_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return ML_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		mantis_set_odr(&private_data->suspend,
+				*((long *)data->data));
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		mantis_set_odr(&private_data->resume,
+				*((long *)data->data));
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		mantis_set_fsr(&private_data->suspend,
+				*((long *)data->data));
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		mantis_set_fsr(&private_data->resume,
+				*((long *)data->data));
+		break;
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		mantis_set_ths(&private_data->suspend,
+			       (*((long *)data->data)));
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		mantis_set_ths(&private_data->resume,
+			       (*((long *)data->data)));
+		break;
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		mantis_set_dur(&private_data->suspend,
+			       (*((long *)data->data)));
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		mantis_set_dur(&private_data->resume,
+			       (*((long *)data->data)));
+		break;
+	default:
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return ML_SUCCESS;
+}
+
+struct ext_slave_descr mantis_descr = {
+	/*.init             = */ mantis_init,
+	/*.exit             = */ mantis_exit,
+	/*.suspend          = */ mantis_suspend,
+	/*.resume           = */ mantis_resume,
+	/*.read             = */ mantis_read,
+	/*.config           = */ mantis_config,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "mantis",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_MPU6000,
+	/*.reg              = */ 0xA8,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {2, 0},
+};
+
+struct ext_slave_descr *mantis_get_slave_descr(void)
+{
+	return &mantis_descr;
+}
+EXPORT_SYMBOL(mantis_get_slave_descr);
+
+/**
+ *  @}
+ */
+
diff --git a/drivers/misc/mpu3050/accel/mma8450.c b/drivers/misc/mpu3050/accel/mma8450.c
new file mode 100755
index 0000000..b5b3728
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/mma8450.c
@@ -0,0 +1,156 @@
+/*
+ $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   mma8450.c
+ *      @brief  Accelerometer setup and handling methods for Freescale MMA8450
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <stdlib.h>
+#include "mpu.h"
+#include "mlsl.h"
+#include "mlos.h"
+#include <string.h>
+
+#include <log.h>
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+#define ACCEL_MMA8450_SLEEP_REG      (0x38)
+#define ACCEL_MMA8450_SLEEP_MASK     (0x3)
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+int mma8450_suspend(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address,
+			   ACCEL_MMA8450_SLEEP_REG, 1, &reg);
+	ERROR_CHECK(result);
+	reg &= ~ACCEL_MMA8450_SLEEP_MASK;
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_MMA8450_SLEEP_REG, reg);
+	ERROR_CHECK(result);
+	return result;
+}
+
+/* full scale setting - register & mask */
+#define ACCEL_MMA8450_CTRL_REG      (0x38)
+#define ACCEL_MMA8450_CTRL_MASK     (0x3)
+
+int mma8450_resume(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+	unsigned char reg;
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address,
+			   ACCEL_MMA8450_CTRL_REG, 1, &reg);
+	ERROR_CHECK(result);
+
+	/* data rate = 200Hz */
+	reg &= 0xE3;
+	reg |= 0x4;
+
+	/* Full Scale */
+	reg &= ~ACCEL_MMA8450_CTRL_MASK;
+	if (slave->range.mantissa == 4)
+		reg |= 0x2;
+	else if (slave->range.mantissa == 8)
+		reg |= 0x3;
+	else {
+		slave->range.mantissa = 2;
+		reg |= 0x1;
+	}
+	slave->range.fraction = 0;
+
+	/* XYZ_DATA_CFG: event flag enabled on all axis */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x16, 0x05);
+	ERROR_CHECK(result);
+	/* CTRL_REG1: rate + scale config + wakeup */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_MMA8450_CTRL_REG, reg);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+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 = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, sizeof(local_data), local_data);
+	ERROR_CHECK(result);
+	memcpy(data, &local_data[1], (slave->len) - 1);
+	return result;
+}
+
+struct ext_slave_descr mma8450_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ mma8450_suspend,
+	/*.resume           = */ mma8450_resume,
+	/*.read             = */ mma8450_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "mma8450",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_MMA8450,
+	/*.reg              = */ 0x00,
+	/*.len              = */ 4,
+	/*.endian           = */ EXT_SLAVE_FS8_BIG_ENDIAN,
+	/*.range            = */ {2, 0},
+};
+
+struct ext_slave_descr *mma8450_get_slave_descr(void)
+{
+	return &mma8450_descr;
+}
+
+EXPORT_SYMBOL(mma8450_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/accel/mma845x.c b/drivers/misc/mpu3050/accel/mma845x.c
new file mode 100755
index 0000000..27150ad
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/mma845x.c
@@ -0,0 +1,158 @@
+/*
+ $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   mma845x.c
+ *      @brief  Accelerometer setup and handling methods for Freescale MMA845X
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#ifdef __KERNEL__
+#include <linux/module.h>
+#endif
+
+#include <stdlib.h>
+#include "mpu.h"
+#include "mlsl.h"
+#include "mlos.h"
+#include <string.h>
+
+#include <log.h>
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+#define ACCEL_MMA845X_CTRL_REG1      (0x2A)
+#define ACCEL_MMA845X_SLEEP_MASK     (0x01)
+
+/* full scale setting - register & mask */
+#define ACCEL_MMA845X_CFG_REG       (0x0E)
+#define ACCEL_MMA845X_CTRL_MASK     (0x03)
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+int mma845x_suspend(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address,
+			   ACCEL_MMA845X_CTRL_REG1, 1, &reg);
+	ERROR_CHECK(result);
+	reg &= ~ACCEL_MMA845X_SLEEP_MASK;
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  ACCEL_MMA845X_CTRL_REG1, reg);
+	ERROR_CHECK(result);
+	return result;
+}
+
+
+int mma845x_resume(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+	unsigned char reg;
+
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				ACCEL_MMA845X_CFG_REG, 1, &reg);
+	ERROR_CHECK(result);
+
+	/* data rate = 200Hz */
+
+	/* Full Scale */
+	reg &= ~ACCEL_MMA845X_CTRL_MASK;
+	if (slave->range.mantissa == 4)
+		reg |= 0x1;
+	else if (slave->range.mantissa == 8)
+		reg |= 0x2;
+	else {
+		slave->range.mantissa = 2;
+		reg |= 0x0;
+	}
+	slave->range.fraction = 0;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				ACCEL_MMA845X_CFG_REG, reg);
+	ERROR_CHECK(result);
+	/* 200Hz + active mode */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+			    ACCEL_MMA845X_CTRL_REG1, 0x11);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+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 = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, sizeof(local_data), local_data);
+	ERROR_CHECK(result);
+	memcpy(data, &local_data[1], slave->len);
+	return result;
+}
+
+struct ext_slave_descr mma845x_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ mma845x_suspend,
+	/*.resume           = */ mma845x_resume,
+	/*.read             = */ mma845x_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "mma845x",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_MMA845X,
+	/*.reg              = */ 0x00,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_FS16_BIG_ENDIAN,
+	/*.range            = */ {2, 0},
+};
+
+struct ext_slave_descr *mma845x_get_slave_descr(void)
+{
+	return &mma845x_descr;
+}
+EXPORT_SYMBOL(mma845x_get_slave_descr);
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/compass/ami304.c b/drivers/misc/mpu3050/compass/ami304.c
new file mode 100755
index 0000000..5c33861
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/ami304.c
@@ -0,0 +1,164 @@
+/*
+ $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   ami304.c
+ *     @brief  Magnetometer setup and handling methods for Aichi ami304 compass.
+*/
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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 AMI304_REG_DATAX (0x10)
+#define AMI304_REG_STAT1 (0x18)
+#define AMI304_REG_CNTL1 (0x1B)
+#define AMI304_REG_CNTL2 (0x1C)
+#define AMI304_REG_CNTL3 (0x1D)
+
+#define AMI304_BIT_CNTL1_PC1  (0x80)
+#define AMI304_BIT_CNTL1_ODR1 (0x10)
+#define AMI304_BIT_CNTL1_FS1  (0x02)
+
+#define AMI304_BIT_CNTL2_IEN  (0x10)
+#define AMI304_BIT_CNTL2_DREN (0x08)
+#define AMI304_BIT_CNTL2_DRP  (0x04)
+#define AMI304_BIT_CNTL3_F0RCE (0x40)
+
+int ami304_suspend(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, AMI304_REG_CNTL1,
+			   1, &reg);
+	ERROR_CHECK(result);
+
+	reg &= ~(AMI304_BIT_CNTL1_PC1|AMI304_BIT_CNTL1_FS1);
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AMI304_REG_CNTL1, reg);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+int ami304_resume(void *mlsl_handle,
+		  struct ext_slave_descr *slave,
+		  struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	/* Set CNTL1 reg to power model active */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AMI304_REG_CNTL1,
+				  AMI304_BIT_CNTL1_PC1|AMI304_BIT_CNTL1_FS1);
+	ERROR_CHECK(result);
+	/* Set CNTL2 reg to DRDY active high and enabled */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AMI304_REG_CNTL2,
+				  AMI304_BIT_CNTL2_DREN |
+				  AMI304_BIT_CNTL2_DRP);
+	ERROR_CHECK(result);
+	/* Set CNTL3 reg to forced measurement period */
+	result =
+		MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AMI304_REG_CNTL3, AMI304_BIT_CNTL3_F0RCE);
+
+	return result;
+}
+
+int ami304_read(void *mlsl_handle,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+	unsigned char stat;
+	int result = ML_SUCCESS;
+
+	/* Read status reg and check if data ready (DRDY) */
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, AMI304_REG_STAT1,
+			   1, &stat);
+	ERROR_CHECK(result);
+
+	if (stat & 0x40) {
+		result =
+		    MLSLSerialRead(mlsl_handle, pdata->address,
+				   AMI304_REG_DATAX, 6,
+				   (unsigned char *) data);
+		ERROR_CHECK(result);
+		/* start another measurement */
+		result =
+			MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					      AMI304_REG_CNTL3,
+					      AMI304_BIT_CNTL3_F0RCE);
+		ERROR_CHECK(result);
+
+		return ML_SUCCESS;
+	}
+
+	return ML_ERROR_COMPASS_DATA_NOT_READY;
+}
+
+struct ext_slave_descr ami304_descr = {
+	/*.suspend          = */ ami304_suspend,
+	/*.resume           = */ ami304_resume,
+	/*.read             = */ ami304_read,
+	/*.name             = */ "ami304",
+	/*.type             = */ EXT_SLAVE_TYPE_COMPASS,
+	/*.id               = */ COMPASS_ID_AICHI,
+	/*.reg              = */ 0x06,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ {5461, 3333}
+};
+
+struct ext_slave_descr *ami304_get_slave_descr(void)
+{
+	return &ami304_descr;
+}
+
+#ifdef __KERNEL__
+EXPORT_SYMBOL(ami304_get_slave_descr);
+#endif
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/ami30x.c b/drivers/misc/mpu3050/compass/ami30x.c
new file mode 100755
index 0000000..5e4a33e
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/ami30x.c
@@ -0,0 +1,167 @@
+/*
+ $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   ami30x.c
+ *     @brief  Magnetometer setup and handling methods for Aichi AMI304/AMI305
+ *             compass.
+*/
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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 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)
+
+int ami30x_suspend(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, AMI30X_REG_CNTL1,
+			   1, &reg);
+	ERROR_CHECK(result);
+
+	reg &= ~(AMI30X_BIT_CNTL1_PC1|AMI30X_BIT_CNTL1_FS1);
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AMI30X_REG_CNTL1, reg);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+int ami30x_resume(void *mlsl_handle,
+		  struct ext_slave_descr *slave,
+		  struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	/* Set CNTL1 reg to power model active */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AMI30X_REG_CNTL1,
+				  AMI30X_BIT_CNTL1_PC1|AMI30X_BIT_CNTL1_FS1);
+	ERROR_CHECK(result);
+	/* Set CNTL2 reg to DRDY active high and enabled */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AMI30X_REG_CNTL2,
+				  AMI30X_BIT_CNTL2_DREN |
+				  AMI30X_BIT_CNTL2_DRP);
+	ERROR_CHECK(result);
+	/* Set CNTL3 reg to forced measurement period */
+	result =
+		MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AMI30X_REG_CNTL3, AMI30X_BIT_CNTL3_F0RCE);
+
+	return result;
+}
+
+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 = ML_SUCCESS;
+
+	/* Read status reg and check if data ready (DRDY) */
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, AMI30X_REG_STAT1,
+			   1, &stat);
+	ERROR_CHECK(result);
+
+	if (stat & 0x40) {
+		result =
+		    MLSLSerialRead(mlsl_handle, pdata->address,
+				   AMI30X_REG_DATAX, 6,
+				   (unsigned char *) data);
+		ERROR_CHECK(result);
+		/* start another measurement */
+		result =
+			MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					      AMI30X_REG_CNTL3,
+					      AMI30X_BIT_CNTL3_F0RCE);
+		ERROR_CHECK(result);
+
+		return ML_SUCCESS;
+	}
+
+	return ML_ERROR_COMPASS_DATA_NOT_READY;
+}
+
+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,
+	/*.reg              = */ 0x06,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ {5461, 3333}
+	/* For AMI305,the range field needs to be modified to {9830.4f}*/
+};
+
+struct ext_slave_descr *ami30x_get_slave_descr(void)
+{
+	return &ami30x_descr;
+}
+EXPORT_SYMBOL(ami30x_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/hmc5883.c b/drivers/misc/mpu3050/compass/hmc5883.c
new file mode 100755
index 0000000..051b071
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/hmc5883.c
@@ -0,0 +1,254 @@
+/*
+ $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/>.
+  $
+ */
+
+/**
+ *  @brief      Provides the interface to setup and handle a compass
+ *              connected to the primary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   hmc5883.c
+ *      @brief  Magnetometer setup and handling methods for honeywell hmc5883
+ *              compass.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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"
+
+/*-----HONEYWELL HMC5883 Registers ------*/
+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
+};
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+int hmc5883_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,
+				  HMC_REG_MODE, HMC_MODE_SLEEP);
+	ERROR_CHECK(result);
+	MLOSSleep(3);
+
+	return result;
+}
+
+int hmc5883_resume(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	/* Use single measurement mode. Start at sleep state. */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  HMC_REG_MODE, HMC_MODE_SLEEP);
+	ERROR_CHECK(result);
+	/* Config normal measurement */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  HMC_REG_CONF_A, 0);
+	ERROR_CHECK(result);
+	/* Adjust gain to 307 LSB/Gauss */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  HMC_REG_CONF_B, HMC_CONF_B_GAIN_5_5);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+int hmc5883_read(void *mlsl_handle,
+		 struct ext_slave_descr *slave,
+		 struct ext_slave_platform_data *pdata,
+		 unsigned char *data)
+{
+	unsigned char stat;
+	tMLError result = ML_SUCCESS;
+	unsigned char tmp;
+	short axisFixed;
+
+	/* Read status reg. to check if data is ready */
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, HMC_REG_STATUS, 1,
+			   &stat);
+	ERROR_CHECK(result);
+	if (stat & 0x01) {
+		result =
+		    MLSLSerialRead(mlsl_handle, pdata->address,
+				   HMC_REG_X_M, 6, (unsigned char *) data);
+		ERROR_CHECK(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 =
+			    MLSLSerialWriteSingle(mlsl_handle,
+							pdata->address,
+							HMC_REG_MODE,
+							HMC_MODE_SINGLE);
+			ERROR_CHECK(result);
+			return ML_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 =
+		    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					  HMC_REG_MODE, HMC_MODE_SINGLE);
+		ERROR_CHECK(result);
+
+		return ML_SUCCESS;
+	} else {
+		/* trigger next measurement read */
+		result =
+		    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					  HMC_REG_MODE, HMC_MODE_SINGLE);
+		ERROR_CHECK(result);
+
+		return ML_ERROR_COMPASS_DATA_NOT_READY;
+	}
+}
+
+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,
+	/*.reg              = */ 0x06,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {10673, 6156},
+};
+
+struct ext_slave_descr *hmc5883_get_slave_descr(void)
+{
+	return &hmc5883_descr;
+}
+EXPORT_SYMBOL(hmc5883_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/hscdtd002b.c b/drivers/misc/mpu3050/compass/hscdtd002b.c
new file mode 100755
index 0000000..bf26cae
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/hscdtd002b.c
@@ -0,0 +1,163 @@
+/*
+ $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/>.
+  $
+ */
+
+/**
+ *  @brief      Provides the interface to setup and handle a compass
+ *              connected to the primary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   hscdtd002b.c
+ *      @brief  Magnetometer setup and handling methods for Alps hscdtd002b
+ *              compass.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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"
+
+/*----- ALPS HSCDTD002B Registers ------*/
+#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)
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Compass Initialization Functions
+*****************************************/
+
+int hscdtd002b_suspend(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	/* Power mode: stand-by */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  COMPASS_HSCDTD002B_CTRL1, 0x00);
+	ERROR_CHECK(result);
+	MLOSSleep(1);		/* turn-off time */
+
+	return result;
+}
+
+int hscdtd002b_resume(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	/* Soft reset */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  COMPASS_HSCDTD002B_CTRL3, 0x80);
+	ERROR_CHECK(result);
+	/* Force state; Power mode: active */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  COMPASS_HSCDTD002B_CTRL1, 0x82);
+	ERROR_CHECK(result);
+	/* Data ready enable */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  COMPASS_HSCDTD002B_CTRL2, 0x08);
+	ERROR_CHECK(result);
+	MLOSSleep(1);		/* turn-on time */
+
+	return result;
+}
+
+int hscdtd002b_read(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata,
+		    unsigned char *data)
+{
+	unsigned char stat;
+	tMLError result = ML_SUCCESS;
+	int status = ML_SUCCESS;
+
+	/* Read status reg. to check if data is ready */
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address,
+			   COMPASS_HSCDTD002B_STAT, 1, &stat);
+	ERROR_CHECK(result);
+	if (stat & 0x40) {
+		result =
+		    MLSLSerialRead(mlsl_handle, pdata->address,
+				   COMPASS_HSCDTD002B_DATAX, 6,
+				   (unsigned char *) data);
+		ERROR_CHECK(result);
+		status = ML_SUCCESS;
+	} else if (stat & 0x20) {
+		status = ML_ERROR_COMPASS_DATA_OVERFLOW;
+	} else {
+		status = ML_ERROR_COMPASS_DATA_NOT_READY;
+	}
+	/* trigger next measurement read */
+	result =
+		MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+			  COMPASS_HSCDTD002B_CTRL3, 0x40);
+	ERROR_CHECK(result);
+
+	return status;
+}
+
+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,
+	/*.reg              = */ 0x10,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ {9830, 4000},
+};
+
+struct ext_slave_descr *hscdtd002b_get_slave_descr(void)
+{
+	return &hscdtd002b_descr;
+}
+EXPORT_SYMBOL(hscdtd002b_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/hscdtd004a.c b/drivers/misc/mpu3050/compass/hscdtd004a.c
new file mode 100755
index 0000000..43fc14a
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/hscdtd004a.c
@@ -0,0 +1,162 @@
+/*
+ $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/>.
+  $
+ */
+
+/**
+ *  @brief      Provides the interface to setup and handle a compass
+ *              connected to the primary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   hscdtd004a.c
+ *      @brief  Magnetometer setup and handling methods for Alps hscdtd004a
+ *              compass.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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"
+
+/*----- ALPS HSCDTD004A Registers ------*/
+#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)
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Compass Initialization Functions
+*****************************************/
+
+int hscdtd004a_suspend(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	/* Power mode: stand-by */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  COMPASS_HSCDTD004A_CTRL1, 0x00);
+	ERROR_CHECK(result);
+	MLOSSleep(1);		/* turn-off time */
+
+	return result;
+}
+
+int hscdtd004a_resume(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	/* Soft reset */
+	result =
+		MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  COMPASS_HSCDTD004A_CTRL3, 0x80);
+	ERROR_CHECK(result);
+	/* Normal state; Power mode: active */
+	result =
+		MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  COMPASS_HSCDTD004A_CTRL1, 0x82);
+	ERROR_CHECK(result);
+	/* Data ready enable */
+	result =
+		MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  COMPASS_HSCDTD004A_CTRL2, 0x7C);
+	ERROR_CHECK(result);
+	MLOSSleep(1);		/* turn-on time */
+	return result;
+}
+
+int hscdtd004a_read(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata,
+		    unsigned char *data)
+{
+	unsigned char stat;
+	tMLError result = ML_SUCCESS;
+	int status = ML_SUCCESS;
+
+	/* Read status reg. to check if data is ready */
+	result =
+		MLSLSerialRead(mlsl_handle, pdata->address,
+			   COMPASS_HSCDTD004A_STAT, 1, &stat);
+	ERROR_CHECK(result);
+	if (stat & 0x48) {
+		result =
+			 MLSLSerialRead(mlsl_handle, pdata->address,
+				   COMPASS_HSCDTD004A_DATAX, 6,
+				   (unsigned char *) data);
+		ERROR_CHECK(result);
+		status = ML_SUCCESS;
+	} else if (stat & 0x68) {
+		status = ML_ERROR_COMPASS_DATA_OVERFLOW;
+	} else {
+		status = ML_ERROR_COMPASS_DATA_NOT_READY;
+	}
+	/* trigger next measurement read */
+	result =
+		MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+			COMPASS_HSCDTD004A_CTRL3, 0x40);
+	ERROR_CHECK(result);
+	return status;
+
+}
+
+struct ext_slave_descr hscdtd004a_descr = {
+	/*.init             = */ NULL,
+	/*.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,
+	/*.reg              = */ 0x10,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ {9830, 4000},
+};
+
+struct ext_slave_descr *hscdtd004a_get_slave_descr(void)
+{
+	return &hscdtd004a_descr;
+}
+EXPORT_SYMBOL(hscdtd004a_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/lsm303m.c b/drivers/misc/mpu3050/compass/lsm303m.c
new file mode 100755
index 0000000..871d002
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/lsm303m.c
@@ -0,0 +1,244 @@
+/*
+ $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/>.
+  $
+ */
+
+/**
+ *  @brief      Provides the interface to setup and handle a compass
+ *              connected to the primary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   lsm303m.c
+ *      @brief  Magnetometer setup and handling methods for ST LSM303.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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"
+
+/*----- ST LSM303 Registers ------*/
+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
+};
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+int lsm303dlhm_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,
+				  LSM_REG_MODE, LSM_MODE_SLEEP);
+	ERROR_CHECK(result);
+	MLOSSleep(3);
+
+	return result;
+}
+
+int lsm303dlhm_resume(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	/* Use single measurement mode. Start at sleep state. */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  LSM_REG_MODE, LSM_MODE_SLEEP);
+	ERROR_CHECK(result);
+	/* Config normal measurement */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  LSM_REG_CONF_A, 0);
+	ERROR_CHECK(result);
+	/* Adjust gain to 320 LSB/Gauss */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  LSM_REG_CONF_B, LSM_CONF_B_GAIN_5_5);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+int lsm303dlhm_read(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata,
+		    unsigned char *data)
+{
+	unsigned char stat;
+	tMLError result = ML_SUCCESS;
+	short axisFixed;
+
+	/* Read status reg. to check if data is ready */
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, LSM_REG_STATUS, 1,
+			   &stat);
+	ERROR_CHECK(result);
+	if (stat & 0x01) {
+		result =
+		    MLSLSerialRead(mlsl_handle, pdata->address,
+				   LSM_REG_X_M, 6, (unsigned char *) data);
+		ERROR_CHECK(result);
+
+		/*drop data if overflows */
+		if ((data[0] == 0xf0) || (data[2] == 0xf0)
+		    || (data[4] == 0xf0)) {
+			/* trigger next measurement read */
+			result =
+				MLSLSerialWriteSingle(mlsl_handle,
+							pdata->address,
+							LSM_REG_MODE,
+							LSM_MODE_SINGLE);
+			ERROR_CHECK(result);
+			return ML_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) approximate of 1.122 (320/285) */
+		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 =
+		    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					  LSM_REG_MODE, LSM_MODE_SINGLE);
+		ERROR_CHECK(result);
+
+		return ML_SUCCESS;
+	} else {
+		/* trigger next measurement read */
+		result =
+		    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					  LSM_REG_MODE, LSM_MODE_SINGLE);
+		ERROR_CHECK(result);
+
+		return ML_ERROR_COMPASS_DATA_NOT_READY;
+	}
+}
+
+struct ext_slave_descr lsm303dlhm_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ lsm303dlhm_suspend,
+	/*.resume           = */ lsm303dlhm_resume,
+	/*.read             = */ lsm303dlhm_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "lsm303dlhm",
+	/*.type             = */ EXT_SLAVE_TYPE_COMPASS,
+	/*.id               = */ COMPASS_ID_LSM303,
+	/*.reg              = */ 0x06,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {10240, 0},
+};
+
+struct ext_slave_descr *lsm303dlhm_get_slave_descr(void)
+{
+	return &lsm303dlhm_descr;
+}
+EXPORT_SYMBOL(lsm303dlhm_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/mmc314x.c b/drivers/misc/mpu3050/compass/mmc314x.c
new file mode 100755
index 0000000..010d7a7
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/mmc314x.c
@@ -0,0 +1,184 @@
+/*
+ $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   mmc314x.c
+ *      @brief  Magnetometer setup and handling methods for ???? compass.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#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"
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+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)
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+int mmc314x_suspend(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	return result;
+}
+
+int mmc314x_resume(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+
+	int result;
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  MMC314X_REG_ST, MMC314X_CNTL_MODE_RESET);
+	ERROR_CHECK(result);
+	MLOSSleep(10);
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  MMC314X_REG_ST, MMC314X_CNTL_MODE_SET);
+	ERROR_CHECK(result);
+	MLOSSleep(10);
+	read_count = 1;
+	return ML_SUCCESS;
+}
+
+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 =
+	    MLSLSerialRead(mlsl_handle, pdata->address, MMC314X_REG_X_MSB,
+			   6, (unsigned char *) data);
+	ERROR_CHECK(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 =
+			    MLSLSerialWriteSingle(mlsl_handle,
+						  pdata->address,
+						  MMC314X_REG_ST,
+						  MMC314X_CNTL_MODE_RESET);
+			ERROR_CHECK(result);
+			reset_mode = 0;
+			return ML_ERROR_COMPASS_DATA_NOT_READY;
+		} else {
+			result =
+			    MLSLSerialWriteSingle(mlsl_handle,
+						  pdata->address,
+						  MMC314X_REG_ST,
+						  MMC314X_CNTL_MODE_SET);
+			ERROR_CHECK(result);
+			reset_mode = 1;
+			read_count++;
+			return ML_ERROR_COMPASS_DATA_NOT_READY;
+		}
+	}
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  MMC314X_REG_ST,
+				  MMC314X_CNTL_MODE_WAKE_UP);
+	ERROR_CHECK(result);
+	read_count++;
+
+	return ML_SUCCESS;
+}
+
+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,
+	/*.reg              = */ 0x01,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {400, 0},
+};
+
+struct ext_slave_descr *mmc314x_get_slave_descr(void)
+{
+	return &mmc314x_descr;
+}
+EXPORT_SYMBOL(mmc314x_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/mmc328x.c b/drivers/misc/mpu3050/compass/mmc328x.c
new file mode 100755
index 0000000..d2914f3
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/mmc328x.c
@@ -0,0 +1,220 @@
+/*
+ * Copyright (C) 2010 MEMSIC, Inc.
+ *
+ * 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/i2c.h>
+
+#include "mpu.h"
+#include "mlsl.h"
+#include "mlos.h"
+
+#include <log.h>
+
+#define MMC328X_I2C_ADDR	0x30
+
+#define MMC328X_REG_CTRL	0x07
+#define MMC328X_REG_DATA	0x00
+#define MMC328X_REG_DS		0x06
+
+#define MMC328X_CTRL_TM		0x01
+#define MMC328X_CTRL_RM		0x20
+
+#define MMC328X_DELAY_TM	10	/* ms */
+#define MMC328X_DELAY_RM	10	/* ms */
+#define MMC328X_DELAY_STDN	1	/* ms */
+
+static int mmc328x_i2c_rx_data(void *mlsl_handle,
+	struct ext_slave_platform_data *pdata, char *buf, int len)
+{
+	uint8_t i;
+	struct i2c_msg msgs[2] = { 0, };
+
+	if (NULL == buf || NULL == mlsl_handle)
+		return -EINVAL;
+
+	msgs[0].addr = pdata->address;
+	msgs[0].flags = 0;
+	msgs[0].len = 1;
+	msgs[0].buf = buf;
+
+	msgs[1].addr = pdata->address;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = buf;
+
+	if (i2c_transfer(mlsl_handle, msgs, 2) >= 0)
+		return -1;
+
+	return 0;
+}
+
+static int mmc328x_i2c_tx_data(void *mlsl_handle,
+	struct ext_slave_platform_data *pdata, char *buf, int len)
+{
+	struct i2c_msg msgs[1];
+	int res;
+
+	if (NULL == buf || NULL == mlsl_handle)
+		return -EINVAL;
+
+	msgs[0].addr = pdata->address;
+	msgs[0].flags = 0;	/* write */
+	msgs[0].buf = (unsigned char *)buf;
+	msgs[0].len = len;
+
+	res = i2c_transfer(mlsl_handle, msgs, 1);
+	if (res < 1)
+		return res;
+	else
+		return 0;
+}
+
+int memsic_api_resume(void *mlsl_handle,
+	struct ext_slave_platform_data *pdata)
+{
+#if 0
+	unsigned char data[2] = { 0 };
+
+	data[0] = MMC328X_REG_CTRL;
+	data[1] = MMC328X_CTRL_RM;
+	if (mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2) < 0)
+		return -1;
+
+	msleep(MMC328X_DELAY_RM);
+#endif
+	return 0;
+}
+
+int memsic_api_suspend(void *mlsl_handle,
+	struct ext_slave_platform_data *pdata)
+{
+#if 0
+	unsigned char data[2] = { 0 };
+
+	data[0] = MMC328X_REG_CTRL;
+	data[1] = 0;
+	if (mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2) < 0)
+		return -1;
+	msleep(MMC328X_DELAY_RM);
+
+	data[0] = MMC328X_REG_CTRL;
+	data[1] = 0;
+	mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2);
+	msleep(MMC328X_DELAY_TM);
+#endif
+	return 0;
+}
+
+int memsic_api_read(void *mlsl_handle, struct ext_slave_platform_data *pdata,
+		unsigned char *raw_data, int *accuracy)
+{
+	unsigned char data[6] = { 0 };
+	int MD_times = 0;
+
+	data[0] = MMC328X_REG_CTRL;
+	data[1] = MMC328X_CTRL_TM;
+	mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2);
+	msleep(MMC328X_DELAY_TM);
+
+	data[0] = MMC328X_REG_DS;
+	if (mmc328x_i2c_rx_data(mlsl_handle, pdata, data, 1) < 0)
+		return -EFAULT;
+
+	while (!(data[0] & 0x01)) {
+		msleep(20);
+		data[0] = MMC328X_REG_DS;
+
+		if (mmc328x_i2c_rx_data(mlsl_handle, pdata, data, 1) < 0)
+			return -EFAULT;
+
+		if (data[0] & 0x01)
+			break;
+
+		MD_times++;
+
+		if (MD_times > 2)
+			return -EFAULT;
+	}
+
+	data[0] = MMC328X_REG_DATA;
+	if (mmc328x_i2c_rx_data(mlsl_handle, pdata, data, 6) < 0)
+		return -EFAULT;
+
+	memcpy(raw_data, data, sizeof(unsigned char) * 6);
+
+	*accuracy = 0;
+
+	return 0;
+}
+
+int mmc328x_read(void *mlsl_handle,
+		 struct ext_slave_descr *slave,
+		 struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+	int xyz[3] = { 0, };
+	int accuracy = 0;
+
+	memsic_api_read(mlsl_handle, pdata, data, &accuracy);
+
+	return ML_SUCCESS;
+}
+
+int mmc328x_suspend(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+	memsic_api_suspend(mlsl_handle, pdata);
+	return result;
+}
+
+int mmc328x_resume(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+
+	memsic_api_resume(mlsl_handle, pdata);
+	return ML_SUCCESS;
+}
+
+struct ext_slave_descr mmc328x_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ mmc328x_suspend,
+	/*.resume           = */ mmc328x_resume,
+	/*.read             = */ mmc328x_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.name             = */ "mmc328x",
+	/*.type             = */ EXT_SLAVE_TYPE_COMPASS,
+	/*.id               = */ COMPASS_ID_MMC328X,
+	/*.reg              = */ 0x01,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {400, 0},
+};
+
+struct ext_slave_descr *mmc328x_get_slave_descr(void)
+{
+	return &mmc328x_descr;
+}
+EXPORT_SYMBOL(mmc328x_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/mpuak8975.c b/drivers/misc/mpu3050/compass/mpuak8975.c
new file mode 100755
index 0000000..991de77
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/mpuak8975.c
@@ -0,0 +1,188 @@
+/*
+ $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;
+}
+
+struct ext_slave_descr ak8975_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ ak8975_suspend,
+	/*.resume           = */ ak8975_resume,
+	/*.read             = */ ak8975_read,
+	/*.config           = */ NULL,
+	/*.get_config       = */ NULL,
+	/*.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/compass/yas529-kernel.c b/drivers/misc/mpu3050/compass/yas529-kernel.c
new file mode 100755
index 0000000..239ab66
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/yas529-kernel.c
@@ -0,0 +1,477 @@
+/*
+ $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   yas529.c
+ *      @brief  Magnetometer setup and handling methods for Yamaha yas529
+ *              compass.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#ifdef __KERNEL__
+#include <linux/module.h>
+#include <linux/i2c.h>
+#endif
+
+#include "mpu.h"
+#include "mlos.h"
+
+#include <log.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 */
+};
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+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;
+
+/*****************************************
+    Yamaha I2C access functions
+*****************************************/
+
+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;
+}
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+int yas529_suspend(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+
+	return result;
+}
+
+int yas529_resume(void *mlsl_handle,
+		  struct ext_slave_descr *slave,
+		  struct ext_slave_platform_data *pdata)
+{
+	int result = ML_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);
+	ERROR_CHECK(result);
+	/* zero config register - "110 00 000" */
+	dummyData[0] = YAS_REG_CONFR | 0x00;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+
+	/* Step 2 - initialization coil operation */
+	dummyData[0] = YAS_REG_ICOILR | 0x11;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x01;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x12;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x02;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x13;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x03;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x14;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x04;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x15;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x05;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x16;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x06;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x17;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x07;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x10;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_ICOILR | 0x00;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(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);
+	ERROR_CHECK(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);
+	ERROR_CHECK(result);
+	MLOSSleep(2);		/* wait at least 1.5ms */
+
+	/* Measurement data read */
+	result =
+	    yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+				   dummyRegister, 6, rawData);
+	ERROR_CHECK(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);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_Y1OFFSETR | y1offset;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(result);
+	dummyData[0] = YAS_REG_Y2OFFSETR | y2offset;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1,
+				    dummyData);
+	ERROR_CHECK(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);
+	ERROR_CHECK(result);
+	/* CAL data read */
+	result =
+	    yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+				   dummyRegister, 9, calData);
+	ERROR_CHECK(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);
+	ERROR_CHECK(result);
+	/* CAL data read */
+	result =
+	    yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+				   dummyRegister, 9, calData);
+	ERROR_CHECK(result);
+
+	/* Calculate coefficients of the sensitivity corrcetion matrix */
+#if 1				/* production sensor */
+	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;
+#else				/* evaluation sensor */
+	a1 = 1.0f;
+	/* [71..66] 6bit */
+	d2 = (calData[0] & 0xFC) >> 2;
+	a2 = (short) d2;
+	/* [65..60] 6bit */
+	d3 = ((calData[0] & 0x03) << 4) | ((calData[1] & 0xF0) >> 4);
+	a3 = (short) d3;
+	/* [59..54] 6bit */
+	d4 = ((calData[1] & 0x0F) << 2) | ((calData[2] & 0xC0) >> 6);
+	a4 = (short) d4;
+	/* [53..48] 6bit */
+	d5 = (calData[2] & 0x3F);
+	a5 = (short) (d5 + 70);
+	/* [47..42] 6bit */
+	d6 = ((calData[3] & 0xFC) >> 2);
+	a6 = (short) d6;
+	/* [41..36] 6bit */
+	d7 = ((calData[3] & 0x03) << 4) | ((calData[4] & 0xF0) >> 4);
+	a7 = (short) d7;
+	/* [35..30] 6bit */
+	d8 = ((calData[4] & 0x0F) << 2) | ((calData[5] & 0xC0) >> 6);
+	a8 = (short) d8;
+	/* [29..24] 6bit */
+	d9 = (calData[5] & 0x3F);
+	a9 = (short) (d9 + 150);
+#endif
+
+	return result;
+}
+
+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;
+	tMLError result = ML_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);
+	ERROR_CHECK(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);
+	ERROR_CHECK(result);
+	MLOSSleep(10);
+	/* Measurement data read */
+	result =
+	    yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+				   dummyRegister, 6,
+				   (unsigned char *) &rawData);
+	ERROR_CHECK(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 ML_ERROR_COMPASS_DATA_UNDERFLOW;
+		if ((SX >= 1024) || (SY1 >= 1024) || (SY2 >= 1024))
+			return ML_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 ML_SUCCESS;
+	} else {
+		return ML_ERROR_COMPASS_DATA_NOT_READY;
+	}
+}
+
+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,
+	/*.reg              = */ 0x06,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_BIG_ENDIAN,
+	/*.range            = */ {19660, 8000},
+};
+
+struct ext_slave_descr *yas529_get_slave_descr(void)
+{
+	return &yas529_descr;
+}
+EXPORT_SYMBOL(yas529_get_slave_descr);
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/log.h b/drivers/misc/mpu3050/log.h
new file mode 100755
index 0000000..197b774
--- /dev/null
+++ b/drivers/misc/mpu3050/log.h
@@ -0,0 +1,290 @@
+/*
+    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/>.
+*/
+
+/*
+ * 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(...) ((void)0)
+#else
+#define MPL_LOGV(...) ((void)MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, __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, ...)   ((void)0)
+#else
+#define MPL_LOGV_IF(cond, ...) \
+	((CONDITION(cond))						\
+		? ((void)MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, __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(...) ((void)MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, __VA_ARGS__))
+#endif
+
+#ifndef MPL_LOGD_IF
+#define MPL_LOGD_IF(cond, ...) \
+	((CONDITION(cond))					       \
+		? ((void)MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, __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(...) ((void)MPL_LOG(LOG_INFO, MPL_LOG_TAG, __VA_ARGS__))
+#endif
+
+#ifndef MPL_LOGI_IF
+#define MPL_LOGI_IF(cond, ...) \
+	((CONDITION(cond))                                              \
+		? ((void)MPL_LOG(LOG_INFO, MPL_LOG_TAG, __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(...) ((void)MPL_LOG(LOG_WARN, MPL_LOG_TAG, __VA_ARGS__))
+#endif
+
+#ifndef MPL_LOGW_IF
+#define MPL_LOGW_IF(cond, ...) \
+	((CONDITION(cond))					       \
+		? ((void)MPL_LOG(LOG_WARN, MPL_LOG_TAG, __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(...) ((void)MPL_LOG(LOG_ERROR, MPL_LOG_TAG, __VA_ARGS__))
+#endif
+
+#ifndef MPL_LOGE_IF
+#define MPL_LOGE_IF(cond, ...) \
+	((CONDITION(cond))					       \
+		? ((void)MPL_LOG(LOG_ERROR, MPL_LOG_TAG, __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, ...) \
+	((CONDITION(cond))					   \
+		? ((void)android_printAssert(#cond, MPL_LOG_TAG, __VA_ARGS__)) \
+		: (void)0)
+
+#define MPL_LOG_ALWAYS_FATAL(...) \
+	(((void)android_printAssert(NULL, MPL_LOG_TAG, __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, ...) ((void)0)
+#define MPL_LOG_FATAL(...)          ((void)0)
+
+#else
+
+#define MPL_LOG_FATAL_IF(cond, ...) MPL_LOG_ALWAYS_FATAL_IF(cond, __VA_ARGS__)
+#define MPL_LOG_FATAL(...)          MPL_LOG_ALWAYS_FATAL(__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, ...) MPL_LOG_FATAL_IF(!(cond), __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, ...) \
+    MPL_LOG_PRI(priority, tag, __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, ...) \
+	LOG(priority, tag, __VA_ARGS__)
+#elif defined __KERNEL__
+#define MPL_LOG_PRI(priority, tag, ...) \
+	printk(MPL_##priority tag __VA_ARGS__)
+#else
+#define MPL_LOG_PRI(priority, tag, ...) \
+	_MLPrintLog(MPL_##priority, tag, __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__
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+    vprintk(MPL_##priority tag fmt, args)
+#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 100755
index 0000000..6ce2c98
--- /dev/null
+++ b/drivers/misc/mpu3050/mldl_cfg.c
@@ -0,0 +1,1742 @@
+/*
+ $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"
+#include "mpu-accel.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 VDDIO level shifters.
+ *              When enabled the voltage interface with AUX or other external
+ *              accelerometer is using Vlogic instead of VDD (supply).
+ *
+ *  @note       Must be called after MLSerialOpen().
+ *  @note       Typically be called before MLDmpOpen().
+ *              If called after MLDmpOpen(), must be followed by a call to
+ *              MLDLApplyLevelShifterBit() to write the setting on the hw.
+ *
+ *  @param[in]  enable
+ *                  1 to enable, 0 to disable
+ *
+ *  @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->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);
+		}
+
+#if 0
+		result = mldl_cfg->accel->resume(accel_handle,
+						 mldl_cfg->accel,
+						 &mldl_cfg->pdata->accel);
+#else
+	result = mpu_accel_resume(mldl_cfg);
+#endif
+		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);
+		}
+
+#if 0
+		result = mldl_cfg->accel->suspend(accel_handle,
+						  mldl_cfg->accel,
+						  &mldl_cfg->pdata->accel);
+#else
+		result = mpu_accel_suspend(mldl_cfg);
+#endif
+		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 100755
index 0000000..b893946
--- /dev/null
+++ b/drivers/misc/mpu3050/mldl_cfg.h
@@ -0,0 +1,209 @@
+/*
+ $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.     - */
+/* --------------------- */
+
+/*exteded variables for only kernel*/
+struct mldl_ext_cfg {
+	void *mpuacc_data; /*Mpu-Accel Data*/
+};
+
+
+/* Platform data for the MPU */
+struct mldl_cfg {
+	/* MPU related configuration */
+	unsigned long requested_sensors;
+	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;
+
+	/*---------------------------------------------------*/
+	/*KERNEL ONLY VARIABLES                              */
+	/*---------------------------------------------------*/
+	struct mldl_ext_cfg ext;
+};
+
+
+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 100755
index 0000000..02f1843
--- /dev/null
+++ b/drivers/misc/mpu3050/mlos-kernel.c
@@ -0,0 +1,93 @@
+/*
+ $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>
+#include <linux/time.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)
+{
+	struct timespec now;
+
+	getnstimeofday(&now);
+
+	return (long)(now.tv_sec * 1000L + now.tv_nsec / 1000000L);
+}
diff --git a/drivers/misc/mpu3050/mlos.h b/drivers/misc/mpu3050/mlos.h
new file mode 100755
index 0000000..4ebb86c
--- /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 100755
index 0000000..908b16f
--- /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) {
+		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 (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 100755
index 0000000..51fe401
--- /dev/null
+++ b/drivers/misc/mpu3050/mlsl.h
@@ -0,0 +1,110 @@
+/*
+ $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. - */
+/* ------------ */
+/* acceleration data */
+struct acc_data {
+	s16 x;
+	s16 y;
+	s16 z;
+};
+
+
+/*
+ * 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 100755
index 0000000..5c1b684
--- /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)                                                  \
+	{								\
+		if (ML_SUCCESS != x) {					\
+			MPL_LOGE("%s|%s|%d returning %d\n",		\
+				__FILE__, __func__, __LINE__, x);	\
+			return x;					\
+		}							\
+	}
+
+#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-accel.c b/drivers/misc/mpu3050/mpu-accel.c
new file mode 100755
index 0000000..4b5c641
--- /dev/null
+++ b/drivers/misc/mpu3050/mpu-accel.c
@@ -0,0 +1,679 @@
+/*
+    mpu-accel.c - mpu3050 input device interface
+
+    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/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/input.h>
+
+#include "mpuirq.h"
+#include "slaveirq.h"
+#include "mlsl.h"
+#include "mpu-i2c.h"
+#include "mldl_cfg.h"
+#include "mpu.h"
+#include "mpu-accel.h"
+
+#define MPUACC_DEBUG 0
+#define MPUACC_DEBUG_CFG 0
+
+#define MPUACCEL_INPUT_NAME "mpu-accel"
+
+struct mpuaccel_data {
+	struct input_dev *input_data;
+	struct delayed_work work;
+	struct mutex data_mutex;
+
+	struct mldl_cfg *mldl_cfg;
+	void *accel_handle;
+
+	atomic_t enable;
+	atomic_t poll_delay;
+	int device_is_on;
+#ifdef MPUACC_USES_CACHED_DATA
+	unsigned char cached_data[6];
+#endif				/* MPUACC_USES_CACHED_DATA */
+};
+
+static struct mpuaccel_data *pThisData;
+extern struct acc_data cal_data;
+
+static void mpu_accel_print_mldl_cfg(struct mldl_cfg *mldl_cfg)
+{
+	if (MPUACC_DEBUG_CFG) {
+		pr_info("requested_sensors:%ld\n", mldl_cfg->requested_sensors);
+/*  pr_info("ignore_system_suspend:%d\n", mldl_cfg->ignore_system_suspend); */
+		pr_info("addr:%d\n", mldl_cfg->addr);
+		pr_info("int_config:%d\n", mldl_cfg->int_config);
+		pr_info("ext_sync:%d\n", mldl_cfg->ext_sync);
+		pr_info("full_scale:%d\n", mldl_cfg->full_scale);
+		pr_info("dmp_enable:%d\n", mldl_cfg->dmp_enable);
+		pr_info("fifo_enable:%d\n", mldl_cfg->fifo_enable);
+		pr_info("dmp_cfg1:%d\n", mldl_cfg->dmp_cfg1);
+		pr_info("dmp_cfg2:%d\n", mldl_cfg->dmp_cfg2);
+		pr_info("gyro_power:%d\n", mldl_cfg->gyro_power);
+		pr_info("gyro_is_bypassed:%d\n", mldl_cfg->gyro_is_bypassed);
+		pr_info("dmp_is_running:%d\n", mldl_cfg->dmp_is_running);
+		pr_info("gyro_is_suspended:%d\n", mldl_cfg->gyro_is_suspended);
+		pr_info("accel_is_suspended:%d\n",
+			mldl_cfg->accel_is_suspended);
+		pr_info("compass_is_suspended:%d\n",
+			mldl_cfg->compass_is_suspended);
+		pr_info("pressure_is_suspended:%d\n",
+			mldl_cfg->pressure_is_suspended);
+		pr_info("gyro_needs_reset:%d\n", mldl_cfg->gyro_needs_reset);
+	}
+}
+
+static int mpu_accel_mutex_lock(struct mpuaccel_data *data)
+{
+	mutex_lock(&data->data_mutex);
+
+	return ML_SUCCESS;
+}
+
+static int mpu_accel_mutex_unlock(struct mpuaccel_data *data)
+{
+	mutex_unlock(&data->data_mutex);
+
+	return ML_SUCCESS;
+}
+
+static int mpu_accel_activate_device(struct mpuaccel_data *data, int enable)
+{
+	int result = ML_SUCCESS;
+	struct mldl_cfg *mldl_cfg = data->mldl_cfg;
+
+	if (enable) {
+		/*turn on accel */
+		if (NULL != mldl_cfg->accel
+		 && NULL != mldl_cfg->accel->resume) {
+			result = mldl_cfg->accel->resume(data->accel_handle,
+							 mldl_cfg->accel,
+							 &mldl_cfg->pdata->
+							 accel);
+		}
+	} else {
+		/*turn off accel */
+		if (NULL != mldl_cfg->accel
+		 && NULL != mldl_cfg->accel->suspend) {
+			result = mldl_cfg->accel->suspend(data->accel_handle,
+							  mldl_cfg->accel,
+							  &mldl_cfg->pdata->
+							  accel);
+		}
+	}
+
+	if (result == ML_SUCCESS)
+		data->device_is_on = enable;
+
+	if (MPUACC_DEBUG)
+		pr_info("activate device:%d, result=%d\n", enable, result);
+
+	return result;
+}
+
+static int mpu_accel_get_data_from_device(struct mpuaccel_data *data,
+					 unsigned char *buffer)
+{
+	int result = ML_SUCCESS;
+	struct mldl_cfg *mldl_cfg = data->mldl_cfg;
+
+	if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->read) {
+		result = mldl_cfg->accel->read(data->accel_handle,
+							 mldl_cfg->accel,
+							 &mldl_cfg->pdata->accel, buffer);
+	}
+
+	return result;
+}
+
+static int mpu_accel_get_data_from_mpu(struct mpuaccel_data *data, unsigned char *buffer)
+{
+	int result = ML_SUCCESS;
+	struct mldl_cfg *mldl_cfg = data->mldl_cfg;
+	result =
+	    MLSLSerialRead(data->accel_handle, mldl_cfg->addr, 0x23, 6, buffer);
+	return result;
+}
+
+static int mpu_accel_get_data(struct mpuaccel_data *data, unsigned char *buffer,
+		   int *from_mpu)
+{
+	int res = ML_SUCCESS;
+	struct mldl_cfg *mldl_cfg = data->mldl_cfg;
+
+	if (mldl_cfg->accel_is_suspended == 1 ||
+	   (mldl_cfg->dmp_is_running == 0
+	   && mldl_cfg->accel_is_suspended == 0)) {
+
+		if (from_mpu != NULL)
+			*from_mpu = 0;
+
+		/*
+		   Retrieve accel data from accel device driver directly.
+		 */
+		res = mpu_accel_get_data_from_device(data, buffer);
+	} else if (mldl_cfg->dmp_is_running &&
+		   mldl_cfg->accel_is_suspended == 0) {
+
+		if (from_mpu != NULL)
+			*from_mpu = 1;
+
+		/*
+		   Retrieve accel data from MPU registers(0x23 to 0x28).
+		 */
+		res = mpu_accel_get_data_from_mpu(data, buffer);
+	}
+
+	return res;
+}
+
+static int mpu_accel_build_data(struct mpuaccel_data *data,
+			const unsigned char *buffer, s16 *val)
+{
+	struct mldl_cfg *mldl_cfg = data->mldl_cfg;
+	int endian = mldl_cfg->accel->endian;
+	int dev_id = mldl_cfg->accel->id;
+
+	if (endian == EXT_SLAVE_LITTLE_ENDIAN) {
+		if (dev_id == ACCEL_ID_BMA150)
+			*val = (*(s16 *)&buffer[0]) >> 6;
+		else if (dev_id == ACCEL_ID_KXTF9) {
+			*val =
+			    (short)(((signed char)buffer[1] << 4) |
+			    (buffer[0] >> 4));
+		} else
+			*val = (buffer[1] << 8) | buffer[0];
+	} else if (endian == EXT_SLAVE_BIG_ENDIAN) {
+		*val = (buffer[0] << 8) | buffer[1];
+	}
+
+	return ML_SUCCESS;
+}
+
+static void mpu_accel_input_work_func(struct work_struct *work)
+{
+	int res = 0;
+	int poll_time = 0;
+	int enable = 0;
+	int i = 0;
+
+	struct mpuaccel_data *data = container_of((struct delayed_work *)work,
+						  struct mpuaccel_data, work);
+
+	struct mldl_cfg *mldl_cfg = data->mldl_cfg;
+
+	poll_time = atomic_read(&data->poll_delay);
+
+	if (MPUACC_DEBUG)
+		pr_info("________________START____________________\n");
+	if (MPUACC_DEBUG_CFG)
+		mpu_accel_print_mldl_cfg(mldl_cfg);
+
+	mpu_accel_mutex_lock(data);
+	enable = atomic_read(&data->enable);
+	mpu_accel_mutex_unlock(data);
+
+	if (enable) {
+		unsigned char buffer[6] = { 0, };
+		s16 raw[3] = { 0, };
+		int data_is_avail = 0;
+		int data_is_from_mpu = 0;
+
+		mpu_accel_mutex_lock(data);
+		mpu_accel_get_data(data, buffer, &data_is_from_mpu);
+		mpu_accel_mutex_unlock(data);
+
+		if (res == ML_SUCCESS)
+			data_is_avail = 1;
+
+		if (data_is_avail) {
+			int data_is_valid = 0;
+
+			for (i = 0; i < 3; i++) {
+				mpu_accel_build_data(data, &buffer[i * 2],
+						     &raw[i]);
+			}
+			raw[0] += cal_data.x;
+			raw[1] += cal_data.y;
+			raw[2] += cal_data.z;
+
+			if (raw[0] && raw[1] && raw[2])
+				data_is_valid = 1;
+
+			if (data_is_valid) {
+				int accel[3] = { 0, };
+
+				/*apply mounting matrix */
+				for (i = 0; i < 3; i++) {
+#ifdef MPUACC_USES_MOUNTING_MATRIX
+					int j = 0;
+					for (j = 0; j < 3; j++) {
+						accel[i] +=
+						    mldl_cfg->pdata->accel.
+						    orientation[i * 3 +
+								j] * raw[j];
+					}
+#else
+					accel[i] = raw[i];
+#endif
+				}
+
+				if (MPUACC_DEBUG) {
+					if (data_is_from_mpu == 1)
+						pr_info
+						 ("MPU_ACCEL:[%d][%d][%d]\n",
+						  accel[0], accel[1],
+						  accel[2]);
+					else
+						pr_info("ACCEL:[%d][%d][%d]\n",
+							 accel[0], accel[1],
+							 accel[2]);
+				}
+#ifdef MPUACC_USES_CACHED_DATA
+				memcpy(data->cached_data, buffer,
+						 sizeof(unsigned char) * 6);
+#endif /* #ifdef MPUACC_USES_CACHED_DATA */
+				input_report_rel(data->input_data, REL_X,
+						 accel[0]);
+				input_report_rel(data->input_data, REL_Y,
+						 accel[1]);
+				input_report_rel(data->input_data, REL_Z,
+						 accel[2]);
+				input_sync(data->input_data);
+
+				if (MPUACC_DEBUG)
+					pr_info("input device is updated\n");
+			}
+		}
+	}
+
+	if (MPUACC_DEBUG)
+		pr_info("________________END____________________\n");
+
+	mpu_accel_mutex_lock(data);
+	enable = atomic_read(&data->enable);
+	mpu_accel_mutex_unlock(data);
+
+	if (enable) {
+		if (poll_time > 0) {
+			schedule_delayed_work(&data->work,
+						msecs_to_jiffies(poll_time)
+						/*+ 1 */);
+		} else {
+			schedule_delayed_work(&data->work, 0);
+		}
+
+	}
+
+}
+
+static int mpu_accel_enable(struct mpuaccel_data *data)
+{
+	int res = ML_SUCCESS;
+	struct mldl_cfg *mldl_cfg = data->mldl_cfg;
+
+	if (MPUACC_DEBUG)
+		pr_info("mpu_accel_enable : %d\n", atomic_read(&data->enable));
+
+	if (atomic_read(&data->enable) != 1) {
+
+		if (MPUACC_DEBUG)
+			pr_info("mpu_accel_enable : enabled\n");
+
+		if (mldl_cfg->accel_is_suspended == 1) {
+			if (MPUACC_DEBUG)
+				pr_info("mpu_accel_enable : turn on accel\n");
+			mpu_accel_activate_device(data, 1);
+		}
+
+		atomic_set(&data->enable, 1);
+		schedule_delayed_work(&data->work, 0);
+
+	}
+
+	return res;
+}
+
+static int mpu_accel_disable(struct mpuaccel_data *data)
+{
+	int res = ML_SUCCESS;
+	struct mldl_cfg *mldl_cfg = data->mldl_cfg;
+
+	if (MPUACC_DEBUG)
+		pr_info("mpu_accel_disable : %d\n", atomic_read(&data->enable));
+
+	if (atomic_read(&data->enable) != 0) {
+		atomic_set(&data->enable, 0);
+		cancel_delayed_work(&data->work);
+
+		if (MPUACC_DEBUG)
+			pr_info("mpu_accel_disable : disabled\n");
+
+		if (mldl_cfg->accel_is_suspended == 1) {
+			if (MPUACC_DEBUG)
+				pr_info("mpu_accel_disable : turn off accel\n");
+
+			/*turn off accel */
+			mpu_accel_activate_device(data, 0);
+		}
+	}
+
+	return res;
+}
+
+static ssize_t mpu_accel_delay_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct input_dev *input_data = to_input_dev(dev);
+	struct mpuaccel_data *data = input_get_drvdata(input_data);
+
+	return sprintf(buf, "%d\n", atomic_read(&data->poll_delay));
+}
+
+static ssize_t mpu_accel_delay_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	struct input_dev *input_data = to_input_dev(dev);
+	struct mpuaccel_data *data = input_get_drvdata(input_data);
+	int value = simple_strtoul(buf, NULL, 10);
+
+	atomic_set(&data->poll_delay, value);
+	return count;
+}
+
+static ssize_t mpu_accel_enable_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct input_dev *input_data = to_input_dev(dev);
+	struct mpuaccel_data *data = input_get_drvdata(input_data);
+
+	return sprintf(buf, "%d\n", atomic_read(&data->enable));
+}
+
+static ssize_t
+mpu_accel_enable_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	struct input_dev *input_data = to_input_dev(dev);
+	struct mpuaccel_data *data = input_get_drvdata(input_data);
+	int value;
+
+	value = simple_strtoul(buf, NULL, 10);
+	if (value != 0 && value != 1)
+		return count;
+
+	mpu_accel_mutex_lock(data);
+
+	if (value)
+		mpu_accel_enable(data);
+	else
+		mpu_accel_disable(data);
+
+	mpu_accel_mutex_unlock(data);
+
+	return count;
+}
+
+int mpu_accel_is_active_device(void)
+{
+	int is_active = 0;
+
+	if (pThisData != NULL) {
+		mpu_accel_mutex_lock(pThisData);
+		is_active = pThisData->device_is_on;
+		mpu_accel_mutex_unlock(pThisData);
+	}
+
+	return is_active;
+}
+
+#ifdef MPUACC_USES_CACHED_DATA
+int mpu_accel_get_cached_data(unsigned char *cache)
+{
+	int res = ML_ERROR;
+
+	if (pThisData != NULL) {
+		if (pThisData->device_is_on == 1) {
+			memcpy(cache, pThisData->cached_data,
+					 sizeof(unsigned char) * 6);
+			pr_info("cached data:[%d][%d][%d][%d][%d][%d]\n",
+				cache[0], cache[1],
+				cache[2], cache[3],
+				cache[4], cache[5]);
+			res = ML_SUCCESS;
+		}
+
+	}
+
+	return res;
+}
+#endif /* MPUACC_USES_CACHED_DATA */
+
+static DEVICE_ATTR(poll_delay, S_IRUGO | S_IWUSR | S_IWGRP,
+			mpu_accel_delay_show, mpu_accel_delay_store);
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP,
+			mpu_accel_enable_show, mpu_accel_enable_store);
+
+static struct attribute *mpuaccel_attributes[] = {
+	&dev_attr_poll_delay.attr,
+	&dev_attr_enable.attr,
+	NULL
+};
+
+static struct attribute_group mpuaccel_attribute_group = {
+	.attrs = mpuaccel_attributes
+};
+
+int mpu_accel_init(struct mldl_cfg *mldl_cfg, void *accel_handle)
+{
+	struct input_dev *input_data = NULL;
+	struct mpuaccel_data *data = NULL;
+	int res = 0;
+
+	data = kzalloc(sizeof(struct mpuaccel_data), GFP_KERNEL);
+	if (data == NULL) {
+		res = -ENOMEM;
+		goto err;
+	}
+
+	data->mldl_cfg = mldl_cfg;
+	data->accel_handle = accel_handle;
+	atomic_set(&data->enable, 0);
+	atomic_set(&data->poll_delay, 20);	/* set 20ms to polling time */
+
+	mutex_init(&data->data_mutex);
+
+	INIT_DELAYED_WORK(&data->work, mpu_accel_input_work_func);
+
+	input_data = input_allocate_device();
+	if (input_data == NULL) {
+		res = -ENOMEM;
+		pr_err(
+				 "mpu_accel_probe: Failed to allocate input_data device\n");
+		goto err;
+	}
+
+	input_data->name = MPUACCEL_INPUT_NAME;
+	input_data->id.bustype = BUS_I2C;
+
+	set_bit(EV_REL, input_data->evbit);
+	input_set_capability(input_data, EV_REL, REL_X);
+	input_set_capability(input_data, EV_REL, REL_Y);
+	input_set_capability(input_data, EV_REL, REL_Z);
+
+	data->input_data = input_data;
+
+	res = input_register_device(input_data);
+	if (res) {
+		pr_err(
+				 "mpu_accel_init: Unable to register input_data device: %s\n",
+				 input_data->name);
+		goto err;
+	}
+
+	input_set_drvdata(input_data, data);
+	mldl_cfg->ext.mpuacc_data = (void *)data;
+
+	pThisData = data;
+
+	res = sysfs_create_group(&input_data->dev.kobj,
+				 &mpuaccel_attribute_group);
+	if (res) {
+		pr_err(
+				 "mpu_accel_init: sysfs_create_group failed[%s]\n",
+				 input_data->name);
+		goto err;
+	}
+
+	return res;
+
+err:
+	sysfs_remove_group(&input_data->dev.kobj, &mpuaccel_attribute_group);
+	input_free_device(input_data);
+	kfree(data);
+	return res;
+
+}
+
+int mpu_accel_exit(struct mldl_cfg *mldl_cfg)
+{
+	struct mpuaccel_data *data = NULL;
+
+	if (mldl_cfg == NULL)
+		return ML_ERROR;
+
+	data = (struct mpuaccel_data *)mldl_cfg->ext.mpuacc_data;
+
+	if (data != NULL) {
+		sysfs_remove_group(&(data->input_data->dev.kobj),
+					&mpuaccel_attribute_group);
+		input_free_device(data->input_data);
+
+		kfree(data);
+		data = NULL;
+
+		mldl_cfg->ext.mpuacc_data = NULL;
+	}
+
+	return ML_SUCCESS;
+}
+
+int mpu_accel_suspend(struct mldl_cfg *mldl_cfg)
+{
+	int result = ML_SUCCESS;
+	int enable = 0;
+	struct mpuaccel_data *data = NULL;
+
+	if (mldl_cfg == NULL)
+		return ML_ERROR;
+
+	data = (struct mpuaccel_data *)mldl_cfg->ext.mpuacc_data;
+
+	mpu_accel_mutex_lock(data);
+	enable = atomic_read(&data->enable);
+
+	pr_info("%s: device_is_on = %d, enable = %d\n",
+		__func__, data->device_is_on, enable);
+
+	if (data->device_is_on == 1 && enable == 0) {
+		pr_info("%s: mpu_accel_activate_device 0\n", __func__);
+		result = mpu_accel_activate_device(data, 0);
+	}
+
+	mpu_accel_mutex_unlock(data);
+
+	return result;
+}
+
+int mpu_accel_resume(struct mldl_cfg *mldl_cfg)
+{
+	int result = ML_SUCCESS;
+	int enable = 0;
+	struct mpuaccel_data *data = NULL;
+
+	if (mldl_cfg == NULL)
+		return ML_ERROR;
+
+	data = (struct mpuaccel_data *)mldl_cfg->ext.mpuacc_data;
+
+	mpu_accel_mutex_lock(data);
+	enable = atomic_read(&data->enable);
+
+	pr_info("%s: device_is_on = %d, enable = %d\n",
+		__func__, data->device_is_on, enable);
+
+	if (data->device_is_on == 0 && enable == 0) {
+		pr_info("%s: mpu_accel_activate_device 1\n", __func__);
+		result = mpu_accel_activate_device(data, 1);
+	}
+
+	mpu_accel_mutex_unlock(data);
+
+	return result;
+}
+
+int mpu_accel_read(struct mldl_cfg *mldl_cfg, unsigned char *buffer)
+{
+	int result = ML_SUCCESS;
+	int enable = 0;
+	struct mpuaccel_data *data = NULL;
+
+	if (mldl_cfg == NULL)
+		return ML_ERROR;
+
+	data = (struct mpuaccel_data *)mldl_cfg->ext.mpuacc_data;
+
+	mpu_accel_mutex_lock(data);
+	enable = atomic_read(&data->enable);
+#ifdef MPUACC_USES_CACHED_DATA
+	if (enable == 1)
+		memcpy(buffer, data->cached_data, sizeof(unsigned char) * 6);
+	else
+#endif /* MPUACC_USES_CACHED_DATA */
+		result = mpu_accel_get_data_from_device(data, buffer);
+	mpu_accel_mutex_unlock(data);
+
+	return result;
+}
diff --git a/drivers/misc/mpu3050/mpu-accel.h b/drivers/misc/mpu3050/mpu-accel.h
new file mode 100755
index 0000000..5dd57c9
--- /dev/null
+++ b/drivers/misc/mpu3050/mpu-accel.h
@@ -0,0 +1,8 @@
+#undef MPUACC_USES_CACHED_DATA
+#define MPUACC_USES_MOUNTING_MATRIX
+
+int mpu_accel_init(struct mldl_cfg *mldl_cfg, void *accel_handle);
+int mpu_accel_exit(struct mldl_cfg *mldl_cfg);
+int mpu_accel_suspend(struct mldl_cfg *mldl_cfg);
+int mpu_accel_resume(struct mldl_cfg *mldl_cfg);
+int mpu_accel_read(struct mldl_cfg *mldl_cfg, unsigned char *buffer);
diff --git a/drivers/misc/mpu3050/mpu-dev.c b/drivers/misc/mpu3050/mpu-dev.c
new file mode 100755
index 0000000..ef04ed7
--- /dev/null
+++ b/drivers/misc/mpu3050/mpu-dev.c
@@ -0,0 +1,2280 @@
+/*
+	mpu-dev.c - mpu3050 char device interface
+
+	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/>.
+*/
+/* Code inside mpudev_ioctl_rdrw is copied from i2c-dev.c
+ */
+
+#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>
+
+#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 "mlos.h"
+#include "mpu-i2c.h"
+#include "mldl_cfg.h"
+#include "mpu-accel.h"
+
+#include "mpu.h"
+
+#define ACCEL_VENDOR_NAME	"KIONIX"
+#define ACCEL_CHIP_NAME		"KXTF9"
+
+#define GYRO_VENDOR_NAME	"INVENSENSE"
+#define GYRO_CHIP_NAME		"MPU-3050"
+
+#define MPU3050_EARLY_SUSPEND_IN_DRIVER 1
+
+#define CALIBRATION_FILE_PATH	"/efs/calibration_data"
+#define CALIBRATION_DATA_AMOUNT	100
+
+struct acc_data cal_data;
+
+/* Platform data for the MPU */
+struct mpu_private_data {
+	struct mldl_cfg mldl_cfg;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+};
+
+static int is_lis3dh;
+
+#define IDEAL_X	0
+#define IDEAL_Y	0
+#define IDEAL_Z	1024
+
+static int pid;
+
+static struct i2c_client *this_client;
+
+int read_accel_raw_xyz(struct acc_data *acc)
+{
+	unsigned char acc_data[6];
+	s32 temp;
+	struct mldl_cfg *mldl_cfg;
+
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(this_client);
+
+	if (!mpu) {
+		pr_info("%s : mpu data is NULL, mpu3050 Init error", __func__);
+		return 0;
+	}
+
+	mldl_cfg = &mpu->mldl_cfg;
+
+	if (mldl_cfg->accel_is_suspended == 1 ||
+	    (mldl_cfg->dmp_is_running == 0
+	     && mldl_cfg->accel_is_suspended == 0)) {
+		if (is_lis3dh) {
+			if (mldl_cfg->accel_is_suspended == 1) {
+				sensor_i2c_write_register(this_client->adapter,
+							  0x19, 0x20, 0x67);
+				MLOSSleep(1);
+			}
+			sensor_i2c_read(this_client->adapter,
+					0x19, 0x28 | 0x80, 6, acc_data);
+			if (mldl_cfg->accel_is_suspended == 1) {
+				sensor_i2c_write_register(this_client->adapter,
+							  0x19, 0x20, 0x18);
+				MLOSSleep(1);
+			}
+		} else
+			sensor_i2c_read(this_client->adapter,
+					0x0F, 0x06, 6, acc_data);
+	} else if (mldl_cfg->dmp_is_running &&
+		   mldl_cfg->accel_is_suspended == 0) {
+		if (sensor_i2c_read(this_client->adapter,
+				    DEFAULT_MPU_SLAVEADDR,
+				    0x23, 6, acc_data) != 0)
+			return -1;
+	} else
+		return -1;
+
+	if (is_lis3dh) {
+		acc->x = ((acc_data[0] << 8) | acc_data[1]);
+		acc->x = (acc->x >> 4);
+		acc->y = ((acc_data[2] << 8) | acc_data[3]);
+		acc->y = (acc->y >> 4);
+		acc->z = ((acc_data[4] << 8) | acc_data[5]);
+		acc->z = (acc->z >> 4);
+	} else {
+		temp = ((acc_data[1] << 4) | (acc_data[0] >> 4));
+		if (temp < 2048)
+			acc->x = (s16) (-temp);
+		else
+			acc->x = (s16) (4096 - temp);
+
+		temp = ((acc_data[3] << 4) | (acc_data[2] >> 4));
+		if (temp < 2048)
+			acc->y = (s16) (-temp);
+		else
+			acc->y = (s16) (4096 - temp);
+
+		temp = ((acc_data[5] << 4) | (acc_data[4] >> 4));
+		if (temp < 2048)
+			acc->z = (s16) (1024 - temp);
+		else
+			acc->z = (s16) (3072 - temp);
+	}
+	return 0;
+}
+
+static int accel_open_calibration(void)
+{
+	struct file *cal_filp = NULL;
+	int err = 0;
+	mm_segment_t old_fs;
+
+	old_fs = get_fs();
+	set_fs(KERNEL_DS);
+
+	cal_filp = filp_open(CALIBRATION_FILE_PATH, O_RDONLY, 0666);
+	if (IS_ERR(cal_filp)) {
+		pr_err("%s: Can't open calibration file\n", __func__);
+		set_fs(old_fs);
+		err = PTR_ERR(cal_filp);
+
+		cal_data.x = 0;
+		cal_data.y = 0;
+		cal_data.z = 0;
+
+		return err;
+	}
+
+	err = cal_filp->f_op->read(cal_filp,
+				   (char *)&cal_data, 3 * sizeof(s16),
+				   &cal_filp->f_pos);
+	if (err != 3 * sizeof(s16)) {
+		pr_err("%s: Can't read the cal data from file\n", __func__);
+		err = -EIO;
+	}
+
+	pr_info("%s : (%u,%u,%u)\n", __func__,
+		cal_data.x, cal_data.y, cal_data.z);
+
+	filp_close(cal_filp, current->files);
+	set_fs(old_fs);
+
+	return err;
+}
+
+static int accel_do_calibrate(bool do_calib)
+{
+	struct acc_data data = { 0, };
+	struct file *cal_filp = NULL;
+	int sum[3] = { 0, };
+	int err = 0;
+	int i;
+	mm_segment_t old_fs;
+
+	if (do_calib) {
+		for (i = 0; i < CALIBRATION_DATA_AMOUNT; i++) {
+			err = read_accel_raw_xyz(&data);
+			if (err < 0) {
+				pr_err("%s: accel_read_accel_raw_xyz() "
+				       "failed in the %dth loop\n",
+				       __func__, i);
+				return err;
+			}
+
+			sum[0] += data.x;
+			sum[1] += data.y;
+			sum[2] += data.z;
+		}
+
+
+		if (is_lis3dh) {
+			cal_data.x = IDEAL_X - cal_data.x;
+			cal_data.y = IDEAL_Y - cal_data.y;
+			cal_data.z = IDEAL_Z - cal_data.z;
+		} else {
+			cal_data.x = sum[0] / CALIBRATION_DATA_AMOUNT;
+			cal_data.y = sum[1] / CALIBRATION_DATA_AMOUNT;
+			cal_data.z = sum[2] / CALIBRATION_DATA_AMOUNT;
+		}
+	} else {
+		cal_data.x = 0;
+		cal_data.y = 0;
+		cal_data.z = 0;
+	}
+
+	pr_info("%s: cal data (%d,%d,%d)\n", __func__,
+		cal_data.x, cal_data.y, cal_data.z);
+
+	old_fs = get_fs();
+	set_fs(KERNEL_DS);
+
+	cal_filp = filp_open(CALIBRATION_FILE_PATH,
+			     O_CREAT | O_TRUNC | O_WRONLY, 0666);
+	if (IS_ERR(cal_filp)) {
+		pr_err("%s: Can't open calibration file\n", __func__);
+		set_fs(old_fs);
+		err = PTR_ERR(cal_filp);
+		return err;
+	}
+
+	err = cal_filp->f_op->write(cal_filp,
+				    (char *)&cal_data, 3 * sizeof(s16),
+				    &cal_filp->f_pos);
+	if (err != 3 * sizeof(s16)) {
+		pr_err("%s: Can't write the cal data to file\n", __func__);
+		err = -EIO;
+	}
+
+	filp_close(cal_filp, current->files);
+	set_fs(old_fs);
+
+	return err;
+}
+
+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;
+
+	accel_open_calibration();
+
+	pr_info("%s", __func__);
+	dev_dbg(&this_client->adapter->dev, "mpu_open\n");
+	dev_dbg(&this_client->adapter->dev, "current->pid %d\n", current->pid);
+	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 */
+	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 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;
+
+	pid = 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);
+	result = mpu3050_suspend(mldl_cfg, client->adapter,
+				 accel_adapter, compass_adapter,
+				 pressure_adapter, TRUE, TRUE, TRUE, TRUE);
+	pr_info("%s", __func__);
+	dev_dbg(&this_client->adapter->dev, "mpu_release\n");
+	return result;
+}
+
+static noinline int mpudev_ioctl_rdrw(struct i2c_client *client,
+				      unsigned long arg)
+{
+	struct i2c_rdwr_ioctl_data rdwr_arg;
+	struct i2c_msg *rdwr_pa;
+	u8 __user **data_ptrs;
+	int i, res;
+
+	if (copy_from_user(&rdwr_arg,
+			   (struct i2c_rdwr_ioctl_data __user *)arg,
+			   sizeof(rdwr_arg)))
+		return -EFAULT;
+
+	/* Put an arbitrary limit on the number of messages that can
+	 * be sent at once */
+	if (rdwr_arg.nmsgs > I2C_RDRW_IOCTL_MAX_MSGS)
+		return -EINVAL;
+
+	rdwr_pa = (struct i2c_msg *)
+	    kmalloc(rdwr_arg.nmsgs * sizeof(struct i2c_msg), GFP_KERNEL);
+	if (!rdwr_pa)
+		return -ENOMEM;
+
+	if (copy_from_user(rdwr_pa, rdwr_arg.msgs,
+			   rdwr_arg.nmsgs * sizeof(struct i2c_msg))) {
+		kfree(rdwr_pa);
+		return -EFAULT;
+	}
+
+	data_ptrs = kmalloc(rdwr_arg.nmsgs * sizeof(u8 __user *), GFP_KERNEL);
+	if (data_ptrs == NULL) {
+		kfree(rdwr_pa);
+		return -ENOMEM;
+	}
+
+	res = 0;
+	for (i = 0; i < rdwr_arg.nmsgs; i++) {
+		/* Limit the size of the message to a sane amount;
+		 * and don't let length change either. */
+		if ((rdwr_pa[i].len > 8192) ||
+		    (rdwr_pa[i].flags & I2C_M_RECV_LEN)) {
+			res = -EINVAL;
+			break;
+		}
+		data_ptrs[i] = (u8 __user *) rdwr_pa[i].buf;
+		rdwr_pa[i].buf = kmalloc(rdwr_pa[i].len, GFP_KERNEL);
+		if (rdwr_pa[i].buf == NULL) {
+			res = -ENOMEM;
+			break;
+		}
+		if (copy_from_user(rdwr_pa[i].buf, data_ptrs[i],
+				   rdwr_pa[i].len)) {
+			++i;	/* Needs to be kfreed too */
+			res = -EFAULT;
+			break;
+		}
+	}
+	if (res < 0) {
+		int j;
+		for (j = 0; j < i; ++j)
+			kfree(rdwr_pa[j].buf);
+		kfree(data_ptrs);
+		kfree(rdwr_pa);
+		return res;
+	}
+
+	res = i2c_transfer(client->adapter, rdwr_pa, rdwr_arg.nmsgs);
+	while (i-- > 0) {
+		if (res >= 0 && (rdwr_pa[i].flags & I2C_M_RD)) {
+			if (copy_to_user(data_ptrs[i], rdwr_pa[i].buf,
+					 rdwr_pa[i].len))
+				res = -EFAULT;
+		}
+		kfree(rdwr_pa[i].buf);
+	}
+	kfree(data_ptrs);
+	kfree(rdwr_pa);
+	return res;
+}
+
+/* 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)
+{
+	char *tmp;
+	int ret;
+
+	struct i2c_client *client = (struct i2c_client *)file->private_data;
+
+	if (count > 8192)
+		count = 8192;
+
+	tmp = kmalloc(count, GFP_KERNEL);
+	if (tmp == NULL)
+		return -ENOMEM;
+
+	pr_info("%s: i2c-dev: i2c-%d reading %zu bytes.\n", __func__,
+		iminor(file->f_path.dentry->d_inode), count);
+
+/* @todo fix this to do a i2c trasnfer from the FIFO */
+	ret = i2c_master_recv(client, tmp, count);
+	if (ret >= 0) {
+		ret = copy_to_user(buf, tmp, count) ? -EFAULT : ret;
+		if (ret)
+			ret = -EFAULT;
+	}
+	kfree(tmp);
+	return ret;
+}
+
+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_config __user *usr_config)
+{
+	int retval = ML_SUCCESS;
+	if ((slave) && (slave->config)) {
+		struct ext_slave_config config;
+		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, &mldl_cfg->pdata->accel, &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_config __user *usr_config)
+{
+	int retval = ML_SUCCESS;
+	if ((slave) && (slave->get_config)) {
+		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) {
+			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,
+					   &mldl_cfg->pdata->accel, &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;
+}
+
+/* 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);
+
+	switch (cmd) {
+	case I2C_RDWR:
+		mpudev_ioctl_rdrw(client, arg);
+		break;
+	case I2C_SLAVE:
+		if ((arg & 0x7E) != (client->addr & 0x7E)) {
+			dev_err(&this_client->adapter->dev,
+				"%s: Invalid I2C_SLAVE arg %lu\n",
+				__func__, arg);
+		}
+		break;
+	case MPU_SET_MPU_CONFIG:
+		retval = mpu_ioctl_set_mpu_config(client, arg);
+		break;
+	case MPU_SET_INT_CONFIG:
+		mldl_cfg->int_config = (unsigned char)arg;
+		break;
+	case MPU_SET_EXT_SYNC:
+		mldl_cfg->ext_sync = (enum mpu_ext_sync)arg;
+		break;
+	case MPU_SET_FULL_SCALE:
+		mldl_cfg->full_scale = (enum mpu_fullscale)arg;
+		break;
+	case MPU_SET_LPF:
+		mldl_cfg->lpf = (enum mpu_filter)arg;
+		break;
+	case MPU_SET_CLK_SRC:
+		mldl_cfg->clk_src = (enum mpu_clock_sel)arg;
+		break;
+	case MPU_SET_DIVIDER:
+		mldl_cfg->divider = (unsigned char)arg;
+		break;
+	case MPU_SET_LEVEL_SHIFTER:
+		mldl_cfg->pdata->level_shifter = (unsigned char)arg;
+		break;
+	case MPU_SET_DMP_ENABLE:
+		mldl_cfg->dmp_enable = (unsigned char)arg;
+		break;
+	case MPU_SET_FIFO_ENABLE:
+		mldl_cfg->fifo_enable = (unsigned char)arg;
+		break;
+	case MPU_SET_DMP_CFG1:
+		mldl_cfg->dmp_cfg1 = (unsigned char)arg;
+		break;
+	case MPU_SET_DMP_CFG2:
+		mldl_cfg->dmp_cfg2 = (unsigned char)arg;
+		break;
+	case MPU_SET_OFFSET_TC:
+		retval = copy_from_user(mldl_cfg->offset_tc,
+					(unsigned char __user *)arg,
+					sizeof(mldl_cfg->offset_tc));
+		if (retval)
+			retval = -EFAULT;
+		break;
+	case MPU_SET_RAM:
+		retval = copy_from_user(mldl_cfg->ram,
+					(unsigned char __user *)arg,
+					sizeof(mldl_cfg->ram));
+		if (retval)
+			retval = -EFAULT;
+		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_GET_INT_CONFIG:
+		retval = put_user(mldl_cfg->int_config,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_EXT_SYNC:
+		retval = put_user(mldl_cfg->ext_sync,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_FULL_SCALE:
+		retval = put_user(mldl_cfg->full_scale,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_LPF:
+		retval = put_user(mldl_cfg->lpf, (unsigned char __user *)arg);
+		break;
+	case MPU_GET_CLK_SRC:
+		retval = put_user(mldl_cfg->clk_src,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_DIVIDER:
+		retval = put_user(mldl_cfg->divider,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_LEVEL_SHIFTER:
+		retval = put_user(mldl_cfg->pdata->level_shifter,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_DMP_ENABLE:
+		retval = put_user(mldl_cfg->dmp_enable,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_FIFO_ENABLE:
+		retval = put_user(mldl_cfg->fifo_enable,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_DMP_CFG1:
+		retval = put_user(mldl_cfg->dmp_cfg1,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_DMP_CFG2:
+		retval = put_user(mldl_cfg->dmp_cfg2,
+				  (unsigned char __user *)arg);
+		break;
+	case MPU_GET_OFFSET_TC:
+		retval = copy_to_user((unsigned char __user *)arg,
+				      mldl_cfg->offset_tc,
+				      sizeof(mldl_cfg->offset_tc));
+		if (retval)
+			retval = -EFAULT;
+		break;
+	case MPU_GET_RAM:
+		retval = copy_to_user((unsigned char __user *)arg,
+				      mldl_cfg->ram, sizeof(mldl_cfg->ram));
+		if (retval)
+			retval = -EFAULT;
+		break;
+	case MPU_CONFIG_ACCEL:
+		retval = slave_config(accel_adapter, mldl_cfg,
+				      mldl_cfg->accel,
+				      (struct ext_slave_config __user *)arg);
+		break;
+	case MPU_CONFIG_COMPASS:
+		retval = slave_config(compass_adapter, mldl_cfg,
+				      mldl_cfg->compass,
+				      (struct ext_slave_config __user *)arg);
+		break;
+	case MPU_CONFIG_PRESSURE:
+		retval = slave_config(pressure_adapter, mldl_cfg,
+				      mldl_cfg->pressure,
+				      (struct ext_slave_config __user *)arg);
+		break;
+	case MPU_GET_CONFIG_ACCEL:
+		retval = slave_get_config(accel_adapter, mldl_cfg,
+					  mldl_cfg->accel,
+					  (struct ext_slave_config __user *)
+					  arg);
+		break;
+	case MPU_GET_CONFIG_COMPASS:
+		retval = slave_get_config(compass_adapter, mldl_cfg,
+					  mldl_cfg->compass,
+					  (struct ext_slave_config __user *)
+					  arg);
+		break;
+	case MPU_GET_CONFIG_PRESSURE:
+		retval = slave_get_config(pressure_adapter, mldl_cfg,
+					  mldl_cfg->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_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;
+	case MPU_READ_MEMORY:
+	case MPU_WRITE_MEMORY:
+	default:
+		dev_err(&this_client->adapter->dev,
+			"%s: Unknown cmd %d, arg %lu\n", __func__, cmd, arg);
+		retval = -EINVAL;
+	}
+
+	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;
+	pr_info("%s", __func__);
+
+	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)
+		(void)mpu3050_suspend(mldl_cfg, this_client->adapter,
+				      accel_adapter, compass_adapter,
+				      pressure_adapter, TRUE, TRUE, TRUE, TRUE);
+}
+
+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 (pid) {
+			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__, pid);
+		}
+	}
+	dev_dbg(&this_client->adapter->dev, "%s: %d\n", __func__, h->level);
+	pr_info("%s: h->level = %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;
+	pr_info("%s", __func__);
+	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);
+
+	(void)mpu3050_suspend(mldl_cfg, this_client->adapter,
+			      accel_adapter, compass_adapter, pressure_adapter,
+			      TRUE, TRUE, TRUE, TRUE);
+	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);
+	pr_info("%s", __func__);
+	if (!mpu->mldl_cfg.gyro_is_suspended) {
+		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);
+	}
+	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);
+	pr_info("%s: accel_adapter = %p\n", __func__, accel_adapter);
+
+	compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+	pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+	if (pid) {
+		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__, pid);
+	}
+
+	pr_info("%s: pid = %d\n", __func__, pid);
+	return 0;
+}
+
+/* define which file operations are supported */
+static const struct file_operations mpu_fops = {
+	.owner = THIS_MODULE,
+	.read = mpu_read,
+#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 };
+
+static struct miscdevice i2c_mpu_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "mpu",		/* Same for both 3050 and 6000 */
+	.fops = &mpu_fops,
+};
+
+#define FACTORY_TEST
+#ifdef FACTORY_TEST
+
+static ssize_t mpu3050_power_on(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+
+	pr_info("%s : this_client = %d\n", __func__, (int)this_client);
+	count = sprintf(buf, "%d\n", (this_client != NULL ? 1 : 0));
+
+	return count;
+}
+
+static int mpu3050_factory_on(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu = 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 prev_gyro_suspended;
+	pr_info("%s", __func__);
+	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);
+
+	prev_gyro_suspended = mldl_cfg->gyro_is_suspended;
+	if (prev_gyro_suspended) {
+		unsigned long sensors = mldl_cfg->requested_sensors;
+		(void)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);
+	}
+
+	return prev_gyro_suspended;
+}
+
+static void mpu3050_factory_off(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu = 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;
+	pr_info("%s", __func__);
+	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);
+
+	(void)mpu3050_suspend(mldl_cfg,
+			      client->adapter,
+			      accel_adapter,
+			      compass_adapter,
+			      pressure_adapter, TRUE, TRUE, TRUE, TRUE);
+}
+
+static ssize_t mpu3050_get_temp(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	short int temperature = 0;
+	unsigned char data[2];
+	int prev_gyro_suspended;
+	pr_info("%s", __func__);
+	prev_gyro_suspended = mpu3050_factory_on(this_client);
+
+	/*MPUREG_TEMP_OUT_H, 27 0x1b */
+	/*MPUREG_TEMP_OUT_L, 28 0x1c */
+	/* TEMP_OUT_H/L: 16-bit temperature data (2's complement data format) */
+	sensor_i2c_read(this_client->adapter, DEFAULT_MPU_SLAVEADDR,
+			MPUREG_TEMP_OUT_H, 2, data);
+	temperature = (short)(((data[0]) << 8) | data[1]);
+	temperature = (((temperature + 13200) / 280) + 35);
+	pr_info("%s :read temperature = %d\n", __func__, temperature);
+
+	count = sprintf(buf, "%d\n", temperature);
+
+	if (prev_gyro_suspended)
+		mpu3050_factory_off(this_client);
+
+	return count;
+}
+
+/*
+	Defines
+*/
+
+#define DEBUG_OUT 1
+
+#define DEF_GYRO_FULLSCALE	(2000)	/* gyro full scale dps */
+#define DEF_GYRO_SENS		(32768.f/DEF_GYRO_FULLSCALE)
+					/* gyro sensitivity LSB/dps */
+#define DEF_PACKET_THRESH	(75)	/* 600 ms / 8ms / sample */
+#define DEF_TIMING_TOL		(.05f)	/* 5% */
+#define DEF_BIAS_THRESH		(40*DEF_GYRO_SENS)
+					/* 40 dps in LSBs */
+#define DEF_RMS_THRESH_SQ	(0.4f*0.4f*DEF_GYRO_SENS*DEF_GYRO_SENS)
+					/* (.2 dps in LSBs ) ^ 2 */
+#define DEF_TEST_TIME_PER_AXIS	(600)	/* ms of time spent collecting
+					   data for each axis,
+					   multiple of 600ms */
+
+/*
+	Macros
+*/
+
+#define CHECK_TEST_ERROR(x)						\
+	if (x) {							\
+		pr_info("error %d @ %s|%d\n", x, __func__, __LINE__);	\
+		return -1;						\
+	}
+
+#define SHORT_TO_TEMP_C(shrt)	(((shrt+13200)/280)+35)
+#define CHARS_TO_SHORT(d)	((((short)(d)[0])<<8)+(d)[1])
+#define fabs(x)			(((x) < 0) ? -(x) : (x))
+
+void mpu3050_usleep(unsigned long t)
+{
+	unsigned long start = MLOSGetTickCount();
+	while (MLOSGetTickCount() - start < t / 1000) {
+	}
+}
+
+#define X	(0)
+#define Y	(1)
+#define Z	(2)
+
+static short mpu3050_selftest_gyro_avg[3];
+static int mpu3050_selftest_result;
+static int mpu3050_selftest_bias[3];
+static int mpu3050_selftest_rms[3];
+
+int mpu3050_test_gyro(struct i2c_client *client, short gyro_biases[3],
+		      short *temp_avg)
+{
+	void *mlsl_handle = client->adapter;
+	int retVal = 0;
+	tMLError result;
+
+	int total_count = 0;
+	int total_count_axis[3] = { 0, 0, 0 };
+	int packet_count;
+	unsigned char regs[7];
+
+	char a_name[3][2] = { "X", "Y", "Z" };
+	int temperature;
+	int Avg[3];
+	int RMS[3];
+	int i, j, tmp;
+	unsigned char dataout[20];
+
+	short *x, *y, *z;
+
+	x = kzalloc(sizeof(*x) * (DEF_TEST_TIME_PER_AXIS / 8 * 4), GFP_KERNEL);
+	y = kzalloc(sizeof(*y) * (DEF_TEST_TIME_PER_AXIS / 8 * 4), GFP_KERNEL);
+	z = kzalloc(sizeof(*z) * (DEF_TEST_TIME_PER_AXIS / 8 * 4), GFP_KERNEL);
+
+	temperature = 0;
+
+	/* sample rate = 8ms */
+	result = MLSLSerialWriteSingle(mlsl_handle, client->addr,
+					MPUREG_SMPLRT_DIV, 0x07);
+
+	if (result)
+		goto out_i2c_faild;
+
+	regs[0] = 0x03;		/* filter = 42Hz, analog_sample rate = 1 KHz */
+	switch (DEF_GYRO_FULLSCALE) {
+	case 2000:
+		regs[0] |= 0x18;
+		break;
+	case 1000:
+		regs[0] |= 0x10;
+		break;
+	case 500:
+		regs[0] |= 0x08;
+		break;
+	case 250:
+	default:
+		regs[0] |= 0x00;
+		break;
+	}
+	result = MLSLSerialWriteSingle(mlsl_handle, client->addr,
+				       MPUREG_DLPF_FS_SYNC, regs[0]);
+	if (result)
+		goto out_i2c_faild;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, client->addr,
+				       MPUREG_INT_CFG, 0x00);
+
+	/* 1st, timing test */
+	for (j = 0; j < 3; j++) {
+
+		pr_info("%s :Collecting gyro data from %s gyro PLL\n",
+			__func__, a_name[j]);
+
+		/* turn on all gyros, use gyro X for clocking
+		   Set to Y and Z for 2nd and 3rd iteration */
+		result = MLSLSerialWriteSingle(mlsl_handle, client->addr,
+					       MPUREG_PWR_MGM, j + 1);
+		if (result)
+			goto out_i2c_faild;
+
+		/* wait for 2 ms after switching clock source */
+		mpu3050_usleep(2000);
+
+		/* we will enable XYZ gyro in FIFO and nothing else */
+		result = MLSLSerialWriteSingle(mlsl_handle, client->addr,
+					       MPUREG_FIFO_EN2, 0x00);
+		if (result)
+			goto out_i2c_faild;
+		/* enable/reset FIFO */
+		result = MLSLSerialWriteSingle(mlsl_handle, client->addr,
+					       MPUREG_USER_CTRL, 0x42);
+
+		tmp = (int)(DEF_TEST_TIME_PER_AXIS / 600);
+
+		while (tmp-- > 0) {
+			/* enable XYZ gyro in FIFO and nothing else */
+			result =
+			    MLSLSerialWriteSingle(mlsl_handle, client->addr,
+						  MPUREG_FIFO_EN1, 0x70);
+			if (result)
+				goto out_i2c_faild;
+
+			/* wait for 600 ms for data */
+			mpu3050_usleep(600000);
+
+			/* stop storing gyro in the FIFO */
+			result =
+			    MLSLSerialWriteSingle(mlsl_handle, client->addr,
+						  MPUREG_FIFO_EN1, 0x00);
+			if (result)
+				goto out_i2c_faild;
+
+			/* Getting number of bytes in FIFO */
+			result = MLSLSerialRead(mlsl_handle, client->addr,
+						MPUREG_FIFO_COUNTH, 2, dataout);
+			if (result)
+				goto out_i2c_faild;
+			/* number of 6 B packets in the FIFO */
+			packet_count = CHARS_TO_SHORT(dataout) / 6;
+			pr_info("%s :Packet Count: %d - ",
+				__func__, packet_count);
+
+			if (abs(packet_count - DEF_PACKET_THRESH)
+			    <=	/* Within +-5% range */
+			    (int)(DEF_TIMING_TOL * DEF_PACKET_THRESH + .5)) {
+				for (i = 0; i < packet_count; i++) {
+					/* getting FIFO data */
+					result =
+					    MLSLSerialReadFifo(mlsl_handle,
+							client->addr, 6,
+							dataout);
+					if (result)
+						goto out_i2c_faild;
+
+					x[total_count + i] =
+					    CHARS_TO_SHORT(&dataout[0]);
+					y[total_count + i] =
+					    CHARS_TO_SHORT(&dataout[2]);
+					z[total_count + i] =
+					    CHARS_TO_SHORT(&dataout[4]);
+					if (DEBUG_OUT && 0) {
+						pr_info("%s :Gyros %-4d    " \
+						": %+13d %+13d %+13d\n",
+						__func__, total_count + i,
+						x[total_count + i],
+						y[total_count + i],
+						z[total_count + i]);
+					}
+				}
+				total_count += packet_count;
+				total_count_axis[j] += packet_count;
+				pr_info("%s :OK\n", __func__);
+			} else {
+				retVal |= 1 << j;
+				pr_info("%s :NOK - samples ignored\n",
+					__func__);
+			}
+		}
+
+		/* remove gyros from FIFO */
+		result = MLSLSerialWriteSingle(mlsl_handle, client->addr,
+					       MPUREG_FIFO_EN1, 0x00);
+		if (result)
+			goto out_i2c_faild;
+
+		/* Read Temperature */
+		result = MLSLSerialRead(mlsl_handle, client->addr,
+					MPUREG_TEMP_OUT_H, 2, dataout);
+		temperature += (short)CHARS_TO_SHORT(dataout);
+	}
+
+	pr_info("%s :\nTotal %d samples\n\n", __func__, total_count);
+
+	/* 2nd, check bias from X and Y PLL clock source */
+	tmp = total_count != 0 ? total_count : 1;
+	for (i = 0, Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f;
+	     i < total_count; i++) {
+		Avg[X] += x[i];
+		Avg[Y] += y[i];
+		Avg[Z] += z[i];
+	}
+
+	Avg[X] /= tmp;
+	Avg[Y] /= tmp;
+	Avg[Z] /= tmp;
+
+	pr_info("%s :bias          : %+13d %+13d %+13d (LSB)\n",
+		__func__, Avg[X], Avg[Y], Avg[Z]);
+	if (DEBUG_OUT) {
+		pr_info("%s :              : %+13d %+13d %+13d (dps)\n",
+			__func__, Avg[X] / 131, Avg[Y] / 131, Avg[Z] / 131);
+	}
+	for (j = 0; j < 3; j++) {
+		if (abs(Avg[j]) > (int)DEF_BIAS_THRESH) {
+			pr_info("%s :%s-Gyro bias (%.0d) exceeded threshold "
+				"(threshold = %f)\n", __func__,
+				a_name[j], Avg[j], DEF_BIAS_THRESH);
+			retVal |= 1 << (3 + j);
+		}
+	}
+
+	/* 3rd and finally, check RMS */
+	for (i = 0, RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f;
+	     i < total_count; i++) {
+		RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]);
+		RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]);
+		RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]);
+	}
+
+	for (j = 0; j < 3; j++) {
+		if (RMS[j] > (int)DEF_RMS_THRESH_SQ * total_count) {
+			pr_info
+			    ("%s :%s-Gyro RMS (%d) exceeded threshold (%.4f)\n",
+			     __func__, a_name[j], RMS[j] / total_count,
+			     DEF_RMS_THRESH_SQ);
+			retVal |= 1 << (6 + j);
+		}
+	}
+
+	pr_info("%s :RMS^2           : %+13d %+13d %+13d (LSB-rms)\n",
+		__func__,
+		(RMS[X] / total_count),
+		(RMS[Y] / total_count), (RMS[Z] / total_count));
+	if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) {
+		/*If any of the RMS noise value returns zero,
+		   then we might have dead gyro or FIFO/register failure,
+		   or the part is sleeping */
+		retVal |= 1 << 9;
+	}
+
+	temperature /= 3;
+	if (DEBUG_OUT)
+		pr_info("%s :Temperature   : %+13d %13s %13s (deg. C)\n",
+			__func__, SHORT_TO_TEMP_C(temperature), "", "");
+
+	/* load into final storage */
+	*temp_avg = (short)temperature;
+	gyro_biases[X] = (short)Avg[X];
+	gyro_biases[Y] = (short)Avg[Y];
+	gyro_biases[Z] = (short)Avg[Z];
+
+	mpu3050_selftest_bias[X] = (int)Avg[X];
+	mpu3050_selftest_bias[Y] = (int)Avg[Y];
+	mpu3050_selftest_bias[Z] = (int)Avg[Z];
+
+	mpu3050_selftest_rms[X] = RMS[X] / total_count;
+	mpu3050_selftest_rms[Y] = RMS[Y] / total_count;
+	mpu3050_selftest_rms[Z] = RMS[Z] / total_count;
+
+out_i2c_faild:
+	if (result)
+		pr_info("%s : error %d", __func__, result);
+
+	kfree(x);
+	kfree(y);
+	kfree(z);
+
+	return retVal;
+}
+
+int mpu3050_self_test_once(struct i2c_client *client)
+{
+	void *mlsl_handle = client->adapter;
+	int result = 0;
+
+	short temp_avg;
+
+	unsigned char regs[5];
+	unsigned long testStart = MLOSGetTickCount();
+
+	pr_info("%s :Collecting %d groups of 600 ms samples for each axis\n\n",
+		__func__, DEF_TEST_TIME_PER_AXIS / 600);
+
+	result = MLSLSerialRead(mlsl_handle, client->addr,
+				MPUREG_PWR_MGM, 1, regs);
+	CHECK_TEST_ERROR(result);
+	/* reset */
+	result = MLSLSerialWriteSingle(mlsl_handle, client->addr,
+				       MPUREG_PWR_MGM, regs[0] | 0x80);
+	CHECK_TEST_ERROR(result);
+	MLOSSleep(5);
+	/* wake up */
+	if (regs[0] & 0x40) {
+		result = MLSLSerialWriteSingle(mlsl_handle, client->addr,
+					       MPUREG_PWR_MGM, 0x00);
+		CHECK_TEST_ERROR(result);
+	}
+	MLOSSleep(60);
+
+	/* collect gyro and temperature data */
+	mpu3050_selftest_result = mpu3050_test_gyro(client,
+						    mpu3050_selftest_gyro_avg,
+						    &temp_avg);
+
+	pr_info("%s :\nTest time : %ld ms\n",
+		__func__, MLOSGetTickCount() - testStart);
+
+	return mpu3050_selftest_result;
+}
+
+static ssize_t mpu3050_self_test(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned char gyro_data[6];
+	short int raw[3];
+	int count = 0;
+	int res = 0;
+	int prev_gyro_suspended;
+
+/*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 */
+
+/* GYRO_XOUT_H/L: 16-bit X gyro output data (2's complement data format) */
+/* GYRO_YOUT_H/L: 16-bit Y gyro output data (2's complement data format) */
+/* GYRO_ZOUT_H/L: 16-bit Z gyro output data (2's complement data format) */
+
+	prev_gyro_suspended = mpu3050_factory_on(this_client);
+
+	mpu3050_self_test_once(this_client);
+
+	res = sensor_i2c_read(this_client->adapter, DEFAULT_MPU_SLAVEADDR,
+			      MPUREG_GYRO_XOUT_H, 6, gyro_data);
+
+	if (res)
+		return 0;
+
+	raw[0] = (short)(((gyro_data[0]) << 8) | gyro_data[1]);
+	raw[1] = (short)(((gyro_data[2]) << 8) | gyro_data[3]);
+	raw[2] = (short)(((gyro_data[4]) << 8) | gyro_data[5]);
+
+	pr_info("%s: %s %s, %d, %d, %d, %d, %d, %d\n", __func__, buf,
+		(!mpu3050_selftest_result ? "OK" : "NG"),
+		raw[0], raw[1], raw[2],
+		mpu3050_selftest_gyro_avg[0],
+		mpu3050_selftest_gyro_avg[1], mpu3050_selftest_gyro_avg[2]);
+
+	count = sprintf(buf, "%s, %d, %d, %d, %d, %d, %d\n",
+			(!mpu3050_selftest_result ? "OK" : "NG"),
+			mpu3050_selftest_bias[0], mpu3050_selftest_bias[1],
+			mpu3050_selftest_bias[2],
+			mpu3050_selftest_rms[0],
+			mpu3050_selftest_rms[1], mpu3050_selftest_rms[2]);
+
+	if (prev_gyro_suspended)
+		mpu3050_factory_off(this_client);
+
+	return count;
+}
+
+static ssize_t mpu3050_acc_read(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	unsigned char acc_data[6];
+	s16 x, y, z;
+	s32 temp;
+
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(this_client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+
+	if (mldl_cfg->accel_is_suspended == 1 ||
+	    (mldl_cfg->dmp_is_running == 0
+	     && mldl_cfg->accel_is_suspended == 0)) {
+		if (is_lis3dh) {
+			if (mldl_cfg->accel_is_suspended == 1) {
+				sensor_i2c_write_register(this_client->adapter,
+							  0x19, 0x20, 0x67);
+				MLOSSleep(1);
+			}
+			sensor_i2c_read(this_client->adapter,
+					0x19, 0x28 | 0x80, 6, acc_data);
+
+			if (mldl_cfg->accel_is_suspended == 1) {
+				sensor_i2c_write_register(this_client->adapter,
+							  0x19, 0x20, 0x18);
+				MLOSSleep(1);
+			}
+		} else
+			sensor_i2c_read(this_client->adapter,
+					0x0F, 0x06, 6, acc_data);
+	} else if (mldl_cfg->dmp_is_running &&
+		   mldl_cfg->accel_is_suspended == 0) {
+		sensor_i2c_read(this_client->adapter,
+				DEFAULT_MPU_SLAVEADDR, 0x23, 6, acc_data);
+	}
+
+	if (is_lis3dh) {
+		x = ((acc_data[0] << 8) | acc_data[1]);
+		x = (x >> 4) + cal_data.x;
+		y = ((acc_data[2] << 8) | acc_data[3]);
+		y = (y >> 4) + cal_data.y;
+		z = ((acc_data[4] << 8) | acc_data[5]);
+		z = (z >> 4) + cal_data.z;
+	} else {
+		temp = (s16) ((acc_data[1] << 4) | (acc_data[0] >> 4))
+		    + cal_data.x;
+		if (temp < 2048)
+			x = (s16) (temp);
+		else
+			x = (s16) ((4096 - temp)) * (-1);
+
+		temp = (s16) ((acc_data[3] << 4) | (acc_data[2] >> 4))
+		    + cal_data.y;
+		if (temp < 2048)
+			y = (s16) (temp);
+		else
+			y = (s16) ((4096 - temp)) * (-1);
+
+		temp = (s16) ((acc_data[5] << 4) | (acc_data[4] >> 4))
+		    + cal_data.z;
+		if (temp < 2048)
+			z = (s16) (temp);
+		else
+			z = (s16) ((4096 - temp)) * (-1);
+	}
+
+#if defined(CONFIG_MACH_P8)
+	/* x *= (-1); */
+	/* y *= (-1); */
+	z *= (-1);
+	return sprintf(buf, "%d, %d, %d\n", y, x, z);
+
+#elif defined(CONFIG_MACH_P8LTE)
+	x *= (-1);
+	/* y *= (-1); */
+	/* z *= (-1); */
+	return sprintf(buf, "%d, %d, %d\n", y, x, z);
+
+#elif defined(CONFIG_MACH_P2)
+	/* x *= (-1); */
+	/* y *= (-1); */
+	z *= (-1);
+	return sprintf(buf, "%d, %d, %d\n", y, x, z);
+
+#else
+	x *= (-1);
+	y *= (-1);
+	z *= (-1);
+	return sprintf(buf, "%d, %d, %d\n", y, x, z);
+
+#endif
+
+}
+
+static ssize_t accel_calibration_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	int err;
+
+	err = accel_open_calibration();
+	if (err < 0)
+		pr_err("%s: accel_open_calibration() failed\n", __func__);
+
+	pr_info("accel_calibration_show :%d %d %d\n",
+		cal_data.x, cal_data.y, cal_data.z);
+
+	if (err < 0)
+		err = 0;
+	else
+		err = 1;
+
+	if (cal_data.x == 0 && cal_data.y == 0 && cal_data.z == 0)
+		err = 0;
+
+	return sprintf(buf, "%d %d %d %d\n",
+		       err, cal_data.x, cal_data.y, cal_data.z);
+}
+
+static ssize_t accel_calibration_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf, size_t size)
+{
+	bool do_calib;
+	int err;
+	int count = 0;
+	char str[11];
+
+	if (sysfs_streq(buf, "1"))
+		do_calib = true;
+	else if (sysfs_streq(buf, "0"))
+		do_calib = false;
+	else {
+		pr_debug("%s: invalid value %d\n", __func__, *buf);
+		return -EINVAL;
+	}
+
+	err = accel_do_calibrate(do_calib);
+	if (err < 0)
+		pr_err("%s: accel_do_calibrate() failed\n", __func__);
+
+	pr_info("accel_calibration_show :%d %d %d\n",
+		cal_data.x, cal_data.y, cal_data.z);
+	if (err > 0)
+		err = 0;
+	count = sprintf(str, "%d\n", err);
+
+	strcpy(str, buf);
+	return count;
+}
+
+static ssize_t get_accel_vendor_name(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s\n", ACCEL_VENDOR_NAME);
+}
+
+static ssize_t get_accel_chip_name(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s\n", ACCEL_CHIP_NAME);
+}
+
+static ssize_t get_gyro_vendor_name(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s\n", GYRO_VENDOR_NAME);
+}
+
+static ssize_t get_gyro_chip_name(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s\n", GYRO_CHIP_NAME);
+}
+
+static struct device_attribute dev_attr_accel_vendor =
+		__ATTR(vendor, S_IRUGO, get_accel_vendor_name, NULL);
+static struct device_attribute dev_attr_accel_chip =
+		__ATTR(name, S_IRUGO, get_accel_chip_name, NULL);
+
+static struct device_attribute dev_attr_gyro_vendor =
+		__ATTR(vendor, S_IRUGO, get_gyro_vendor_name, NULL);
+static struct device_attribute dev_attr_gyro_chip =
+		__ATTR(name, S_IRUGO, get_gyro_chip_name, NULL);
+
+static DEVICE_ATTR(calibration, 0664,
+			accel_calibration_show, accel_calibration_store);
+static DEVICE_ATTR(raw_data, S_IRUGO, mpu3050_acc_read, NULL);
+
+static DEVICE_ATTR(power_on, S_IRUGO | S_IWUSR, mpu3050_power_on, NULL);
+static DEVICE_ATTR(temperature, S_IRUGO | S_IWUSR, mpu3050_get_temp, NULL);
+static DEVICE_ATTR(selftest, S_IRUGO | S_IWUSR, mpu3050_self_test, NULL);
+
+static DEVICE_ATTR(gyro_power_on, S_IRUGO | S_IWUSR, mpu3050_power_on, NULL);
+static DEVICE_ATTR(gyro_get_temp, S_IRUGO | S_IWUSR, mpu3050_get_temp, NULL);
+static DEVICE_ATTR(gyro_selftest, S_IRUGO | S_IWUSR, mpu3050_self_test, NULL);
+
+
+static struct device_attribute *accel_sensor_attrs[] = {
+	&dev_attr_raw_data,
+	&dev_attr_calibration,
+	&dev_attr_accel_vendor,
+	&dev_attr_accel_chip,
+	NULL,
+};
+
+static struct device_attribute *gyro_sensor_attrs[] = {
+	&dev_attr_power_on,
+	&dev_attr_temperature,
+	&dev_attr_selftest,
+	&dev_attr_gyro_vendor,
+	&dev_attr_gyro_chip,
+	NULL,
+};
+
+extern struct class *sec_class;
+extern struct class *sensors_class;
+
+static struct device *sec_mpu3050_dev;
+static struct device *accel_sensor_device;
+static struct device *gyro_sensor_device;
+
+extern int sensors_register(struct device *dev, void *drvdata,
+			    struct device_attribute *attributes[], char *name);
+#endif
+#define FEATURE_MPU_AUTO_PROBING
+
+#if defined(CONFIG_MPU_SENSORS_KXTF9_LIS3DH)
+
+static int mpu350_auto_probe_accel(struct mpu3050_platform_data *pdata)
+{
+	int ret = ML_SUCCESS;
+	unsigned char reg = 0;
+	struct mpu_private_data *mpu;
+	struct mldl_cfg *mldl_cfg;
+	struct i2c_adapter *accel_adapter = NULL;
+	struct ext_slave_descr *slave_descr = NULL;
+	accel_adapter = i2c_get_adapter(pdata->accel.adapt_num);
+
+	slave_descr = lis3dh_get_slave_descr();
+	ret = MLSLSerialRead(accel_adapter, 0x19, 0x0, 1, &reg);
+	if (ret == 0) {
+		pdata->accel.get_slave_descr = lis3dh_get_slave_descr;
+		pdata->accel.address = 0x19;
+
+		mpu = (struct mpu_private_data *)
+		    i2c_get_clientdata(this_client);
+		mldl_cfg = &mpu->mldl_cfg;
+		mldl_cfg->accel = slave_descr;
+/*
+			printk("auto probe : found %s\n",
+				pdata->accel.get_slave_descr()->name);
+*/
+		is_lis3dh = 1;
+	}
+	return ret;
+}
+
+#endif
+
+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;
+	int retry = 5;
+
+	struct i2c_adapter *accel_adapter = NULL;
+	struct i2c_adapter *compass_adapter = NULL;
+	struct i2c_adapter *pressure_adapter = NULL;
+
+	pr_info("================%s===============\n", __func__);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		res = -ENODEV;
+		goto out_check_functionality_failed;
+	}
+#ifdef FACTORY_TEST
+	res = sensors_register(accel_sensor_device, NULL,
+		accel_sensor_attrs, "accelerometer_sensor");
+	if (res)
+		pr_err("%s: cound not register accelerometer "\
+			"sensor device(%d).\n", __func__, res);
+
+	res = sensors_register(gyro_sensor_device, NULL,
+		gyro_sensor_attrs, "gyro_sensor");
+	if (res)
+		pr_err("%s: cound not register gyro "\
+			"sensor device(%d).\n", __func__, res);
+
+	sec_mpu3050_dev = device_create(sec_class, NULL, 0, NULL,
+					"sec_mpu3050");
+	if (IS_ERR(sec_mpu3050_dev))
+		pr_info("%s :Failed to create device!", __func__);
+
+	if (device_create_file(sec_mpu3050_dev, &dev_attr_gyro_power_on) < 0) {
+		pr_info("%s :Failed to create device file(%s)!\n", __func__,
+			dev_attr_gyro_power_on.attr.name);
+		return -1;
+	}
+	if (device_create_file(sec_mpu3050_dev, &dev_attr_gyro_get_temp) < 0) {
+		pr_info("%s :Failed to create device file(%s)!\n", __func__,
+			dev_attr_gyro_get_temp.attr.name);
+		device_remove_file(sec_mpu3050_dev, &dev_attr_gyro_power_on);
+		return -1;
+	}
+	if (device_create_file(sec_mpu3050_dev, &dev_attr_gyro_selftest) < 0) {
+		pr_info("%s :Failed to create device file(%s)!\n", __func__,
+			dev_attr_gyro_selftest.attr.name);
+		device_remove_file(sec_mpu3050_dev, &dev_attr_gyro_power_on);
+		device_remove_file(sec_mpu3050_dev, &dev_attr_gyro_get_temp);
+		return -1;
+	}
+#endif
+	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;
+	pdata = (struct mpu3050_platform_data *)client->dev.platform_data;
+	if (!pdata) {
+		dev_warn(&this_client->adapter->dev,
+			 "Warning no platform data for mpu3050\n");
+	} else {
+		mldl_cfg->pdata = pdata;
+
+		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;
+
+		is_lis3dh = 0;
+#if defined(CONFIG_MPU_SENSORS_KXTF9_LIS3DH)
+		mpu350_auto_probe_accel(pdata);
+		if (pdata->accel.get_slave_descr && !is_lis3dh) {
+			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 (!accel_adapter) {
+				pr_info("%s : accel_adapter i2c get fail",
+					__func__);
+				goto out_accel_failed;
+			}
+
+			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
+#else
+		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 (!accel_adapter) {
+				pr_info("%s : accel_adapter i2c get fail",
+					__func__);
+				goto out_accel_failed;
+			}
+
+			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
+#endif
+		{
+			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 (!compass_adapter) {
+				pr_info("%s : compass_adapter i2c get fail",
+					__func__);
+				goto out_compass_failed;
+			}
+
+			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 (!pressure_adapter) {
+				pr_info("%s : pressure_adapter i2c get fail",
+					__func__);
+				goto out_pressure_failed;
+			}
+
+			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;
+
+	do {
+		res = mpu3050_open(&mpu->mldl_cfg, client->adapter,
+				   accel_adapter, compass_adapter,
+				   pressure_adapter);
+		if (res) {
+			dev_err(&this_client->adapter->dev,
+				"%s i2c Init Error, ret = %d\n", MPU_NAME, res);
+			mpu3050_usleep(5000);
+		}
+	} while (retry-- && res);
+
+	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,
+			 "WARNING: %s irq not assigned\n", MPU_NAME);
+	}
+
+	mpu_accel_init(&mpu->mldl_cfg, client->adapter);
+
+#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:
+out_pressure_failed:
+	if (pdata && pdata->compass.get_slave_descr && pdata->compass.irq)
+		slaveirq_exit(&pdata->compass);
+out_compassirq_failed:
+out_compass_failed:
+	if (pdata && pdata->accel.get_slave_descr && pdata->accel.irq)
+		slaveirq_exit(&pdata->accel);
+out_accelirq_failed:
+out_accel_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);
+
+	mpu_accel_exit(mldl_cfg);
+
+	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,
+		   },
+	.address_list = normal_i2c,
+	.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);
+	pid = 0;
+
+	pr_info("%s res=%d\n", __func__, res);
+	if (res)
+		dev_err(&this_client->adapter->dev, "%s failed\n", __func__);
+	return res;
+}
+
+static void __exit mpu_exit(void)
+{
+	pr_info(KERN_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 100755
index 0000000..b1298d3
--- /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 100755
index 0000000..0bbc8c6
--- /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 100755
index 0000000..8623855
--- /dev/null
+++ b/drivers/misc/mpu3050/mpuirq.c
@@ -0,0 +1,448 @@
+/*
+ $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"
+#include "mpu-accel.h"
+
+#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/ioport.h>
+#include <linux/delay.h>
+#include <linux/serial_core.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+
+#include <mach/regs-gpio.h>
+
+#include <mach/map.h>
+#include <mach/regs-mem.h>
+#include <mach/regs-clock.h>
+#include <mach/media.h>
+#include <mach/gpio.h>
+#endif
+#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);
+
+#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT
+static irqreturn_t mpuirq_selftest_handler(int irq, void *dev_id);
+unsigned long long selftest_get_times[10];
+#endif
+
+static int mpuirq_open(struct inode *inode, struct file *file);
+static int mpuirq_release(struct inode *inode, struct file *file);
+static ssize_t mpuirq_read(struct file *file, char *buf, size_t count,
+			 loff_t *ppos);
+unsigned int mpuirq_poll(struct file *file, struct poll_table_struct *poll);
+static long mpuirq_ioctl(struct file *file, unsigned int cmd,
+			 unsigned long arg);
+static irqreturn_t mpuirq_handler(int irq, void *dev_id);
+
+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;
+	/* we could do some checking on the flags supplied by "open" */
+	/* i.e. O_NONBLOCK */
+	/* -> set some flag to disable interruptible_sleep_on in mpuirq_read */
+	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 > 0) {
+		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;
+
+#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT
+	case MPUIRQ_SET_SELFTEST_IRQ_HANDLER:
+		{
+			int res;
+			/* unregister mpuirq handler */
+			if (mpuirq_dev_data.irq > 0) {
+				free_irq(mpuirq_dev_data.irq,
+					 &mpuirq_dev_data.irq);
+			}
+			/* register selftest irq handler */
+			if (mpuirq_dev_data.irq > 0) {
+				struct mldl_cfg *mldl_cfg =
+				    (struct mldl_cfg *)
+				    i2c_get_clientdata(mpuirq_dev_data.
+					mpu_client);
+				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_selftest_handler, flags,
+						interface,
+						&mpuirq_dev_data.irq);
+
+				if (res) {
+					pr_info("MPUIRQ_SET_SELFTEST_IRQ_" \
+					"HANDLER: cannot register IRQ %d\n",
+					mpuirq_dev_data.irq);
+				}
+			}
+		}
+		break;
+	case MPUIRQ_SET_MPUIRQ_HANDLER:
+		{
+			int res;
+			/* unregister selftest irq handler */
+			if (mpuirq_dev_data.irq > 0)
+				free_irq(mpuirq_dev_data.irq,
+					 &mpuirq_dev_data.irq);
+
+			/*  register mpuirq handler */
+			if (mpuirq_dev_data.irq > 0) {
+				struct mldl_cfg *mldl_cfg =
+				    (struct mldl_cfg *)
+				    i2c_get_clientdata(mpuirq_dev_data.
+						       mpu_client);
+				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);
+			}
+		}
+		break;
+	case MPUIRQ_GET_MPUIRQ_JIFFIES:
+		{
+
+			mpuirq_data.mpuirq_jiffies = 0;
+
+			if (copy_to_user
+			    ((void *)arg, selftest_get_times,
+			     sizeof(selftest_get_times))) {
+				memset(selftest_get_times, 0,
+				       sizeof(selftest_get_times));
+				return -EFAULT;
+			}
+
+			memset(selftest_get_times, 0,
+			       sizeof(selftest_get_times));
+		}
+		break;
+#endif
+	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);
+}
+
+#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT
+static irqreturn_t mpuirq_selftest_handler(int irq, void *dev_id)
+{
+/* static int mycount=0; */
+	struct timeval irqtime;
+	unsigned long long temp_time;
+
+	do_gettimeofday(&irqtime);
+
+	temp_time = (((long long)irqtime.tv_sec) << 32);
+	temp_time += irqtime.tv_usec;
+
+	if (mpuirq_data.mpuirq_jiffies < 10)
+		selftest_get_times[mpuirq_data.mpuirq_jiffies++] = temp_time;
+
+	return IRQ_HANDLED;
+}
+#endif
+
+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 100755
index 0000000..d6c1987
--- /dev/null
+++ b/drivers/misc/mpu3050/mpuirq.h
@@ -0,0 +1,48 @@
+/*
+ $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>
+#endif
+
+#define MPUIRQ_ENABLE_DEBUG          (1)
+#define MPUIRQ_GET_INTERRUPT_CNT     (2)
+#define MPUIRQ_GET_IRQ_TIME          (3)
+#define MPUIRQ_GET_LED_VALUE         (4)
+#define MPUIRQ_SET_TIMEOUT           (5)
+#define MPUIRQ_SET_ACCEL_INFO        (6)
+#define MPUIRQ_SET_FREQUENCY_DIVIDER (7)
+
+#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT
+#define MPUIRQ_GET_MPUIRQ_JIFFIES       (8)
+#define MPUIRQ_SET_SELFTEST_IRQ_HANDLER (9)
+#define MPUIRQ_SET_MPUIRQ_HANDLER       (10)
+#endif
+
+#ifdef __KERNEL__
+
+void mpuirq_exit(void);
+int mpuirq_init(struct i2c_client *mpu_client);
+
+#endif
+
+#endif
diff --git a/drivers/misc/mpu3050/sensors_core.c b/drivers/misc/mpu3050/sensors_core.c
new file mode 100755
index 0000000..b652631
--- /dev/null
+++ b/drivers/misc/mpu3050/sensors_core.c
@@ -0,0 +1,100 @@
+/*
+ *  Universal sensors core class
+ *
+ *  Author : Ryunkyun Park <ryun.park@samsung.com>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+/* #include <linux/sensors_core.h> */
+
+struct class *sensors_class;
+static atomic_t sensor_count;
+static DEFINE_MUTEX(sensors_mutex);
+
+/**
+ * Create sysfs interface
+ */
+static void set_sensor_attr(struct device *dev,
+			    struct device_attribute *attributes[])
+{
+	int i;
+
+	for (i = 0; attributes[i] != NULL; i++) {
+		if ((device_create_file(dev, attributes[i])) < 0) {
+			pr_info("[SENSOR CORE] fail!!! device_create_file" \
+				"( dev, attributes[%d] )\n", i);
+		}
+	}
+}
+
+int sensors_register(struct device *dev, void *drvdata,
+		     struct device_attribute *attributes[], char *name)
+{
+	int ret = 0;
+
+	if (!sensors_class) {
+		sensors_class = class_create(THIS_MODULE, "sensors");
+		if (IS_ERR(sensors_class))
+			return PTR_ERR(sensors_class);
+	}
+
+	mutex_lock(&sensors_mutex);
+
+	dev = device_create(sensors_class, NULL, 0, drvdata, "%s", name);
+
+	if (IS_ERR(dev)) {
+		ret = PTR_ERR(dev);
+		pr_err("[SENSORS CORE] device_create failed! [%d]\n", ret);
+		return ret;
+	}
+
+	set_sensor_attr(dev, attributes);
+
+	atomic_inc(&sensor_count);
+
+	mutex_unlock(&sensors_mutex);
+
+	return 0;
+}
+
+void sensors_unregister(struct device *dev)
+{
+	/* TODO : Unregister device */
+}
+
+static int __init sensors_class_init(void)
+{
+	pr_info("[SENSORS CORE] sensors_class_init\n");
+	sensors_class = class_create(THIS_MODULE, "sensors");
+
+	if (IS_ERR(sensors_class))
+		return PTR_ERR(sensors_class);
+
+	atomic_set(&sensor_count, 0);
+	sensors_class->dev_uevent = NULL;
+
+	return 0;
+}
+
+static void __exit sensors_class_exit(void)
+{
+	class_destroy(sensors_class);
+}
+
+EXPORT_SYMBOL_GPL(sensors_register);
+EXPORT_SYMBOL_GPL(sensors_unregister);
+
+/* exported for the APM Power driver, APM emulation */
+EXPORT_SYMBOL_GPL(sensors_class);
+
+subsys_initcall(sensors_class_init);
+module_exit(sensors_class_exit);
+
+MODULE_DESCRIPTION("Universal sensors core class");
+MODULE_AUTHOR("Ryunkyun Park <ryun.park@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/mpu3050/slaveirq.c b/drivers/misc/mpu3050/slaveirq.c
new file mode 100755
index 0000000..2ee5385
--- /dev/null
+++ b/drivers/misc/mpu3050/slaveirq.c
@@ -0,0 +1,270 @@
+/*
+ $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) {
+		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;
+}
+
+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 */
+	/* and ignore first interrupt generated in module init */
+	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;
+
+	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;
+	}
+
+	init_waitqueue_head(&data->slaveirq_wait);
+	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 100755
index 0000000..ca9c7e4
--- /dev/null
+++ b/drivers/misc/mpu3050/slaveirq.h
@@ -0,0 +1,46 @@
+/*
+ $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_ENABLE_DEBUG          (1)
+#define SLAVEIRQ_GET_INTERRUPT_CNT     (2)
+#define SLAVEIRQ_GET_IRQ_TIME          (3)
+#define SLAVEIRQ_GET_LED_VALUE         (4)
+#define SLAVEIRQ_SET_TIMEOUT           (5)
+#define SLAVEIRQ_SET_SLAVE_INFO        (6)
+
+#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 100755
index 0000000..53b42d2
--- /dev/null
+++ b/drivers/misc/mpu3050/timerirq.c
@@ -0,0 +1,294 @@
+/*
+$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;
+
+	/* dev_info(data->dev->this_device,
+	   "%s, %ld\n", __func__, (unsigned long)data); */
+
+	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;
+
+	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) {
+		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);
+		kfree(data);
+		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 100755
index 0000000..a38b490
--- /dev/null
+++ b/drivers/misc/mpu3050/timerirq.h
@@ -0,0 +1,28 @@
+/*
+ $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__
+
+#define TIMERIRQ_SET_TIMEOUT           (5)
+#define TIMERIRQ_GET_INTERRUPT_CNT     (7)
+#define TIMERIRQ_START                 (8)
+#define TIMERIRQ_STOP                  (9)
+
+#endif
diff --git a/drivers/misc/pmem.c b/drivers/misc/pmem.c
new file mode 100644
index 0000000..c7c9179
--- /dev/null
+++ b/drivers/misc/pmem.c
@@ -0,0 +1,1386 @@
+/* drivers/android/pmem.c
+ *
+ * Copyright (C) 2007 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 <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/mm.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/debugfs.h>
+#include <linux/android_pmem.h>
+#include <linux/mempolicy.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/cacheflush.h>
+#if defined(CONFIG_S5P_MEM_CMA)
+#include <linux/cma.h>
+#endif
+
+#define PMEM_MAX_DEVICES 10
+#define PMEM_MAX_ORDER 128
+#define PMEM_MIN_ALLOC PAGE_SIZE
+
+#define PMEM_DEBUG 1
+
+/* indicates that a refernce to this file has been taken via get_pmem_file,
+ * the file should not be released until put_pmem_file is called */
+#define PMEM_FLAGS_BUSY 0x1
+/* indicates that this is a suballocation of a larger master range */
+#define PMEM_FLAGS_CONNECTED 0x1 << 1
+/* indicates this is a master and not a sub allocation and that it is mmaped */
+#define PMEM_FLAGS_MASTERMAP 0x1 << 2
+/* submap and unsubmap flags indicate:
+ * 00: subregion has never been mmaped
+ * 10: subregion has been mmaped, reference to the mm was taken
+ * 11: subretion has ben released, refernece to the mm still held
+ * 01: subretion has been released, reference to the mm has been released
+ */
+#define PMEM_FLAGS_SUBMAP 0x1 << 3
+#define PMEM_FLAGS_UNSUBMAP 0x1 << 4
+
+
+struct pmem_data {
+	/* in alloc mode: an index into the bitmap
+	 * in no_alloc mode: the size of the allocation */
+	int index;
+	/* see flags above for descriptions */
+	unsigned int flags;
+	/* protects this data field, if the mm_mmap sem will be held at the
+	 * same time as this sem, the mm sem must be taken first (as this is
+	 * the order for vma_open and vma_close ops */
+	struct rw_semaphore sem;
+	/* info about the mmaping process */
+	struct vm_area_struct *vma;
+	/* task struct of the mapping process */
+	struct task_struct *task;
+	/* process id of teh mapping process */
+	pid_t pid;
+	/* file descriptor of the master */
+	int master_fd;
+	/* file struct of the master */
+	struct file *master_file;
+	/* a list of currently available regions if this is a suballocation */
+	struct list_head region_list;
+	/* a linked list of data so we can access them for debugging */
+	struct list_head list;
+#if PMEM_DEBUG
+	int ref;
+#endif
+};
+
+struct pmem_bits {
+	unsigned allocated:1;		/* 1 if allocated, 0 if free */
+	unsigned order:7;		/* size of the region in pmem space */
+};
+
+struct pmem_region_node {
+	struct pmem_region region;
+	struct list_head list;
+};
+
+#define PMEM_DEBUG_MSGS 0
+#if PMEM_DEBUG_MSGS
+#define DLOG(fmt,args...) \
+	do { printk(KERN_INFO "[%s:%s:%d] "fmt, __FILE__, __func__, __LINE__, \
+		    ##args); } \
+	while (0)
+#else
+#define DLOG(x...) do {} while (0)
+#endif
+
+struct pmem_info {
+	struct miscdevice dev;
+	/* physical start address of the remaped pmem space */
+	unsigned long base;
+	/* vitual start address of the remaped pmem space */
+	unsigned char __iomem *vbase;
+	/* total size of the pmem space */
+	unsigned long size;
+	/* number of entries in the pmem space */
+	unsigned long num_entries;
+	/* pfn of the garbage page in memory */
+	unsigned long garbage_pfn;
+	/* index of the garbage page in the pmem space */
+	int garbage_index;
+	/* the bitmap for the region indicating which entries are allocated
+	 * and which are free */
+	struct pmem_bits *bitmap;
+	/* indicates the region should not be managed with an allocator */
+	unsigned no_allocator;
+	/* indicates maps of this region should be cached, if a mix of
+	 * cached and uncached is desired, set this and open the device with
+	 * O_SYNC to get an uncached region */
+	unsigned cached;
+	unsigned buffered;
+	/* in no_allocator mode the first mapper gets the whole space and sets
+	 * this flag */
+	unsigned allocated;
+	/* for debugging, creates a list of pmem file structs, the
+	 * data_list_lock should be taken before pmem_data->sem if both are
+	 * needed */
+	struct mutex data_list_lock;
+	struct list_head data_list;
+	/* pmem_sem protects the bitmap array
+	 * a write lock should be held when modifying entries in bitmap
+	 * a read lock should be held when reading data from bits or
+	 * dereferencing a pointer into bitmap
+	 *
+	 * pmem_data->sem protects the pmem data of a particular file
+	 * Many of the function that require the pmem_data->sem have a non-
+	 * locking version for when the caller is already holding that sem.
+	 *
+	 * IF YOU TAKE BOTH LOCKS TAKE THEM IN THIS ORDER:
+	 * down(pmem_data->sem) => down(bitmap_sem)
+	 */
+	struct rw_semaphore bitmap_sem;
+
+	long (*ioctl)(struct file *, unsigned int, unsigned long);
+	int (*release)(struct inode *, struct file *);
+};
+
+static struct pmem_info pmem[PMEM_MAX_DEVICES];
+static int id_count;
+
+#define PMEM_IS_FREE(id, index) !(pmem[id].bitmap[index].allocated)
+#define PMEM_ORDER(id, index) pmem[id].bitmap[index].order
+#define PMEM_BUDDY_INDEX(id, index) (index ^ (1 << PMEM_ORDER(id, index)))
+#define PMEM_NEXT_INDEX(id, index) (index + (1 << PMEM_ORDER(id, index)))
+#define PMEM_OFFSET(index) (index * PMEM_MIN_ALLOC)
+#define PMEM_START_ADDR(id, index) (PMEM_OFFSET(index) + pmem[id].base)
+#define PMEM_LEN(id, index) ((1 << PMEM_ORDER(id, index)) * PMEM_MIN_ALLOC)
+#define PMEM_END_ADDR(id, index) (PMEM_START_ADDR(id, index) + \
+	PMEM_LEN(id, index))
+#define PMEM_START_VADDR(id, index) (PMEM_OFFSET(id, index) + pmem[id].vbase)
+#define PMEM_END_VADDR(id, index) (PMEM_START_VADDR(id, index) + \
+	PMEM_LEN(id, index))
+#define PMEM_REVOKED(data) (data->flags & PMEM_FLAGS_REVOKED)
+#define PMEM_IS_PAGE_ALIGNED(addr) (!((addr) & (~PAGE_MASK)))
+#define PMEM_IS_SUBMAP(data) ((data->flags & PMEM_FLAGS_SUBMAP) && \
+	(!(data->flags & PMEM_FLAGS_UNSUBMAP)))
+
+static int pmem_release(struct inode *, struct file *);
+static int pmem_mmap(struct file *, struct vm_area_struct *);
+static int pmem_open(struct inode *, struct file *);
+static long pmem_ioctl(struct file *, unsigned int, unsigned long);
+
+struct file_operations pmem_fops = {
+	.release = pmem_release,
+	.mmap = pmem_mmap,
+	.open = pmem_open,
+	.unlocked_ioctl = pmem_ioctl,
+};
+
+static int get_id(struct file *file)
+{
+	return MINOR(file->f_dentry->d_inode->i_rdev);
+}
+
+int is_pmem_file(struct file *file)
+{
+	int id;
+
+	if (unlikely(!file || !file->f_dentry || !file->f_dentry->d_inode))
+		return 0;
+	id = get_id(file);
+	if (unlikely(id >= PMEM_MAX_DEVICES))
+		return 0;
+	if (unlikely(file->f_dentry->d_inode->i_rdev !=
+	     MKDEV(MISC_MAJOR, pmem[id].dev.minor)))
+		return 0;
+	return 1;
+}
+
+static int has_allocation(struct file *file)
+{
+	struct pmem_data *data;
+	/* check is_pmem_file first if not accessed via pmem_file_ops */
+
+	if (unlikely(!file->private_data))
+		return 0;
+	data = (struct pmem_data *)file->private_data;
+	if (unlikely(data->index < 0))
+		return 0;
+	return 1;
+}
+
+static int is_master_owner(struct file *file)
+{
+	struct file *master_file;
+	struct pmem_data *data;
+	int put_needed, ret = 0;
+
+	if (!is_pmem_file(file) || !has_allocation(file))
+		return 0;
+	data = (struct pmem_data *)file->private_data;
+	if (PMEM_FLAGS_MASTERMAP & data->flags)
+		return 1;
+	master_file = fget_light(data->master_fd, &put_needed);
+	if (master_file && data->master_file == master_file)
+		ret = 1;
+	fput_light(master_file, put_needed);
+	return ret;
+}
+
+static int pmem_free(int id, int index)
+{
+	/* caller should hold the write lock on pmem_sem! */
+	int buddy, curr = index;
+	DLOG("index %d\n", index);
+
+	if (pmem[id].no_allocator) {
+		pmem[id].allocated = 0;
+		return 0;
+	}
+	/* clean up the bitmap, merging any buddies */
+	pmem[id].bitmap[curr].allocated = 0;
+	/* find a slots buddy Buddy# = Slot# ^ (1 << order)
+	 * if the buddy is also free merge them
+	 * repeat until the buddy is not free or end of the bitmap is reached
+	 */
+	do {
+		buddy = PMEM_BUDDY_INDEX(id, curr);
+		if (PMEM_IS_FREE(id, buddy) &&
+				PMEM_ORDER(id, buddy) == PMEM_ORDER(id, curr)) {
+			PMEM_ORDER(id, buddy)++;
+			PMEM_ORDER(id, curr)++;
+			curr = min(buddy, curr);
+		} else {
+			break;
+		}
+	} while (curr < pmem[id].num_entries);
+
+	return 0;
+}
+
+static void pmem_revoke(struct file *file, struct pmem_data *data);
+
+static int pmem_release(struct inode *inode, struct file *file)
+{
+	struct pmem_data *data = (struct pmem_data *)file->private_data;
+	struct pmem_region_node *region_node;
+	struct list_head *elt, *elt2;
+	int id = get_id(file), ret = 0;
+
+
+	mutex_lock(&pmem[id].data_list_lock);
+	/* if this file is a master, revoke all the memory in the connected
+	 *  files */
+	if (PMEM_FLAGS_MASTERMAP & data->flags) {
+		struct pmem_data *sub_data;
+		list_for_each(elt, &pmem[id].data_list) {
+			sub_data = list_entry(elt, struct pmem_data, list);
+			down_read(&sub_data->sem);
+			if (PMEM_IS_SUBMAP(sub_data) &&
+			    file == sub_data->master_file) {
+				up_read(&sub_data->sem);
+				pmem_revoke(file, sub_data);
+			}  else
+				up_read(&sub_data->sem);
+		}
+	}
+	list_del(&data->list);
+	mutex_unlock(&pmem[id].data_list_lock);
+
+
+	down_write(&data->sem);
+
+	/* if its not a conencted file and it has an allocation, free it */
+	if (!(PMEM_FLAGS_CONNECTED & data->flags) && has_allocation(file)) {
+		down_write(&pmem[id].bitmap_sem);
+		ret = pmem_free(id, data->index);
+		up_write(&pmem[id].bitmap_sem);
+	}
+
+	/* if this file is a submap (mapped, connected file), downref the
+	 * task struct */
+	if (PMEM_FLAGS_SUBMAP & data->flags)
+		if (data->task) {
+			put_task_struct(data->task);
+			data->task = NULL;
+		}
+
+	file->private_data = NULL;
+
+	list_for_each_safe(elt, elt2, &data->region_list) {
+		region_node = list_entry(elt, struct pmem_region_node, list);
+		list_del(elt);
+		kfree(region_node);
+	}
+	BUG_ON(!list_empty(&data->region_list));
+
+	up_write(&data->sem);
+	kfree(data);
+	if (pmem[id].release)
+		ret = pmem[id].release(inode, file);
+
+	return ret;
+}
+
+static int pmem_open(struct inode *inode, struct file *file)
+{
+	struct pmem_data *data;
+	int id = get_id(file);
+	int ret = 0;
+
+	DLOG("current %u file %p(%d)\n", current->pid, file, file_count(file));
+	/* setup file->private_data to indicate its unmapped */
+	/*  you can only open a pmem device one time */
+	if (file->private_data != NULL && file_count(file) != 1)
+		return -1;
+	data = kmalloc(sizeof(struct pmem_data), GFP_KERNEL);
+	if (!data) {
+		printk("pmem: unable to allocate memory for pmem metadata.");
+		return -1;
+	}
+	data->flags = 0;
+	data->index = -1;
+	data->task = NULL;
+	data->vma = NULL;
+	data->pid = 0;
+	data->master_file = NULL;
+#if PMEM_DEBUG
+	data->ref = 0;
+#endif
+	INIT_LIST_HEAD(&data->region_list);
+	init_rwsem(&data->sem);
+
+	file->private_data = data;
+	INIT_LIST_HEAD(&data->list);
+
+	mutex_lock(&pmem[id].data_list_lock);
+	list_add(&data->list, &pmem[id].data_list);
+	mutex_unlock(&pmem[id].data_list_lock);
+	return ret;
+}
+
+static unsigned long pmem_order(unsigned long len)
+{
+	int i;
+
+	len = (len + PMEM_MIN_ALLOC - 1)/PMEM_MIN_ALLOC;
+	len--;
+	for (i = 0; i < sizeof(len)*8; i++)
+		if (len >> i == 0)
+			break;
+	return i;
+}
+
+static int pmem_allocate(int id, unsigned long len)
+{
+	/* caller should hold the write lock on pmem_sem! */
+	/* return the corresponding pdata[] entry */
+	int curr = 0;
+	int end = pmem[id].num_entries;
+	int best_fit = -1;
+	unsigned long order = pmem_order(len);
+
+	if (pmem[id].no_allocator) {
+		DLOG("no allocator");
+		if ((len > pmem[id].size) || pmem[id].allocated)
+			return -1;
+		pmem[id].allocated = 1;
+		return len;
+	}
+
+	if (order > PMEM_MAX_ORDER)
+		return -1;
+	DLOG("order %lx\n", order);
+
+	/* look through the bitmap:
+	 * 	if you find a free slot of the correct order use it
+	 * 	otherwise, use the best fit (smallest with size > order) slot
+	 */
+	while (curr < end) {
+		if (PMEM_IS_FREE(id, curr)) {
+			if (PMEM_ORDER(id, curr) == (unsigned char)order) {
+				/* set the not free bit and clear others */
+				best_fit = curr;
+				break;
+			}
+			if (PMEM_ORDER(id, curr) > (unsigned char)order &&
+			    (best_fit < 0 ||
+			     PMEM_ORDER(id, curr) < PMEM_ORDER(id, best_fit)))
+				best_fit = curr;
+		}
+		curr = PMEM_NEXT_INDEX(id, curr);
+	}
+
+	/* if best_fit < 0, there are no suitable slots,
+	 * return an error
+	 */
+	if (best_fit < 0) {
+		printk("pmem: no space left to allocate!\n");
+		return -1;
+	}
+
+	/* now partition the best fit:
+	 * 	split the slot into 2 buddies of order - 1
+	 * 	repeat until the slot is of the correct order
+	 */
+	while (PMEM_ORDER(id, best_fit) > (unsigned char)order) {
+		int buddy;
+		PMEM_ORDER(id, best_fit) -= 1;
+		buddy = PMEM_BUDDY_INDEX(id, best_fit);
+		PMEM_ORDER(id, buddy) = PMEM_ORDER(id, best_fit);
+	}
+	pmem[id].bitmap[best_fit].allocated = 1;
+	return best_fit;
+}
+
+static pgprot_t pmem_access_prot(struct file *file, pgprot_t vma_prot)
+{
+	int id = get_id(file);
+#ifdef pgprot_noncached
+	if (pmem[id].cached == 0 || file->f_flags & O_SYNC)
+		return pgprot_noncached(vma_prot);
+#endif
+#ifdef pgprot_ext_buffered
+	else if (pmem[id].buffered)
+		return pgprot_ext_buffered(vma_prot);
+#endif
+	return vma_prot;
+}
+
+static unsigned long pmem_start_addr(int id, struct pmem_data *data)
+{
+	if (pmem[id].no_allocator)
+		return PMEM_START_ADDR(id, 0);
+	else
+		return PMEM_START_ADDR(id, data->index);
+
+}
+
+static void *pmem_start_vaddr(int id, struct pmem_data *data)
+{
+	return pmem_start_addr(id, data) - pmem[id].base + pmem[id].vbase;
+}
+
+static unsigned long pmem_len(int id, struct pmem_data *data)
+{
+	if (pmem[id].no_allocator)
+		return data->index;
+	else
+		return PMEM_LEN(id, data->index);
+}
+
+static int pmem_map_garbage(int id, struct vm_area_struct *vma,
+			    struct pmem_data *data, unsigned long offset,
+			    unsigned long len)
+{
+	int i, garbage_pages = len >> PAGE_SHIFT;
+
+	vma->vm_flags |= VM_IO | VM_RESERVED | VM_PFNMAP | VM_SHARED | VM_WRITE;
+	for (i = 0; i < garbage_pages; i++) {
+		if (vm_insert_pfn(vma, vma->vm_start + offset + (i * PAGE_SIZE),
+		    pmem[id].garbage_pfn))
+			return -EAGAIN;
+	}
+	return 0;
+}
+
+static int pmem_unmap_pfn_range(int id, struct vm_area_struct *vma,
+				struct pmem_data *data, unsigned long offset,
+				unsigned long len)
+{
+	int garbage_pages;
+	DLOG("unmap offset %lx len %lx\n", offset, len);
+
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(len));
+
+	garbage_pages = len >> PAGE_SHIFT;
+	zap_page_range(vma, vma->vm_start + offset, len, NULL);
+	pmem_map_garbage(id, vma, data, offset, len);
+	return 0;
+}
+
+static int pmem_map_pfn_range(int id, struct vm_area_struct *vma,
+			      struct pmem_data *data, unsigned long offset,
+			      unsigned long len)
+{
+	DLOG("map offset %lx len %lx\n", offset, len);
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(vma->vm_start));
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(vma->vm_end));
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(len));
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(offset));
+
+	if (io_remap_pfn_range(vma, vma->vm_start + offset,
+		(pmem_start_addr(id, data) + offset) >> PAGE_SHIFT,
+		len, vma->vm_page_prot)) {
+		return -EAGAIN;
+	}
+	return 0;
+}
+
+static int pmem_remap_pfn_range(int id, struct vm_area_struct *vma,
+			      struct pmem_data *data, unsigned long offset,
+			      unsigned long len)
+{
+	/* hold the mm semp for the vma you are modifying when you call this */
+	BUG_ON(!vma);
+	zap_page_range(vma, vma->vm_start + offset, len, NULL);
+	return pmem_map_pfn_range(id, vma, data, offset, len);
+}
+
+static void pmem_vma_open(struct vm_area_struct *vma)
+{
+	struct file *file = vma->vm_file;
+	struct pmem_data *data = file->private_data;
+	int id = get_id(file);
+	/* this should never be called as we don't support copying pmem
+	 * ranges via fork */
+	BUG_ON(!has_allocation(file));
+	down_write(&data->sem);
+	/* remap the garbage pages, forkers don't get access to the data */
+	pmem_unmap_pfn_range(id, vma, data, 0, vma->vm_start - vma->vm_end);
+	up_write(&data->sem);
+}
+
+static void pmem_vma_close(struct vm_area_struct *vma)
+{
+	struct file *file = vma->vm_file;
+	struct pmem_data *data = file->private_data;
+
+	DLOG("current %u ppid %u file %p count %d\n", current->pid,
+	     current->parent->pid, file, file_count(file));
+	if (unlikely(!is_pmem_file(file) || !has_allocation(file))) {
+		printk(KERN_WARNING "pmem: something is very wrong, you are "
+		       "closing a vm backing an allocation that doesn't "
+		       "exist!\n");
+		return;
+	}
+	down_write(&data->sem);
+	if (data->vma == vma) {
+		data->vma = NULL;
+		if ((data->flags & PMEM_FLAGS_CONNECTED) &&
+		    (data->flags & PMEM_FLAGS_SUBMAP))
+			data->flags |= PMEM_FLAGS_UNSUBMAP;
+	}
+	/* the kernel is going to free this vma now anyway */
+	up_write(&data->sem);
+}
+
+static struct vm_operations_struct vm_ops = {
+	.open = pmem_vma_open,
+	.close = pmem_vma_close,
+};
+
+static int pmem_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct pmem_data *data;
+	int index;
+	unsigned long vma_size =  vma->vm_end - vma->vm_start;
+	int ret = 0, id = get_id(file);
+
+	if (vma->vm_pgoff || !PMEM_IS_PAGE_ALIGNED(vma_size)) {
+#if PMEM_DEBUG
+		printk(KERN_ERR "pmem: mmaps must be at offset zero, aligned"
+				" and a multiple of pages_size.\n");
+#endif
+		return -EINVAL;
+	}
+
+	data = (struct pmem_data *)file->private_data;
+	down_write(&data->sem);
+	/* check this file isn't already mmaped, for submaps check this file
+	 * has never been mmaped */
+	if ((data->flags & PMEM_FLAGS_SUBMAP) ||
+	    (data->flags & PMEM_FLAGS_UNSUBMAP)) {
+#if PMEM_DEBUG
+		printk(KERN_ERR "pmem: you can only mmap a pmem file once, "
+		       "this file is already mmaped. %x\n", data->flags);
+#endif
+		ret = -EINVAL;
+		goto error;
+	}
+	/* if file->private_data == unalloced, alloc*/
+	if (data && data->index == -1) {
+		down_write(&pmem[id].bitmap_sem);
+		index = pmem_allocate(id, vma->vm_end - vma->vm_start);
+		up_write(&pmem[id].bitmap_sem);
+		data->index = index;
+	}
+	/* either no space was available or an error occured */
+	if (!has_allocation(file)) {
+		ret = -EINVAL;
+		printk("pmem: could not find allocation for map.\n");
+		goto error;
+	}
+
+	if (pmem_len(id, data) < vma_size) {
+#if PMEM_DEBUG
+		printk(KERN_WARNING "pmem: mmap size [%lu] does not match"
+		       "size of backing region [%lu].\n", vma_size,
+		       pmem_len(id, data));
+#endif
+		ret = -EINVAL;
+		goto error;
+	}
+
+	vma->vm_pgoff = pmem_start_addr(id, data) >> PAGE_SHIFT;
+	vma->vm_page_prot = pmem_access_prot(file, vma->vm_page_prot);
+
+	if (data->flags & PMEM_FLAGS_CONNECTED) {
+		struct pmem_region_node *region_node;
+		struct list_head *elt;
+		if (pmem_map_garbage(id, vma, data, 0, vma_size)) {
+			printk("pmem: mmap failed in kernel!\n");
+			ret = -EAGAIN;
+			goto error;
+		}
+		list_for_each(elt, &data->region_list) {
+			region_node = list_entry(elt, struct pmem_region_node,
+						 list);
+			DLOG("remapping file: %p %lx %lx\n", file,
+				region_node->region.offset,
+				region_node->region.len);
+			if (pmem_remap_pfn_range(id, vma, data,
+						 region_node->region.offset,
+						 region_node->region.len)) {
+				ret = -EAGAIN;
+				goto error;
+			}
+		}
+		data->flags |= PMEM_FLAGS_SUBMAP;
+		get_task_struct(current->group_leader);
+		data->task = current->group_leader;
+		data->vma = vma;
+#if PMEM_DEBUG
+		data->pid = current->pid;
+#endif
+		DLOG("submmapped file %p vma %p pid %u\n", file, vma,
+		     current->pid);
+	} else {
+		if (pmem_map_pfn_range(id, vma, data, 0, vma_size)) {
+			printk(KERN_INFO "pmem: mmap failed in kernel!\n");
+			ret = -EAGAIN;
+			goto error;
+		}
+		data->flags |= PMEM_FLAGS_MASTERMAP;
+		data->pid = current->pid;
+	}
+	vma->vm_ops = &vm_ops;
+error:
+	up_write(&data->sem);
+	return ret;
+}
+
+/* the following are the api for accessing pmem regions by other drivers
+ * from inside the kernel */
+int get_pmem_user_addr(struct file *file, unsigned long *start,
+		   unsigned long *len)
+{
+	struct pmem_data *data;
+	if (!is_pmem_file(file) || !has_allocation(file)) {
+#if PMEM_DEBUG
+		printk(KERN_INFO "pmem: requested pmem data from invalid"
+				  "file.\n");
+#endif
+		return -1;
+	}
+	data = (struct pmem_data *)file->private_data;
+	down_read(&data->sem);
+	if (data->vma) {
+		*start = data->vma->vm_start;
+		*len = data->vma->vm_end - data->vma->vm_start;
+	} else {
+		*start = 0;
+		*len = 0;
+	}
+	up_read(&data->sem);
+	return 0;
+}
+
+int get_pmem_addr(struct file *file, unsigned long *start,
+		  unsigned long *vstart, unsigned long *len)
+{
+	struct pmem_data *data;
+	int id;
+
+	if (!is_pmem_file(file) || !has_allocation(file)) {
+		return -1;
+	}
+
+	data = (struct pmem_data *)file->private_data;
+	if (data->index == -1) {
+#if PMEM_DEBUG
+		printk(KERN_INFO "pmem: requested pmem data from file with no "
+		       "allocation.\n");
+		return -1;
+#endif
+	}
+	id = get_id(file);
+
+	down_read(&data->sem);
+	*start = pmem_start_addr(id, data);
+	*len = pmem_len(id, data);
+	*vstart = (unsigned long)pmem_start_vaddr(id, data);
+	up_read(&data->sem);
+#if PMEM_DEBUG
+	down_write(&data->sem);
+	data->ref++;
+	up_write(&data->sem);
+#endif
+	return 0;
+}
+
+int get_pmem_file(int fd, unsigned long *start, unsigned long *vstart,
+		  unsigned long *len, struct file **filp)
+{
+	struct file *file;
+
+	file = fget(fd);
+	if (unlikely(file == NULL)) {
+		printk(KERN_INFO "pmem: requested data from file descriptor "
+		       "that doesn't exist.");
+		return -1;
+	}
+
+	if (get_pmem_addr(file, start, vstart, len))
+		goto end;
+
+	if (filp)
+		*filp = file;
+	return 0;
+end:
+	fput(file);
+	return -1;
+}
+
+void put_pmem_file(struct file *file)
+{
+	struct pmem_data *data;
+	int id;
+
+	if (!is_pmem_file(file))
+		return;
+	id = get_id(file);
+	data = (struct pmem_data *)file->private_data;
+#if PMEM_DEBUG
+	down_write(&data->sem);
+	if (data->ref == 0) {
+		printk("pmem: pmem_put > pmem_get %s (pid %d)\n",
+		       pmem[id].dev.name, data->pid);
+		BUG();
+	}
+	data->ref--;
+	up_write(&data->sem);
+#endif
+	fput(file);
+}
+
+void flush_pmem_file(struct file *file, unsigned long offset, unsigned long len)
+{
+	struct pmem_data *data;
+	int id;
+	void *vaddr;
+	struct pmem_region_node *region_node;
+	struct list_head *elt;
+	void *flush_start, *flush_end;
+
+	if (!is_pmem_file(file) || !has_allocation(file)) {
+		return;
+	}
+
+	id = get_id(file);
+	data = (struct pmem_data *)file->private_data;
+	if (!pmem[id].cached || file->f_flags & O_SYNC)
+		return;
+
+	down_read(&data->sem);
+	vaddr = pmem_start_vaddr(id, data);
+	/* if this isn't a submmapped file, flush the whole thing */
+	if (unlikely(!(data->flags & PMEM_FLAGS_CONNECTED))) {
+		if (pmem_len(id, data) >= SZ_1M) {
+			flush_all_cpu_caches();
+			outer_flush_all();
+		} else if (pmem_len(id, data) >= SZ_64K) {
+			flush_all_cpu_caches();
+			outer_flush_range(virt_to_phys(vaddr), virt_to_phys(vaddr + pmem_len(id, data)));
+		} else {
+			dmac_flush_range(vaddr, vaddr + pmem_len(id, data));
+			outer_flush_range(virt_to_phys(vaddr), virt_to_phys(vaddr + pmem_len(id, data)));
+		}
+		goto end;
+	}
+	/* otherwise, flush the region of the file we are drawing */
+	list_for_each(elt, &data->region_list) {
+		region_node = list_entry(elt, struct pmem_region_node, list);
+		if ((offset >= region_node->region.offset) &&
+		    ((offset + len) <= (region_node->region.offset +
+			region_node->region.len))) {
+			flush_start = vaddr + region_node->region.offset;
+			flush_end = flush_start + region_node->region.len;
+
+			if (pmem_len(id, data) >= SZ_1M) {
+				flush_all_cpu_caches();
+				outer_flush_all();
+			} else if (pmem_len(id, data) >= SZ_64K) {
+				flush_all_cpu_caches();
+				outer_flush_range(virt_to_phys(flush_start), virt_to_phys(flush_end));
+			} else {
+				dmac_flush_range(flush_start, flush_end);
+				outer_flush_range(virt_to_phys(flush_start), virt_to_phys(flush_end));
+			}
+			break;
+		}
+	}
+end:
+	up_read(&data->sem);
+}
+
+static int pmem_connect(unsigned long connect, struct file *file)
+{
+	struct pmem_data *data = (struct pmem_data *)file->private_data;
+	struct pmem_data *src_data;
+	struct file *src_file;
+	int ret = 0, put_needed;
+
+	down_write(&data->sem);
+	/* retrieve the src file and check it is a pmem file with an alloc */
+	src_file = fget_light(connect, &put_needed);
+	DLOG("connect %p to %p\n", file, src_file);
+	if (!src_file) {
+		printk("pmem: src file not found!\n");
+		ret = -EINVAL;
+		goto err_no_file;
+	}
+	if (unlikely(!is_pmem_file(src_file) || !has_allocation(src_file))) {
+		printk(KERN_INFO "pmem: src file is not a pmem file or has no "
+		       "alloc!\n");
+		ret = -EINVAL;
+		goto err_bad_file;
+	}
+	src_data = (struct pmem_data *)src_file->private_data;
+
+	if (has_allocation(file) && (data->index != src_data->index)) {
+		printk("pmem: file is already mapped but doesn't match this"
+		       " src_file!\n");
+		ret = -EINVAL;
+		goto err_bad_file;
+	}
+	data->index = src_data->index;
+	data->flags |= PMEM_FLAGS_CONNECTED;
+	data->master_fd = connect;
+	data->master_file = src_file;
+
+err_bad_file:
+	fput_light(src_file, put_needed);
+err_no_file:
+	up_write(&data->sem);
+	return ret;
+}
+
+static void pmem_unlock_data_and_mm(struct pmem_data *data,
+				    struct mm_struct *mm)
+{
+	up_write(&data->sem);
+	if (mm != NULL) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+}
+
+static int pmem_lock_data_and_mm(struct file *file, struct pmem_data *data,
+				 struct mm_struct **locked_mm)
+{
+	int ret = 0;
+	struct mm_struct *mm = NULL;
+	*locked_mm = NULL;
+lock_mm:
+	down_read(&data->sem);
+	if (PMEM_IS_SUBMAP(data)) {
+		mm = get_task_mm(data->task);
+		if (!mm) {
+#if PMEM_DEBUG
+			printk("pmem: can't remap task is gone!\n");
+#endif
+			up_read(&data->sem);
+			return -1;
+		}
+	}
+	up_read(&data->sem);
+
+	if (mm)
+		down_write(&mm->mmap_sem);
+
+	down_write(&data->sem);
+	/* check that the file didn't get mmaped before we could take the
+	 * data sem, this should be safe b/c you can only submap each file
+	 * once */
+	if (PMEM_IS_SUBMAP(data) && !mm) {
+		pmem_unlock_data_and_mm(data, mm);
+		goto lock_mm;
+	}
+	/* now check that vma.mm is still there, it could have been
+	 * deleted by vma_close before we could get the data->sem */
+	if ((data->flags & PMEM_FLAGS_UNSUBMAP) && (mm != NULL)) {
+		/* might as well release this */
+		if (data->flags & PMEM_FLAGS_SUBMAP) {
+			put_task_struct(data->task);
+			data->task = NULL;
+			/* lower the submap flag to show the mm is gone */
+			data->flags &= ~(PMEM_FLAGS_SUBMAP);
+		}
+		pmem_unlock_data_and_mm(data, mm);
+		return -1;
+	}
+	*locked_mm = mm;
+	return ret;
+}
+
+int pmem_remap(struct pmem_region *region, struct file *file,
+		      unsigned operation)
+{
+	int ret;
+	struct pmem_region_node *region_node;
+	struct mm_struct *mm = NULL;
+	struct list_head *elt, *elt2;
+	int id = get_id(file);
+	struct pmem_data *data = (struct pmem_data *)file->private_data;
+
+	/* pmem region must be aligned on a page boundry */
+	if (unlikely(!PMEM_IS_PAGE_ALIGNED(region->offset) ||
+		 !PMEM_IS_PAGE_ALIGNED(region->len))) {
+#if PMEM_DEBUG
+		printk("pmem: request for unaligned pmem suballocation "
+		       "%lx %lx\n", region->offset, region->len);
+#endif
+		return -EINVAL;
+	}
+
+	/* if userspace requests a region of len 0, there's nothing to do */
+	if (region->len == 0)
+		return 0;
+
+	/* lock the mm and data */
+	ret = pmem_lock_data_and_mm(file, data, &mm);
+	if (ret)
+		return 0;
+
+	/* only the owner of the master file can remap the client fds
+	 * that back in it */
+	if (!is_master_owner(file)) {
+#if PMEM_DEBUG
+		printk("pmem: remap requested from non-master process\n");
+#endif
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* check that the requested range is within the src allocation */
+	if (unlikely((region->offset > pmem_len(id, data)) ||
+		     (region->len > pmem_len(id, data)) ||
+		     (region->offset + region->len > pmem_len(id, data)))) {
+#if PMEM_DEBUG
+		printk(KERN_INFO "pmem: suballoc doesn't fit in src_file!\n");
+#endif
+		ret = -EINVAL;
+		goto err;
+	}
+
+	if (operation == PMEM_MAP) {
+		region_node = kmalloc(sizeof(struct pmem_region_node),
+			      GFP_KERNEL);
+		if (!region_node) {
+			ret = -ENOMEM;
+#if PMEM_DEBUG
+			printk(KERN_INFO "No space to allocate metadata!");
+#endif
+			goto err;
+		}
+		region_node->region = *region;
+		list_add(&region_node->list, &data->region_list);
+	} else if (operation == PMEM_UNMAP) {
+		int found = 0;
+		list_for_each_safe(elt, elt2, &data->region_list) {
+			region_node = list_entry(elt, struct pmem_region_node,
+				      list);
+			if (region->len == 0 ||
+			    (region_node->region.offset == region->offset &&
+			    region_node->region.len == region->len)) {
+				list_del(elt);
+				kfree(region_node);
+				found = 1;
+			}
+		}
+		if (!found) {
+#if PMEM_DEBUG
+			printk("pmem: Unmap region does not map any mapped "
+				"region!");
+#endif
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+
+	if (data->vma && PMEM_IS_SUBMAP(data)) {
+		if (operation == PMEM_MAP)
+			ret = pmem_remap_pfn_range(id, data->vma, data,
+						   region->offset, region->len);
+		else if (operation == PMEM_UNMAP)
+			ret = pmem_unmap_pfn_range(id, data->vma, data,
+						   region->offset, region->len);
+	}
+
+err:
+	pmem_unlock_data_and_mm(data, mm);
+	return ret;
+}
+
+static void pmem_revoke(struct file *file, struct pmem_data *data)
+{
+	struct pmem_region_node *region_node;
+	struct list_head *elt, *elt2;
+	struct mm_struct *mm = NULL;
+	int id = get_id(file);
+	int ret = 0;
+
+	data->master_file = NULL;
+	ret = pmem_lock_data_and_mm(file, data, &mm);
+	/* if lock_data_and_mm fails either the task that mapped the fd, or
+	 * the vma that mapped it have already gone away, nothing more
+	 * needs to be done */
+	if (ret)
+		return;
+	/* unmap everything */
+	/* delete the regions and region list nothing is mapped any more */
+	if (data->vma)
+		list_for_each_safe(elt, elt2, &data->region_list) {
+			region_node = list_entry(elt, struct pmem_region_node,
+						 list);
+			pmem_unmap_pfn_range(id, data->vma, data,
+					     region_node->region.offset,
+					     region_node->region.len);
+			list_del(elt);
+			kfree(region_node);
+	}
+	/* delete the master file */
+	pmem_unlock_data_and_mm(data, mm);
+}
+
+static void pmem_get_size(struct pmem_region *region, struct file *file)
+{
+	struct pmem_data *data = (struct pmem_data *)file->private_data;
+	int id = get_id(file);
+
+	if (!has_allocation(file)) {
+		region->offset = 0;
+		region->len = 0;
+		return;
+	} else {
+		region->offset = pmem_start_addr(id, data);
+		region->len = pmem_len(id, data);
+	}
+	DLOG("offset %lx len %lx\n", region->offset, region->len);
+}
+
+
+static long pmem_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct pmem_data *data;
+	int id = get_id(file);
+
+	switch (cmd) {
+	case PMEM_GET_PHYS:
+		{
+			struct pmem_region region;
+			DLOG("get_phys\n");
+			if (!has_allocation(file)) {
+				region.offset = 0;
+				region.len = 0;
+			} else {
+				data = (struct pmem_data *)file->private_data;
+				region.offset = pmem_start_addr(id, data);
+				region.len = pmem_len(id, data);
+			}
+			printk(KERN_DEBUG "pmem: request for physical address of pmem region "
+					"from process %d.\n", current->pid);
+			if (copy_to_user((void __user *)arg, &region,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			break;
+		}
+	case PMEM_MAP:
+		{
+			struct pmem_region region;
+			if (copy_from_user(&region, (void __user *)arg,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			data = (struct pmem_data *)file->private_data;
+			return pmem_remap(&region, file, PMEM_MAP);
+		}
+		break;
+	case PMEM_UNMAP:
+		{
+			struct pmem_region region;
+			if (copy_from_user(&region, (void __user *)arg,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			data = (struct pmem_data *)file->private_data;
+			return pmem_remap(&region, file, PMEM_UNMAP);
+			break;
+		}
+	case PMEM_GET_SIZE:
+		{
+			struct pmem_region region;
+			DLOG("get_size\n");
+			pmem_get_size(&region, file);
+			if (copy_to_user((void __user *)arg, &region,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			break;
+		}
+	case PMEM_GET_TOTAL_SIZE:
+		{
+			struct pmem_region region;
+			DLOG("get total size\n");
+			region.offset = 0;
+			get_id(file);
+			region.len = pmem[id].size;
+			if (copy_to_user((void __user *)arg, &region,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			break;
+		}
+	case PMEM_ALLOCATE:
+		{
+			if (has_allocation(file))
+				return -EINVAL;
+			data = (struct pmem_data *)file->private_data;
+			data->index = pmem_allocate(id, arg);
+			break;
+		}
+	case PMEM_CONNECT:
+		DLOG("connect\n");
+		return pmem_connect(arg, file);
+		break;
+	case PMEM_CACHE_FLUSH:
+		{
+			struct pmem_region region;
+			DLOG("flush\n");
+			if (copy_from_user(&region, (void __user *)arg,
+					   sizeof(struct pmem_region)))
+				return -EFAULT;
+			flush_pmem_file(file, region.offset, region.len);
+			break;
+		}
+	default:
+		if (pmem[id].ioctl)
+			return pmem[id].ioctl(file, cmd, arg);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+#if PMEM_DEBUG
+static ssize_t debug_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static ssize_t debug_read(struct file *file, char __user *buf, size_t count,
+			  loff_t *ppos)
+{
+	struct list_head *elt, *elt2;
+	struct pmem_data *data;
+	struct pmem_region_node *region_node;
+	int id = (int)file->private_data;
+	const int debug_bufmax = 4096;
+	static char buffer[4096];
+	int n = 0;
+
+	DLOG("debug open\n");
+	n = scnprintf(buffer, debug_bufmax,
+		      "pid #: mapped regions (offset, len) (offset,len)...\n");
+
+	mutex_lock(&pmem[id].data_list_lock);
+	list_for_each(elt, &pmem[id].data_list) {
+		data = list_entry(elt, struct pmem_data, list);
+		down_read(&data->sem);
+		n += scnprintf(buffer + n, debug_bufmax - n, "pid %u:",
+				data->pid);
+		list_for_each(elt2, &data->region_list) {
+			region_node = list_entry(elt2, struct pmem_region_node,
+				      list);
+			n += scnprintf(buffer + n, debug_bufmax - n,
+					"(%lx,%lx) ",
+					region_node->region.offset,
+					region_node->region.len);
+		}
+		n += scnprintf(buffer + n, debug_bufmax - n, "\n");
+		up_read(&data->sem);
+	}
+	mutex_unlock(&pmem[id].data_list_lock);
+
+	n++;
+	buffer[n] = 0;
+	return simple_read_from_buffer(buf, count, ppos, buffer, n);
+}
+
+static struct file_operations debug_fops = {
+	.read = debug_read,
+	.open = debug_open,
+};
+#endif
+
+#if 0
+static struct miscdevice pmem_dev = {
+	.name = "pmem",
+	.fops = &pmem_fops,
+};
+#endif
+
+int pmem_setup(struct android_pmem_platform_data *pdata,
+	       long (*ioctl)(struct file *, unsigned int, unsigned long),
+	       int (*release)(struct inode *, struct file *))
+{
+	int err = 0;
+	int i, index = 0;
+	int id = id_count;
+	struct page **pages;
+	int nr_pages;
+	int prot;
+	id_count++;
+
+#if defined(CONFIG_S5P_MEM_CMA)
+	pdata->start = cma_alloc_from(pdata->name, pdata->size, 0);
+#endif
+
+	pmem[id].no_allocator = pdata->no_allocator;
+	pmem[id].cached = pdata->cached;
+	pmem[id].buffered = pdata->buffered;
+	pmem[id].base = pdata->start;
+	pmem[id].size = pdata->size;
+	pmem[id].ioctl = ioctl;
+	pmem[id].release = release;
+	init_rwsem(&pmem[id].bitmap_sem);
+	mutex_init(&pmem[id].data_list_lock);
+	INIT_LIST_HEAD(&pmem[id].data_list);
+	pmem[id].dev.name = pdata->name;
+	pmem[id].dev.minor = id;
+	pmem[id].dev.fops = &pmem_fops;
+	printk(KERN_INFO "%s: %d init\n", pdata->name, pdata->cached);
+
+	err = misc_register(&pmem[id].dev);
+	if (err) {
+		printk(KERN_ALERT "Unable to register pmem driver!\n");
+		goto err_cant_register_device;
+	}
+	pmem[id].num_entries = pmem[id].size / PMEM_MIN_ALLOC;
+
+	pmem[id].bitmap = kmalloc(pmem[id].num_entries *
+				  sizeof(struct pmem_bits), GFP_KERNEL);
+	if (!pmem[id].bitmap)
+		goto err_no_mem_for_metadata;
+
+	memset(pmem[id].bitmap, 0, sizeof(struct pmem_bits) *
+					  pmem[id].num_entries);
+
+	for (i = sizeof(pmem[id].num_entries) * 8 - 1; i >= 0; i--) {
+		if ((pmem[id].num_entries) &  1<<i) {
+			PMEM_ORDER(id, index) = i;
+			index = PMEM_NEXT_INDEX(id, index);
+		}
+	}
+
+	nr_pages = pmem[id].size >> PAGE_SHIFT;
+	pages = kmalloc(nr_pages * sizeof(*pages), GFP_KERNEL);
+	if (!pages)
+		goto err_no_mem_for_pages;
+
+	for (i = 0; i < nr_pages; i++)
+		pages[i] = phys_to_page(pmem[id].base + i * PAGE_SIZE);
+
+	if (pmem[id].cached)
+		prot = PAGE_KERNEL;
+	else if (pmem[id].buffered)
+		prot = pgprot_writecombine(PAGE_KERNEL);
+	else
+		prot = pgprot_noncached(PAGE_KERNEL);
+
+	pmem[id].vbase = vmap(pages, nr_pages, VM_MAP, prot);
+
+	kfree(pages);
+
+	if (pmem[id].vbase == 0)
+		goto error_cant_remap;
+
+	pmem[id].garbage_pfn = page_to_pfn(alloc_page(GFP_KERNEL));
+	if (pmem[id].no_allocator)
+		pmem[id].allocated = 0;
+
+#if PMEM_DEBUG
+	debugfs_create_file(pdata->name, S_IFREG | S_IRUGO, NULL, (void *)id,
+			    &debug_fops);
+#endif
+	return 0;
+error_cant_remap:
+err_no_mem_for_pages:
+	kfree(pmem[id].bitmap);
+err_no_mem_for_metadata:
+	misc_deregister(&pmem[id].dev);
+err_cant_register_device:
+	return -1;
+}
+
+static int pmem_probe(struct platform_device *pdev)
+{
+	struct android_pmem_platform_data *pdata;
+
+	if (!pdev || !pdev->dev.platform_data) {
+		printk(KERN_ALERT "Unable to probe pmem!\n");
+		return -1;
+	}
+	pdata = pdev->dev.platform_data;
+	return pmem_setup(pdata, NULL, NULL);
+}
+
+
+static int pmem_remove(struct platform_device *pdev)
+{
+	int id = pdev->id;
+	__free_page(pfn_to_page(pmem[id].garbage_pfn));
+#if defined(CONFIG_S5P_MEM_CMA)
+	cma_free(((struct android_pmem_platform_data *)(pdev->dev.platform_data))->start);
+#endif
+	misc_deregister(&pmem[id].dev);
+	return 0;
+}
+
+static struct platform_driver pmem_driver = {
+	.probe = pmem_probe,
+	.remove = pmem_remove,
+	.driver = { .name = "android_pmem" }
+};
+
+
+static int __init pmem_init(void)
+{
+	return platform_driver_register(&pmem_driver);
+}
+
+static void __exit pmem_exit(void)
+{
+	platform_driver_unregister(&pmem_driver);
+}
+
+module_init(pmem_init);
+module_exit(pmem_exit);
+
diff --git a/drivers/misc/pn544.c b/drivers/misc/pn544.c
new file mode 100644
index 0000000..24a069e
--- /dev/null
+++ b/drivers/misc/pn544.c
@@ -0,0 +1,475 @@
+/*
+ * Copyright (C) 2010 Trusted Logic S.A.
+ *
+ * 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 St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/jiffies.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/miscdevice.h>
+#include <linux/spinlock.h>
+#include <linux/pn544.h>
+
+#define MAX_BUFFER_SIZE	512
+#define READ_IRQ_MODIFY /* DY_TEST */
+
+#ifdef READ_IRQ_MODIFY /* DY_TEST */
+bool do_reading;
+static bool cancle_read;
+#endif
+#define NFC_DEBUG 0
+
+struct pn544_dev {
+	wait_queue_head_t read_wq;
+	struct mutex read_mutex;
+	struct i2c_client *client;
+	struct miscdevice pn544_device;
+	unsigned int ven_gpio;
+	unsigned int firm_gpio;
+	unsigned int irq_gpio;
+	bool irq_enabled;
+	spinlock_t irq_enabled_lock;
+};
+
+bool do_reading;
+
+static struct pn544_dev *pn544_dev_imsi;
+
+static void pn544_disable_irq(struct pn544_dev *pn544_dev)
+{
+	/* unsigned long flags; */
+	/* spin_lock_irqsave(&pn544_dev->irq_enabled_lock, flags); */
+	if (pn544_dev->irq_enabled) {
+		pn544_dev->irq_enabled = false;
+		disable_irq_nosync(pn544_dev->client->irq);
+	}
+	/* spin_unlock_irqrestore(&pn544_dev->irq_enabled_lock, flags); */
+}
+
+static irqreturn_t pn544_dev_irq_handler(int irq, void *dev_id)
+{
+	struct pn544_dev *pn544_dev = dev_id;
+
+	if (!gpio_get_value_cansleep(pn544_dev->irq_gpio)) {
+		pr_info("pn544 : pn544_dev_irq_handler error\n");
+		return IRQ_HANDLED;
+	}
+	/* pn544_disable_irq(pn544_dev); */
+#ifdef READ_IRQ_MODIFY /* DY_TEST */
+	do_reading = 1;
+#endif
+	/* Wake up waiting readers */
+	wake_up(&pn544_dev->read_wq);
+
+#if NFC_DEBUG
+	pr_info("pn544 : call\n");
+#endif
+
+	return IRQ_HANDLED;
+}
+
+static ssize_t pn544_dev_read(struct file *filp, char __user *buf,
+		size_t count, loff_t *offset)
+{
+	struct pn544_dev *pn544_dev = filp->private_data;
+	char tmp[MAX_BUFFER_SIZE];
+	int ret;
+
+	if (count > MAX_BUFFER_SIZE)
+		count = MAX_BUFFER_SIZE;
+
+	pr_debug("%s : reading %zu bytes. irq=%s\n", __func__, count,
+		 gpio_get_value_cansleep(pn544_dev->irq_gpio) ? "1" : "0");
+
+#if NFC_DEBUG
+	pr_info("pn544 : + r\n");
+#endif
+
+	mutex_lock(&pn544_dev->read_mutex);
+
+ /* wait_irq: */
+	if (!gpio_get_value_cansleep(pn544_dev->irq_gpio)) {
+		if (filp->f_flags & O_NONBLOCK) {
+			pr_info("%s : O_NONBLOCK\n", __func__);
+			ret = -EAGAIN;
+			goto fail;
+		}
+
+		pn544_dev->irq_enabled = true;
+#ifdef READ_IRQ_MODIFY /* DY_TEST */
+		do_reading = 0;
+#endif
+		enable_irq(pn544_dev->client->irq);
+
+		if (gpio_get_value_cansleep(pn544_dev->irq_gpio)) {
+			pn544_disable_irq(pn544_dev);
+		} else {
+#ifdef READ_IRQ_MODIFY
+			ret = wait_event_interruptible(pn544_dev->read_wq,
+				do_reading);
+#else
+			ret = wait_event_interruptible(pn544_dev->read_wq,
+				gpio_get_value(pn544_dev->irq_gpio));
+#endif
+			/* gpio_get_value(pn544_dev->irq_gpio)); */
+			/* pr_info("pn544 : h\n"); */
+			pn544_disable_irq(pn544_dev);
+
+#if NFC_DEBUG
+			pr_info("pn544: h\n");
+#endif
+#ifdef READ_IRQ_MODIFY /* DY_TEST */
+			if (cancle_read == true) {
+				cancle_read = false;
+				ret = -1;
+				goto fail;
+			}
+#endif
+
+			if (ret)
+				goto fail;
+		}
+	}
+	/* Read data */
+	ret = i2c_master_recv(pn544_dev->client, tmp, count);
+	mutex_unlock(&pn544_dev->read_mutex);
+
+	if (ret < 0) {
+		pr_err("%s: i2c_master_recv returned %d\n", __func__, ret);
+		return ret;
+	}
+	if (ret > count) {
+		pr_err("%s: received too many bytes from i2c (%d)\n",
+			__func__, ret);
+		return -EIO;
+	}
+	if (copy_to_user(buf, tmp, ret)) {
+		pr_err("%s : failed to copy to user space\n", __func__);
+		return -EFAULT;
+	}
+
+	return ret;
+
+ fail:
+	mutex_unlock(&pn544_dev->read_mutex);
+	pr_err("%s : wait_event_interruptible fail\n", __func__);
+
+	return ret;
+}
+
+static ssize_t pn544_dev_write(struct file *filp, const char __user *buf,
+		size_t count, loff_t *offset)
+{
+	struct pn544_dev *pn544_dev;
+	char tmp[MAX_BUFFER_SIZE];
+	int ret;
+
+	pn544_dev = filp->private_data;
+
+#if NFC_DEBUG
+	pr_info("pn544 : + w\n");
+#endif
+
+	if (count > MAX_BUFFER_SIZE)
+		count = MAX_BUFFER_SIZE;
+
+	if (copy_from_user(tmp, buf, count)) {
+		pr_err("%s : failed to copy from user space\n", __func__);
+		return -EFAULT;
+	}
+
+	pr_debug("%s : writing %zu bytes.\n", __func__, count);
+	/* Write data */
+	ret = i2c_master_send(pn544_dev->client, tmp, count);
+
+#if NFC_DEBUG
+	pr_info("pn544 : - w\n");
+#endif
+
+	if (ret != count) {
+		pr_err("%s : i2c_master_send returned %d\n", __func__, ret);
+		ret = -EIO;
+	}
+
+	return ret;
+}
+
+static int pn544_dev_open(struct inode *inode, struct file *filp)
+{
+/*
+	struct pn544_dev *pn544_dev = container_of(filp->private_data,
+						struct pn544_dev,
+						pn544_device);
+*/
+	filp->private_data = pn544_dev_imsi;
+
+	pr_debug("%s : %d,%d\n", __func__, imajor(inode), iminor(inode));
+
+	return 0;
+}
+
+static long pn544_dev_ioctl(struct file *filp,
+		unsigned int cmd, unsigned long arg)
+{
+	struct pn544_dev *pn544_dev = filp->private_data;
+
+	switch (cmd) {
+	case PN544_SET_PWR:
+		if (arg == 2) {
+			/* power on with firmware download (requires hw reset)
+			 */
+			pr_info("%s power on with firmware\n", __func__);
+			gpio_set_value_cansleep(pn544_dev->ven_gpio, 1);
+			gpio_set_value(pn544_dev->firm_gpio, 1);
+			usleep_range(10000, 10000);
+			gpio_set_value_cansleep(pn544_dev->ven_gpio, 0);
+			usleep_range(10000, 10000);
+			gpio_set_value_cansleep(pn544_dev->ven_gpio, 1);
+			usleep_range(10000, 10000);
+		} else if (arg == 1) {
+			/* power on */
+			pr_info("%s power on\n", __func__);
+			gpio_set_value(pn544_dev->firm_gpio, 0);
+			gpio_set_value_cansleep(pn544_dev->ven_gpio, 1);
+			usleep_range(10000, 10000);
+		} else if (arg == 0) {
+			/* power off */
+			pr_info("%s power off\n", __func__);
+			/* printk("irq_gpio :%x ,firm_gpio %x\n",
+			   pn544_dev->irq_gpio, pn544_dev->firm_gpio); */
+			gpio_set_value(pn544_dev->firm_gpio, 0);
+			gpio_set_value_cansleep(pn544_dev->ven_gpio, 0);
+			usleep_range(10000, 10000);
+#ifdef READ_IRQ_MODIFY
+		} else if (arg == 3) { /* DY_TEST */
+			pr_info("%s Read Cancle\n", __func__);
+			cancle_read = true;
+			do_reading = 1;
+			wake_up(&pn544_dev->read_wq);
+#endif
+		} else {
+			/* pr_err("%s bad arg %l\n", __func__, arg); */
+			return -EINVAL;
+		}
+		break;
+	default:
+		pr_err("%s bad ioctl %u\n", __func__, cmd);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct file_operations pn544_dev_fops = {
+	.owner = THIS_MODULE,
+	.llseek = no_llseek,
+	.read = pn544_dev_read,
+	.write = pn544_dev_write,
+	.open = pn544_dev_open,
+	.unlocked_ioctl = pn544_dev_ioctl,
+};
+
+static int pn544_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	int ret;
+	int irq_num;
+	struct pn544_i2c_platform_data *platform_data;
+	struct pn544_dev *pn544_dev;
+
+	platform_data = client->dev.platform_data;
+
+	if (platform_data == NULL) {
+		pr_err("%s : nfc probe fail\n", __func__);
+		return -ENODEV;
+	}
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		pr_err("%s : need I2C_FUNC_I2C\n", __func__);
+		return -ENODEV;
+	}
+
+	ret = gpio_request(platform_data->irq_gpio, "nfc_int");
+	if (ret)
+		return -ENODEV;
+	ret = gpio_request(platform_data->ven_gpio, "nfc_ven");
+	if (ret)
+		goto err_ven;
+	ret = gpio_request(platform_data->firm_gpio, "nfc_firm");
+	if (ret)
+		goto err_firm;
+
+	pn544_dev = kzalloc(sizeof(*pn544_dev), GFP_KERNEL);
+	if (pn544_dev == NULL) {
+		dev_err(&client->dev,
+			"failed to allocate memory for module data\n");
+		ret = -ENOMEM;
+		goto err_exit;
+	}
+
+	pr_info("%s : IRQ num %d\n", __func__, client->irq);
+
+	pn544_dev->irq_gpio = platform_data->irq_gpio;
+	pn544_dev->ven_gpio = platform_data->ven_gpio;
+	pn544_dev->firm_gpio = platform_data->firm_gpio;
+	pn544_dev->client = client;
+
+	/* printk("irq_gpio :%x ,firm_gpio %x\n",pn544_dev->irq_gpio,
+		pn544_dev->firm_gpio); */
+
+	/* init mutex and queues */
+	init_waitqueue_head(&pn544_dev->read_wq);
+	mutex_init(&pn544_dev->read_mutex);
+	spin_lock_init(&pn544_dev->irq_enabled_lock);
+
+	pn544_dev->pn544_device.minor = MISC_DYNAMIC_MINOR;
+	pn544_dev->pn544_device.name = "pn544";
+	pn544_dev->pn544_device.fops = &pn544_dev_fops;
+
+	ret = misc_register(&pn544_dev->pn544_device);
+	if (ret) {
+		pr_err("%s : misc_register failed\n", __FILE__);
+		goto err_misc_register;
+	}
+
+	/* request irq.  the irq is set whenever the chip has data available
+	 * for reading.  it is cleared when all data has been read.
+	 */
+	pr_info("%s : requesting IRQ %d\n", __func__, client->irq);
+	pn544_dev->irq_enabled = true;
+	gpio_direction_input(pn544_dev->irq_gpio);
+	gpio_direction_output(pn544_dev->ven_gpio, 0);
+	gpio_direction_output(pn544_dev->firm_gpio, 0);
+	irq_num = gpio_to_irq(pn544_dev->irq_gpio);
+	client->irq = irq_num;
+
+	ret = request_threaded_irq(irq_num, NULL, pn544_dev_irq_handler,
+			IRQF_TRIGGER_RISING, "pn544", pn544_dev);
+	if (ret) {
+		dev_err(&client->dev, "request_irq failed\n");
+		goto err_request_irq_failed;
+	}
+	pn544_disable_irq(pn544_dev);
+#if defined(CONFIG_TARGET_LOCALE_EUR_U1_NFC)
+	enable_irq_wake(client->irq);
+#endif
+	i2c_set_clientdata(client, pn544_dev);
+	pn544_dev_imsi = pn544_dev;
+	/* printk("pn544 prove success\n"); */
+
+	return 0;
+
+ err_request_irq_failed:
+	misc_deregister(&pn544_dev->pn544_device);
+ err_misc_register:
+	mutex_destroy(&pn544_dev->read_mutex);
+	kfree(pn544_dev);
+ err_exit:
+	gpio_free(platform_data->firm_gpio);
+ err_firm:
+	gpio_free(platform_data->ven_gpio);
+ err_ven:
+	gpio_free(platform_data->irq_gpio);
+	return ret;
+}
+
+static int pn544_remove(struct i2c_client *client)
+{
+	struct pn544_dev *pn544_dev;
+
+	pn544_dev = i2c_get_clientdata(client);
+
+	free_irq(client->irq, pn544_dev);
+	misc_deregister(&pn544_dev->pn544_device);
+	mutex_destroy(&pn544_dev->read_mutex);
+	gpio_free(pn544_dev->irq_gpio);
+	gpio_free(pn544_dev->ven_gpio);
+	gpio_free(pn544_dev->firm_gpio);
+	kfree(pn544_dev);
+
+	return 0;
+}
+
+static void pn544_shutdown(struct i2c_client *client)
+{
+	struct pn544_dev *pn544_dev;
+
+	pn544_dev = i2c_get_clientdata(client);
+	pr_info("%s\n", __func__);
+	gpio_set_value_cansleep(pn544_dev->ven_gpio, 0);
+
+	free_irq(client->irq, pn544_dev);
+	misc_deregister(&pn544_dev->pn544_device);
+	mutex_destroy(&pn544_dev->read_mutex);
+	gpio_free(pn544_dev->irq_gpio);
+	gpio_free(pn544_dev->ven_gpio);
+	gpio_free(pn544_dev->firm_gpio);
+	kfree(pn544_dev);
+
+}
+
+static const struct i2c_device_id pn544_id[] = {
+	{"pn544", 0},
+	{}
+};
+
+static struct i2c_driver pn544_driver = {
+	.id_table = pn544_id,
+	.probe = pn544_probe,
+	.remove = pn544_remove,
+	.shutdown = pn544_shutdown,
+	.driver = {
+			.owner = THIS_MODULE,
+			.name = "pn544",
+	},
+};
+
+/*
+ * module load/unload record keeping
+ */
+
+static int __init pn544_dev_init(void)
+{
+	pr_info("Loading pn544 driver\n");
+	return i2c_add_driver(&pn544_driver);
+}
+
+module_init(pn544_dev_init);
+
+static void __exit pn544_dev_exit(void)
+{
+	pr_info("Unloading pn544 driver\n");
+	i2c_del_driver(&pn544_driver);
+}
+
+module_exit(pn544_dev_exit);
+
+MODULE_AUTHOR("Sylvain Fonteneau");
+MODULE_DESCRIPTION("NFC PN544 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/sec_jack.c b/drivers/misc/sec_jack.c
new file mode 100644
index 0000000..d2d4524
--- /dev/null
+++ b/drivers/misc/sec_jack.c
@@ -0,0 +1,788 @@
+/*  drivers/misc/sec_jack.c
+ *
+ *  Copyright (C) 2010 Samsung Electronics Co.Ltd
+ *
+ *  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.
+ */
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/types.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/switch.h>
+#include <linux/input.h>
+#include <linux/timer.h>
+#include <linux/wakelock.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/gpio_event.h>
+#include <linux/sec_jack.h>
+
+#include <plat/adc.h>
+
+#if defined(CONFIG_STMPE811_ADC)
+#define SEC_JACK_ADC_CH		4
+#else
+#define SEC_JACK_ADC_CH		3
+#endif
+#define SEC_JACK_SAMPLE_SIZE	5
+
+#define MAX_ZONE_LIMIT		10
+/* keep this value if you support double-pressed concept */
+#if defined(CONFIG_TARGET_LOCALE_KOR)
+#define SEND_KEY_CHECK_TIME_MS	20		/* 20ms - GB VOC in KOR*/
+#elif defined(CONFIG_MACH_Q1_BD)
+/* 27ms, total delay is approximately double more
+   because hrtimer is called twice by gpio input driver,
+   new sec spec total delay is 60ms +/-10ms */
+#define SEND_KEY_CHECK_TIME_MS	27
+#else
+#define SEND_KEY_CHECK_TIME_MS	40		/* 40ms */
+#endif
+#define WAKE_LOCK_TIME		(HZ * 5)	/* 5 sec */
+#define EAR_CHECK_LOOP_CNT	10
+
+#if defined(CONFIG_MACH_PX) || defined(CONFIG_MACH_P4NOTE)
+#define JACK_CLASS_NAME "audio"
+#define JACK_DEV_NAME "earjack"
+#else
+#define JACK_CLASS_NAME "jack"
+#define JACK_DEV_NAME "jack_selector"
+#endif
+
+static struct class *jack_class;
+static struct device *jack_dev;
+
+struct sec_jack_info {
+	struct s3c_adc_client *padc;
+	struct sec_jack_platform_data *pdata;
+	struct delayed_work jack_detect_work;
+	struct work_struct buttons_work;
+	struct workqueue_struct *queue;
+	struct input_dev *input_dev;
+	struct wake_lock det_wake_lock;
+	struct sec_jack_zone *zone;
+	struct input_handler handler;
+	struct input_handle handle;
+	struct input_device_id ids[2];
+	int det_irq;
+	int dev_id;
+	int pressed;
+	int pressed_code;
+	struct platform_device *send_key_dev;
+	unsigned int cur_jack_type;
+	int det_status;
+};
+
+/* with some modifications like moving all the gpio structs inside
+ * the platform data and getting the name for the switch and
+ * gpio_event from the platform data, the driver could support more than
+ * one headset jack, but currently user space is looking only for
+ * one key file and switch for a headset so it'd be overkill and
+ * untestable so we limit to one instantiation for now.
+ */
+static atomic_t instantiated = ATOMIC_INIT(0);
+
+/* sysfs name HeadsetObserver.java looks for to track headset state
+ */
+struct switch_dev switch_jack_detection = {
+	.name = "h2w",
+};
+
+/* To support AT+FCESTEST=1 */
+struct switch_dev switch_sendend = {
+		.name = "send_end",
+};
+
+static struct gpio_event_direct_entry sec_jack_key_map[] = {
+	{
+		.code	= KEY_UNKNOWN,
+	},
+};
+
+static struct gpio_event_input_info sec_jack_key_info = {
+	.info.func = gpio_event_input_func,
+	.info.no_suspend = true,
+	.type = EV_KEY,
+	.debounce_time.tv64 = SEND_KEY_CHECK_TIME_MS * NSEC_PER_MSEC,
+	.keymap = sec_jack_key_map,
+	.keymap_size = ARRAY_SIZE(sec_jack_key_map)
+};
+
+static struct gpio_event_info *sec_jack_input_info[] = {
+	&sec_jack_key_info.info,
+};
+
+static struct gpio_event_platform_data sec_jack_input_data = {
+	.name = "sec_jack",
+	.info = sec_jack_input_info,
+	.info_count = ARRAY_SIZE(sec_jack_input_info),
+};
+
+static int sec_jack_get_adc_data(struct s3c_adc_client *padc)
+{
+	int adc_data;
+	int adc_max = 0;
+	int adc_min = 0xFFFF;
+	int adc_total = 0;
+	int adc_retry_cnt = 0;
+	int i;
+
+	for (i = 0; i < SEC_JACK_SAMPLE_SIZE; i++) {
+
+	#if defined(CONFIG_STMPE811_ADC)
+		adc_data = stmpe811_get_adc_data(SEC_JACK_ADC_CH);
+	#else
+		adc_data = s3c_adc_read(padc, SEC_JACK_ADC_CH);
+	#endif
+
+		if (adc_data < 0) {
+
+			adc_retry_cnt++;
+
+			if (adc_retry_cnt > 10)
+				return adc_data;
+		}
+
+		if (i != 0) {
+			if (adc_data > adc_max)
+				adc_max = adc_data;
+			else if (adc_data < adc_min)
+				adc_min = adc_data;
+		} else {
+			adc_max = adc_data;
+			adc_min = adc_data;
+		}
+		adc_total += adc_data;
+	}
+
+	return (adc_total - adc_max - adc_min) / (SEC_JACK_SAMPLE_SIZE - 2);
+}
+
+/* gpio_input driver does not support to read adc value.
+ * We use input filter to support 3-buttons of headset
+ * without changing gpio_input driver.
+ */
+static bool sec_jack_buttons_filter(struct input_handle *handle,
+				    unsigned int type, unsigned int code,
+				    int value)
+{
+	struct sec_jack_info *hi = handle->handler->private;
+
+	if (hi->det_status == true)
+		return false;
+
+	if (type != EV_KEY || code != KEY_UNKNOWN)
+		return false;
+
+	hi->pressed = value;
+
+	/* This is called in timer handler of gpio_input driver.
+	 * We use workqueue to read adc value.
+	 */
+	queue_work(hi->queue, &hi->buttons_work);
+
+	return true;
+}
+
+static int sec_jack_buttons_connect(struct input_handler *handler,
+				    struct input_dev *dev,
+				    const struct input_device_id *id)
+{
+	struct sec_jack_info *hi;
+	struct sec_jack_platform_data *pdata;
+	struct sec_jack_buttons_zone *btn_zones;
+	int err;
+	int i;
+
+	/* bind input_handler to input device related to only sec_jack */
+	if (dev->name != sec_jack_input_data.name)
+		return -ENODEV;
+
+	hi = handler->private;
+	pdata = hi->pdata;
+	btn_zones = pdata->buttons_zones;
+
+	hi->input_dev = dev;
+	hi->handle.dev = dev;
+	hi->handle.handler = handler;
+	hi->handle.open = 0;
+	hi->handle.name = "sec_jack_buttons";
+
+	err = input_register_handle(&hi->handle);
+	if (err) {
+		pr_err("%s: Failed to register sec_jack buttons handle, "
+			"error %d\n", __func__, err);
+		goto err_register_handle;
+	}
+
+	err = input_open_device(&hi->handle);
+	if (err) {
+		pr_err("%s: Failed to open input device, error %d\n",
+			__func__, err);
+		goto err_open_device;
+	}
+
+	for (i = 0; i < pdata->num_buttons_zones; i++)
+		input_set_capability(dev, EV_KEY, btn_zones[i].code);
+
+	return 0;
+
+ err_open_device:
+	input_unregister_handle(&hi->handle);
+ err_register_handle:
+
+	return err;
+}
+
+static void sec_jack_buttons_disconnect(struct input_handle *handle)
+{
+	input_close_device(handle);
+	input_unregister_handle(handle);
+}
+
+static void sec_jack_set_type(struct sec_jack_info *hi, int jack_type)
+{
+	struct sec_jack_platform_data *pdata = hi->pdata;
+
+	/* this can happen during slow inserts where we think we identified
+	 * the type but then we get another interrupt and do it again
+	 */
+	if (jack_type == hi->cur_jack_type) {
+		if (jack_type != SEC_HEADSET_4POLE)
+			pdata->set_micbias_state(false);
+
+		return;
+	}
+
+	if (jack_type == SEC_HEADSET_4POLE) {
+		/* for a 4 pole headset, enable detection of send/end key */
+		if (hi->send_key_dev == NULL)
+			/* enable to get events again */
+			hi->send_key_dev = platform_device_register_data(NULL,
+					GPIO_EVENT_DEV_NAME,
+					hi->dev_id,
+					&sec_jack_input_data,
+					sizeof(sec_jack_input_data));
+	} else {
+		/* for all other jacks, disable send/end key detection */
+		if (hi->send_key_dev != NULL) {
+			/* disable to prevent false events on next insert */
+			platform_device_unregister(hi->send_key_dev);
+			hi->send_key_dev = NULL;
+		}
+		/* micbias is left enabled for 4pole and disabled otherwise */
+		pdata->set_micbias_state(false);
+	}
+	/* if user inserted ear jack slowly, different jack event can occur
+	 * sometimes because irq_thread is defined IRQ_ONESHOT, detach status
+	 * can be ignored sometimes so in that case, driver inform detach
+	 * event to user side
+	 */
+	switch_set_state(&switch_jack_detection, SEC_JACK_NO_DEVICE);
+
+	hi->cur_jack_type = jack_type;
+	pr_info("%s : jack_type = %d\n", __func__, jack_type);
+
+	switch_set_state(&switch_jack_detection, jack_type);
+}
+
+static void handle_jack_not_inserted(struct sec_jack_info *hi)
+{
+	sec_jack_set_type(hi, SEC_JACK_NO_DEVICE);
+	hi->pdata->set_micbias_state(false);
+}
+
+static void determine_jack_type(struct sec_jack_info *hi)
+{
+	struct sec_jack_platform_data *pdata = hi->pdata;
+	struct sec_jack_zone *zones = pdata->zones;
+	int size = pdata->num_zones;
+	int count[MAX_ZONE_LIMIT] = {0};
+	int adc;
+	int i;
+	unsigned npolarity = !pdata->det_active_high;
+
+	/* set mic bias to enable adc */
+	pdata->set_micbias_state(true);
+
+	while (gpio_get_value(pdata->det_gpio) ^ npolarity) {
+		adc = sec_jack_get_adc_data(hi->padc);
+
+		pr_debug("%s: adc = %d\n", __func__, adc);
+
+		if (adc < 0)
+			break;
+
+		/* determine the type of headset based on the
+		 * adc value.  An adc value can fall in various
+		 * ranges or zones.  Within some ranges, the type
+		 * can be returned immediately.  Within others, the
+		 * value is considered unstable and we need to sample
+		 * a few more types (up to the limit determined by
+		 * the range) before we return the type for that range.
+		 */
+		for (i = 0; i < size; i++) {
+			if (adc <= zones[i].adc_high) {
+				if (++count[i] > zones[i].check_count) {
+					sec_jack_set_type(hi,
+						 zones[i].jack_type);
+					return;
+				}
+				msleep(zones[i].delay_ms);
+				break;
+			}
+		}
+	}
+
+	/* jack removed before detection complete */
+	pr_debug("%s : jack removed before detection complete\n", __func__);
+	handle_jack_not_inserted(hi);
+}
+
+/* thread run whenever the headset detect state changes (either insertion
+ * or removal).
+ */
+static irqreturn_t sec_jack_detect_irq_thread(int irq, void *dev_id)
+{
+	struct sec_jack_info *hi = dev_id;
+	struct sec_jack_platform_data *pdata = hi->pdata;
+	unsigned npolarity = !pdata->det_active_high;
+	int curr_data;
+	int pre_data;
+	int loopcnt;
+	int check_loop_cnt = EAR_CHECK_LOOP_CNT;
+
+	hi->det_status = true;
+
+	/* prevent suspend to allow user space to respond to switch */
+	wake_lock_timeout(&hi->det_wake_lock, WAKE_LOCK_TIME);
+
+	/* debounce headset jack.  don't try to determine the type of
+	 * headset until the detect state is true for a while.
+	 */
+	pre_data = 0;
+	loopcnt = 0;
+	while (true) {
+		curr_data = gpio_get_value(pdata->det_gpio);
+		if (pre_data == curr_data)
+			loopcnt++;
+		else
+			loopcnt = 0;
+
+		pre_data = curr_data;
+
+		if (loopcnt >= check_loop_cnt) {
+			if (!curr_data ^ npolarity) {
+				/* jack not detected. */
+				handle_jack_not_inserted(hi);
+				hi->det_status = false;
+				return IRQ_HANDLED;
+			}
+			break;
+		}
+		msleep(20);
+	}
+
+	/* jack presence was detected the whole time, figure out which type */
+	determine_jack_type(hi);
+	hi->det_status = false;
+
+	return IRQ_HANDLED;
+}
+
+/* thread run whenever the button of headset is pressed or released */
+void sec_jack_buttons_work(struct work_struct *work)
+{
+	struct sec_jack_info *hi =
+		container_of(work, struct sec_jack_info, buttons_work);
+	struct sec_jack_platform_data *pdata = hi->pdata;
+	struct sec_jack_buttons_zone *btn_zones = pdata->buttons_zones;
+	int adc;
+	int i;
+
+	/* prevent suspend to allow user space to respond to switch */
+	wake_lock_timeout(&hi->det_wake_lock, WAKE_LOCK_TIME);
+
+	/* when button is released */
+	if (hi->pressed == 0) {
+		input_report_key(hi->input_dev, hi->pressed_code, 0);
+		switch_set_state(&switch_sendend, 0);
+		input_sync(hi->input_dev);
+		pr_info("%s: earkey is released\n", __func__);
+		pr_debug("keycode=%d\n", hi->pressed_code);
+		return;
+	}
+
+	/* when button is pressed */
+	adc = sec_jack_get_adc_data(hi->padc);
+
+	for (i = 0; i < pdata->num_buttons_zones; i++)
+		if (adc >= btn_zones[i].adc_low &&
+		    adc <= btn_zones[i].adc_high) {
+			hi->pressed_code = btn_zones[i].code;
+			input_report_key(hi->input_dev, btn_zones[i].code, 1);
+			switch_set_state(&switch_sendend, 1);
+			input_sync(hi->input_dev);
+			pr_info("%s: earkey is pressed (adc:%d)\n",
+					__func__, adc);
+			pr_debug("keycode=%d, is pressed\n", btn_zones[i].code);
+			return;
+		}
+
+	pr_warn("%s: key is skipped. ADC value is %d\n", __func__, adc);
+}
+
+static ssize_t select_jack_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	pr_info("%s : operate nothing\n", __func__);
+
+	return 0;
+}
+
+static ssize_t select_jack_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct sec_jack_info *hi = dev_get_drvdata(dev);
+	struct sec_jack_platform_data *pdata = hi->pdata;
+	int value = 0;
+
+
+	sscanf(buf, "%d", &value);
+	pr_err("%s: User  selection : 0X%x", __func__, value);
+	if (value == SEC_HEADSET_4POLE) {
+		pdata->set_micbias_state(true);
+		msleep(100);
+	}
+
+	sec_jack_set_type(hi, value);
+
+	return size;
+}
+
+#if defined(CONFIG_MACH_PX) || defined(CONFIG_MACH_P4NOTE)
+static ssize_t earjack_key_state_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct sec_jack_info *hi = dev_get_drvdata(dev);
+	int value = 0;
+
+	if (hi->pressed <= 0)
+		value = 0;
+	else
+		value = 1;
+
+	return sprintf(buf, "%d\n", value);
+}
+
+static ssize_t earjack_key_state_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t size)
+{
+	pr_info("%s : operate nothing\n", __func__);
+
+	return size;
+}
+
+static ssize_t earjack_state_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct sec_jack_info *hi = dev_get_drvdata(dev);
+	int value = 0;
+
+	if (hi->cur_jack_type == SEC_HEADSET_4POLE)
+		value = 1;
+	else
+		value = 0;
+
+	return sprintf(buf, "%d\n", value);
+}
+
+static ssize_t earjack_state_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t size)
+{
+	pr_info("%s : operate nothing\n", __func__);
+
+	return size;
+}
+static DEVICE_ATTR(key_state, S_IRUGO | S_IWUSR | S_IWGRP,
+		   earjack_key_state_show, earjack_key_state_store);
+
+static DEVICE_ATTR(state, S_IRUGO | S_IWUSR | S_IWGRP,
+		   earjack_state_show, earjack_state_store);
+#endif
+
+static DEVICE_ATTR(select_jack, S_IRUGO | S_IWUSR | S_IWGRP,
+		select_jack_show, select_jack_store);
+
+static int sec_jack_probe(struct platform_device *pdev)
+{
+	struct sec_jack_info *hi;
+	struct sec_jack_platform_data *pdata = pdev->dev.platform_data;
+	int ret;
+
+	pr_info("%s : Registering jack driver\n", __func__);
+	if (!pdata) {
+		pr_err("%s : pdata is NULL.\n", __func__);
+		return -ENODEV;
+	}
+
+	if (!pdata->zones
+		|| !pdata->set_micbias_state
+		|| pdata->num_zones > MAX_ZONE_LIMIT) {
+		pr_err("%s : need to check pdata\n", __func__);
+		return -ENODEV;
+	}
+
+	if (atomic_xchg(&instantiated, 1)) {
+		pr_err("%s : already instantiated, can only have one\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	sec_jack_key_map[0].gpio = pdata->send_end_gpio;
+
+	/* If no other keys in pdata, make all keys default to KEY_MEDIA */
+	if (pdata->num_buttons_zones == 0)
+		sec_jack_key_map[0].code = KEY_MEDIA;
+
+	hi = kzalloc(sizeof(struct sec_jack_info), GFP_KERNEL);
+	if (hi == NULL) {
+		pr_err("%s : Failed to allocate memory.\n", __func__);
+		ret = -ENOMEM;
+		goto err_kzalloc;
+	}
+
+	hi->pdata = pdata;
+
+	/* make the id of our gpio_event device the same as our platform device,
+	 * which makes it the responsiblity of the board file to make sure
+	 * it is unique relative to other gpio_event devices
+	 */
+	hi->dev_id = pdev->id;
+
+	ret = gpio_request(pdata->det_gpio, "ear_jack_detect");
+	if (ret) {
+		pr_err("%s : gpio_request failed for %d\n",
+		       __func__, pdata->det_gpio);
+		goto err_gpio_request;
+	}
+
+	ret = switch_dev_register(&switch_jack_detection);
+	if (ret < 0) {
+		pr_err("%s : Failed to register switch device\n", __func__);
+		goto err_switch_dev_register;
+	}
+
+	ret = switch_dev_register(&switch_sendend);
+	if (ret < 0) {
+		printk(KERN_ERR "SEC JACK: Failed to register switch device\n");
+		goto err_switch_dev_register_send_end;
+	}
+	wake_lock_init(&hi->det_wake_lock, WAKE_LOCK_SUSPEND, "sec_jack_det");
+
+	INIT_WORK(&hi->buttons_work, sec_jack_buttons_work);
+	hi->queue = create_singlethread_workqueue("sec_jack_wq");
+	if (hi->queue == NULL) {
+		ret = -ENOMEM;
+		pr_err("%s: Failed to create workqueue\n", __func__);
+		goto err_create_wq_failed;
+	}
+
+	hi->det_irq = gpio_to_irq(pdata->det_gpio);
+
+	jack_class = class_create(THIS_MODULE, JACK_CLASS_NAME);
+	if (IS_ERR(jack_class))
+		pr_err("Failed to create class(sec_jack)\n");
+
+	/* support PBA function test */
+
+	jack_dev = device_create(jack_class, NULL, 0, hi, JACK_DEV_NAME);
+	if (IS_ERR(jack_dev))
+		pr_err("Failed to create device(sec_jack)!= %ld\n",
+			IS_ERR(jack_dev));
+
+	if (device_create_file(jack_dev, &dev_attr_select_jack) < 0)
+		pr_err("Failed to create device file(%s)!\n",
+			dev_attr_select_jack.attr.name);
+#if defined(CONFIG_MACH_PX) || defined(CONFIG_MACH_P4NOTE)
+	if (device_create_file(jack_dev, &dev_attr_key_state) < 0)
+		pr_err("Failed to create device file (%s)!\n",
+			dev_attr_key_state.attr.name);
+
+	if (device_create_file(jack_dev, &dev_attr_state) < 0)
+		pr_err("Failed to create device file (%s)!\n",
+			dev_attr_state.attr.name);
+#endif
+	set_bit(EV_KEY, hi->ids[0].evbit);
+	hi->ids[0].flags = INPUT_DEVICE_ID_MATCH_EVBIT;
+	hi->handler.filter = sec_jack_buttons_filter;
+	hi->handler.connect = sec_jack_buttons_connect;
+	hi->handler.disconnect = sec_jack_buttons_disconnect;
+	hi->handler.name = "sec_jack_buttons";
+	hi->handler.id_table = hi->ids;
+	hi->handler.private = hi;
+
+	/* Register adc client */
+	hi->padc = s3c_adc_register(pdev, NULL, NULL, 0);
+
+	if (IS_ERR(hi->padc)) {
+		dev_err(&pdev->dev, "cannot register adc\n");
+		ret = PTR_ERR(hi->padc);
+		goto err_register_adc;
+	}
+
+	ret = input_register_handler(&hi->handler);
+	if (ret) {
+		pr_err("%s : Failed to register_handler\n", __func__);
+		goto err_register_input_handler;
+	}
+	ret = request_threaded_irq(hi->det_irq, NULL,
+				   sec_jack_detect_irq_thread,
+				   IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING |
+				   IRQF_ONESHOT, "sec_headset_detect", hi);
+	if (ret) {
+		pr_err("%s : Failed to request_irq.\n", __func__);
+		goto err_request_detect_irq;
+	}
+
+	/* to handle insert/removal when we're sleeping in a call */
+	ret = enable_irq_wake(hi->det_irq);
+	if (ret) {
+		pr_err("%s : Failed to enable_irq_wake.\n", __func__);
+		goto err_enable_irq_wake;
+	}
+
+	dev_set_drvdata(&pdev->dev, hi);
+
+	/* Prove current earjack state */
+	determine_jack_type(hi);
+
+
+	return 0;
+
+err_enable_irq_wake:
+	free_irq(hi->det_irq, hi);
+err_request_detect_irq:
+	input_unregister_handler(&hi->handler);
+err_register_input_handler:
+	s3c_adc_release(hi->padc);
+err_register_adc:
+	destroy_workqueue(hi->queue);
+err_create_wq_failed:
+	wake_lock_destroy(&hi->det_wake_lock);
+	switch_dev_unregister(&switch_sendend);
+err_switch_dev_register_send_end:
+	switch_dev_unregister(&switch_jack_detection);
+err_switch_dev_register:
+	gpio_free(pdata->det_gpio);
+err_gpio_request:
+	kfree(hi);
+err_kzalloc:
+	atomic_set(&instantiated, 0);
+
+	return ret;
+}
+
+static int sec_jack_remove(struct platform_device *pdev)
+{
+
+	struct sec_jack_info *hi = dev_get_drvdata(&pdev->dev);
+
+	pr_info("%s :\n", __func__);
+	disable_irq_wake(hi->det_irq);
+	free_irq(hi->det_irq, hi);
+	destroy_workqueue(hi->queue);
+	if (hi->send_key_dev) {
+		platform_device_unregister(hi->send_key_dev);
+		hi->send_key_dev = NULL;
+	}
+	input_unregister_handler(&hi->handler);
+	wake_lock_destroy(&hi->det_wake_lock);
+	switch_dev_unregister(&switch_sendend);
+	switch_dev_unregister(&switch_jack_detection);
+	gpio_free(hi->pdata->det_gpio);
+	s3c_adc_release(hi->padc);
+	kfree(hi);
+	atomic_set(&instantiated, 0);
+
+	return 0;
+}
+
+static int sec_jack_suspend(struct device *dev)
+{
+	struct sec_jack_info *hi = dev_get_drvdata(dev);
+	int ret;
+
+	ret = enable_irq_wake(hi->det_irq);
+
+	pr_info("%s: enable_irq_wake(%d)\n", __func__, ret);
+	disable_irq(hi->det_irq);
+
+	return 0;
+}
+
+static int sec_jack_resume(struct device *dev)
+{
+	struct sec_jack_info *hi = dev_get_drvdata(dev);
+	int ret;
+
+	ret = disable_irq_wake(hi->det_irq);
+
+	pr_info("%s: disable_irq_wake(%d)\n", __func__, ret);
+	enable_irq(hi->det_irq);
+
+	return 0;
+}
+
+static const struct dev_pm_ops sec_jack_dev_pm_ops = {
+	.suspend	= sec_jack_suspend,
+	.resume		= sec_jack_resume,
+};
+
+static struct platform_driver sec_jack_driver = {
+	.probe	= sec_jack_probe,
+	.remove	= sec_jack_remove,
+	.driver	= {
+		.name = "sec_jack",
+		.owner = THIS_MODULE,
+		.pm	= &sec_jack_dev_pm_ops,
+	},
+};
+
+static int __init sec_jack_init(void)
+{
+	int ret;
+
+	ret =  platform_driver_register(&sec_jack_driver);
+
+	if (ret)
+		pr_err("%s: Failed to add sec jack driver\n", __func__);
+
+	return ret;
+}
+
+static void __exit sec_jack_exit(void)
+{
+	platform_driver_unregister(&sec_jack_driver);
+}
+
+module_init(sec_jack_init);
+module_exit(sec_jack_exit);
+
+MODULE_AUTHOR("ms17.kim@samsung.com");
+MODULE_DESCRIPTION("Samsung Electronics Corp Ear-Jack detection driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/stmpe811-adc.c b/drivers/misc/stmpe811-adc.c
new file mode 100644
index 0000000..9a02186
--- /dev/null
+++ b/drivers/misc/stmpe811-adc.c
@@ -0,0 +1,508 @@
+/*
+ * stmpe811-adc.c
+ *
+ * Copyright (C) 2011 Samsung Electronics
+ * SangYoung Son <hello.son@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/fs.h>
+#include <linux/gpio.h>
+#include <mach/midas-thermistor.h>
+#include <linux/stmpe811-adc.h>
+#include <plat/gpio-cfg.h>
+
+#define STMPE811_CHIP_ID	0x00
+#define STMPE811_ID_VER		0x02
+#define STMPE811_SYS_CTRL1	0x03
+#define STMPE811_SYS_CTRL2	0x04
+#define STMPE811_INT_CTRL	0x09
+#define STMPE811_INT_EN		0x0A
+#define STMPE811_INT_STA	0x0B
+#define STMPE811_ADC_INT_EN	0x0E
+#define STMPE811_ADC_INT_STA	0x0F
+#define STMPE811_ADC_CTRL1	0x20
+#define STMPE811_ADC_CTRL2	0x21
+#define STMPE811_ADC_CAPT	0x22
+#define STMPE811_ADC_DATA_CH0	0x30
+#define STMPE811_ADC_DATA_CH1	0x32
+#define STMPE811_ADC_DATA_CH2	0x34
+#define STMPE811_ADC_DATA_CH3	0x36
+#define STMPE811_ADC_DATA_CH4	0x38
+#define STMPE811_ADC_DATA_CH5	0x3A
+#define STMPE811_ADC_DATA_CH6	0x3C
+#define STMPE811_ADC_DATA_CH7	0x3E
+#define STMPE811_GPIO_AF	0x17
+#define STMPE811_TSC_CTRL	0x40
+
+static struct device *adc_dev;
+static struct i2c_client *stmpe811_adc_i2c_client;
+
+struct stmpe811_adc_data {
+	struct i2c_client	*client;
+	struct stmpe811_platform_data	*pdata;
+
+	struct mutex		adc_lock;
+};
+
+static int stmpe811_i2c_read(struct i2c_client *client,
+							u8 reg,
+							u8 *data,
+							u8 length)
+{
+	int ret;
+
+	ret = i2c_smbus_read_i2c_block_data(client, (u8)reg, length, data);
+	if (ret < 0) {
+		pr_err("%s: err %d, reg: 0x%02x\n", __func__, ret, reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+int stmpe811_write_register(struct i2c_client *client,
+							u8 reg,
+							u16 w_data)
+{
+	int ret;
+
+	ret = i2c_smbus_write_word_data(client, (u8)reg, w_data);
+	if (ret < 0) {
+		pr_err("%s: err %d, reg: 0x%02x\n", __func__, ret, reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+u16 stmpe811_get_adc_data(u8 ch)
+{
+	struct i2c_client *client = stmpe811_adc_i2c_client;
+	u8 data[2];
+	u16 w_data;
+	int prog, retry_cnt;
+	int data_channel_addr;
+
+	stmpe811_write_register(client, STMPE811_ADC_CAPT, (1 << ch));
+
+	prog = 0;
+	retry_cnt = 10;
+	do {
+		stmpe811_i2c_read(client, STMPE811_ADC_CAPT, data, (u8)1);
+		pr_debug("%s: ADC_CAPT(0x%x)\n", __func__, data[0]);
+
+		prog = (data[0] & (1 << ch));
+
+		if (prog) {
+			pr_debug("%s: ch%d conversion completed\n",
+							__func__, ch);
+			break;
+		} else {
+			pr_info("%s: ch%d conversion progressing\n",
+							__func__, ch);
+			msleep(20);
+		}
+	} while ((!prog) && (--retry_cnt > 0));
+
+	if (retry_cnt == 0) {
+		pr_err("%s: ch%d conversion fail\n", __func__, ch);
+		return -EBUSY;
+	}
+
+	data_channel_addr = STMPE811_ADC_DATA_CH0 + (ch * 2);
+	stmpe811_i2c_read(client, data_channel_addr, data, (u8)2);
+	w_data = ((data[0] << 8) | data[1]) & 0x0FFF;
+	pr_debug("%s: ADC_DATA_CH%d(0x%x, %d)\n", __func__, ch, w_data, w_data);
+
+	return w_data;
+}
+EXPORT_SYMBOL(stmpe811_get_adc_data);
+
+int stmpe811_get_adc_value(u8 channel)
+{
+	struct stmpe811_platform_data *pdata =
+		stmpe811_adc_i2c_client->dev.platform_data;
+	int adc_data;
+	int adc_value;
+	int low, mid, high;
+	struct adc_table_data *temper_table = NULL;
+	pr_debug("%s\n", __func__);
+
+	adc_data = stmpe811_get_adc_data(channel);
+
+	low = mid = high = 0;
+	switch (channel) {
+	case 0:
+		if ((!pdata->adc_table_ch0) || (!pdata->table_size_ch0))
+			goto table_err;
+		temper_table = pdata->adc_table_ch0;
+		high = pdata->table_size_ch0 - 1;
+		break;
+	case 1:
+		if ((!pdata->adc_table_ch1) || (!pdata->table_size_ch1))
+			goto table_err;
+		temper_table = pdata->adc_table_ch1;
+		high = pdata->table_size_ch1 - 1;
+		break;
+	case 2:
+		if ((!pdata->adc_table_ch2) || (!pdata->table_size_ch2))
+			goto table_err;
+		temper_table = pdata->adc_table_ch2;
+		high = pdata->table_size_ch2 - 1;
+		break;
+	case 3:
+		if ((!pdata->adc_table_ch3) || (!pdata->table_size_ch3))
+			goto table_err;
+		temper_table = pdata->adc_table_ch3;
+		high = pdata->table_size_ch3 - 1;
+		break;
+	case 4:
+		if ((!pdata->adc_table_ch4) || (!pdata->table_size_ch4))
+			goto table_err;
+		temper_table = pdata->adc_table_ch4;
+		high = pdata->table_size_ch4 - 1;
+		break;
+	case 5:
+		if ((!pdata->adc_table_ch5) || (!pdata->table_size_ch5))
+			goto table_err;
+		temper_table = pdata->adc_table_ch5;
+		high = pdata->table_size_ch5 - 1;
+		break;
+	case 6:
+		if ((!pdata->adc_table_ch6) || (!pdata->table_size_ch6))
+			goto table_err;
+		temper_table = pdata->adc_table_ch6;
+		high = pdata->table_size_ch6 - 1;
+		break;
+	case 7:
+		if ((!pdata->adc_table_ch7) || (!pdata->table_size_ch7))
+			goto table_err;
+		temper_table = pdata->adc_table_ch7;
+		high = pdata->table_size_ch7 - 1;
+		break;
+	default:
+		pr_info("%s: not exist temper table for ch(%d)\n", __func__,
+							channel);
+		return -EINVAL;
+		break;
+	}
+
+	/* Out of table range */
+	if (adc_data >= temper_table[low].adc) {
+		adc_value = temper_table[low].value * 10;
+		return adc_value;
+	} else if (adc_data <= temper_table[high].adc) {
+		adc_value = temper_table[high].value * 10;
+		return adc_value;
+	}
+
+	while (low <= high) {
+		mid = (low + high) / 2;
+		if (temper_table[mid].adc > adc_data)
+			low = mid + 1;
+		else if (temper_table[mid].adc < adc_data)
+			high = mid - 1;
+		else
+			break;
+	}
+	adc_value = temper_table[mid].value * 10;
+
+	pr_debug("%s: adc data(%d), adc value(%d)\n", __func__,
+					adc_data, adc_value);
+	return adc_value;
+
+table_err:
+	pr_err("%s: table for ch%d does not exist\n", __func__, channel);
+	return -EINVAL;
+}
+EXPORT_SYMBOL(stmpe811_get_adc_value);
+
+static ssize_t adc_data_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	return sprintf(buf, "%s\n", "adc_test_show");
+}
+
+static ssize_t adc_data_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf,
+				size_t count)
+{
+	int mode;
+	s16 val;
+	sscanf(buf, "%d", &mode);
+
+	if (mode < 0 || mode > 7) {
+		pr_err("invalid channel: %d", mode);
+		return -EINVAL;
+	}
+
+	val = stmpe811_get_adc_data((u8)mode);
+	pr_info("adc data from ch%d: %d\n", mode, val);
+
+	return count;
+}
+static DEVICE_ATTR(adc_data, S_IRUGO | S_IWUSR | S_IWGRP,
+		adc_data_show, adc_data_store);
+
+static ssize_t adc_value_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	return sprintf(buf, "%s\n", "adc_test_show");
+}
+
+static ssize_t adc_value_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf,
+				size_t count)
+{
+	int mode;
+	s16 val;
+	sscanf(buf, "%d", &mode);
+
+	if (mode < 0 || mode > 7) {
+		pr_err("invalid channel: %d", mode);
+		return -EINVAL;
+	}
+
+	val = stmpe811_get_adc_value((u8)mode);
+	pr_info("adc value from ch%d: %d\n", mode, val);
+
+	return count;
+}
+static DEVICE_ATTR(adc_value, S_IRUGO | S_IWUSR | S_IWGRP,
+		adc_value_show, adc_value_store);
+
+static int stmpe811_reg_init(struct stmpe811_adc_data *adc_data)
+{
+	struct i2c_client *client = adc_data->client;
+	int ret;
+	u8 data[2];
+	u16 w_data;
+	pr_debug("%s\n", __func__);
+
+	/* read device identification */
+	ret = stmpe811_i2c_read(client, STMPE811_CHIP_ID, data, (u8)2);
+	pr_info("%s: ret: %d\n", __func__, ret);
+	if (ret < 0) {
+		pr_err("%s: reg init error: %d\n", __func__, ret);
+		goto reg_init_error;
+	}
+
+	w_data = ((data[0]<<8) | data[1]) & 0x0FFF;
+	pr_info("%s: STMPE811_CHIP_ID(0x%x)\n", __func__, w_data);
+
+	/* read revision number, 0x01 for es, 0x03 for final silicon */
+	stmpe811_i2c_read(client, STMPE811_ID_VER, data, (u8)1);
+	pr_info("%s: STMPE811_ID_VER(0x%x)\n", __func__, data[0]);
+
+	/* clock control: only adc on */
+	w_data = 0x0E;
+	stmpe811_write_register(client, STMPE811_SYS_CTRL2, w_data);
+	stmpe811_i2c_read(client, STMPE811_SYS_CTRL2, data, (u8)1);
+	pr_info("%s: STMPE811_SYS_CTRL2(0x%x)\n", __func__, data[0]);
+
+	/* interrupt enable: disable interrupt */
+	w_data = 0x00;
+	stmpe811_write_register(client, STMPE811_INT_EN, w_data);
+	stmpe811_i2c_read(client, STMPE811_INT_EN, data, (u8)1);
+	pr_info("%s: STMPE811_INT_EN(0x%x)\n", __func__, data[0]);
+
+	/* adc control: 64 sample time, 12bit adc, internel referance*/
+	w_data = 0x38;
+	stmpe811_write_register(client, STMPE811_ADC_CTRL1, w_data);
+	stmpe811_i2c_read(client, STMPE811_ADC_CTRL1, data, (u8)1);
+	pr_info("%s: STMPE811_ADC_CTRL1(0x%x)\n", __func__, data[0]);
+
+	/* adc control: 1.625MHz typ */
+	w_data = 0x03;
+	stmpe811_write_register(client, STMPE811_ADC_CTRL2, w_data);
+	stmpe811_i2c_read(client, STMPE811_ADC_CTRL2, data, (u8)1);
+	pr_info("%s: STMPE811_ADC_CTRL2(0x%x)\n", __func__, data[0]);
+
+	/* alt func: use for adc */
+	w_data = 0x00;
+	stmpe811_write_register(client, STMPE811_GPIO_AF, w_data);
+	stmpe811_i2c_read(client, STMPE811_GPIO_AF, data, (u8)1);
+	pr_info("%s: STMPE811_GPIO_AF(0x%x)\n", __func__, data[0]);
+
+	/* ts control: tsc disable */
+	w_data = 0x00;
+	stmpe811_write_register(client, STMPE811_TSC_CTRL, w_data);
+	stmpe811_i2c_read(client, STMPE811_TSC_CTRL, data, (u8)1);
+	pr_info("%s: STMPE811_TSC_CTRL(0x%x)\n", __func__, data[0]);
+
+reg_init_error:
+	return ret;
+}
+
+static const struct file_operations stmpe811_fops = {
+	.owner = THIS_MODULE,
+};
+
+static struct miscdevice stmpe811_adc_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "sec_adc",
+	.fops = &stmpe811_fops,
+};
+
+static int stmpe811_adc_i2c_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct stmpe811_adc_data *adc_data;
+	int ret;
+	u8 i2c_data[2];
+	pr_info("%s: stmpe811 adc driver loading\n", __func__);
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE))
+		return -EIO;
+
+	adc_data = kzalloc(sizeof(struct stmpe811_adc_data), GFP_KERNEL);
+	if (!adc_data)
+		return -ENOMEM;
+
+	adc_data->client = client;
+	adc_data->pdata = client->dev.platform_data;
+
+	i2c_set_clientdata(client, adc_data);
+
+	stmpe811_adc_i2c_client = client;
+
+	ret = misc_register(&stmpe811_adc_device);
+	if (ret)
+		goto misc_register_fail;
+
+	mutex_init(&adc_data->adc_lock);
+
+	/* initialize adc registers */
+	ret = stmpe811_reg_init(adc_data);
+	if (ret < 0)
+		goto reg_init_error;
+
+	/* TODO: ADC_INT setting */
+
+	/* create sysfs for debugging and factory mode*/
+	adc_dev = device_create(sec_class, NULL, 0, NULL, "adc");
+	if (IS_ERR(adc_dev)) {
+		ret = -ENOMEM;
+		goto deregister_misc;
+	}
+
+	ret = device_create_file(adc_dev, &dev_attr_adc_data);
+	if (ret < 0)
+		goto destroy_device;
+
+	ret = device_create_file(adc_dev, &dev_attr_adc_value);
+	if (ret < 0)
+		goto dev_attr_adc_value_err;
+
+	stmpe811_i2c_read(client, STMPE811_CHIP_ID, i2c_data, (u8)2);
+	pr_info("stmpe811 adc(id 0x%x) initialized\n",
+				((i2c_data[0]<<8) | i2c_data[1]));
+
+	return 0;
+
+dev_attr_adc_value_err:
+	device_remove_file(adc_dev, &dev_attr_adc_data);
+destroy_device:
+	device_destroy(sec_class, 0);
+deregister_misc:
+reg_init_error:
+	misc_deregister(&stmpe811_adc_device);
+misc_register_fail:
+	pr_info("stmpe811 probe fail: %d\n", ret);
+	kfree(adc_data);
+	return ret;
+}
+
+static int stmpe811_adc_i2c_remove(struct i2c_client *client)
+{
+	struct stmpe811_adc_data *adc = i2c_get_clientdata(client);
+	pr_info("%s\n", __func__);
+
+	misc_deregister(&stmpe811_adc_device);
+
+	device_remove_file(adc_dev, &dev_attr_adc_value);
+	device_remove_file(adc_dev, &dev_attr_adc_data);
+	device_destroy(sec_class, 0);
+
+	mutex_destroy(&adc->adc_lock);
+	kfree(adc);
+
+	return 0;
+}
+
+static int stmpe811_adc_suspend(struct device *dev)
+{
+
+	return 0;
+}
+
+static int stmpe811_adc_resume(struct device *dev)
+{
+	return 0;
+}
+
+static const struct i2c_device_id stmpe811_adc_device_id[] = {
+	{"stmpe811-adc", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, stmpe811_adc_device_id);
+
+static const struct dev_pm_ops stmpe811_adc_pm_ops = {
+	.suspend = stmpe811_adc_suspend,
+	.resume = stmpe811_adc_resume,
+};
+
+static struct i2c_driver stmpe811_adc_i2c_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "stmpe811-adc",
+		.pm = &stmpe811_adc_pm_ops,
+	},
+	.probe		= stmpe811_adc_i2c_probe,
+	.remove		= stmpe811_adc_i2c_remove,
+	.id_table	= stmpe811_adc_device_id,
+};
+
+static int __init stmpe811_adc_init(void)
+{
+	return i2c_add_driver(&stmpe811_adc_i2c_driver);
+}
+
+static void __exit stmpe811_adc_exit(void)
+{
+	i2c_del_driver(&stmpe811_adc_i2c_driver);
+}
+
+module_init(stmpe811_adc_init);
+module_exit(stmpe811_adc_exit);
+
+MODULE_AUTHOR("SangYoung Son <hello.son@samsung.com>");
+MODULE_DESCRIPTION("stmpe811 adc driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/tzic.c b/drivers/misc/tzic.c
new file mode 100644
index 0000000..966de5a
--- /dev/null
+++ b/drivers/misc/tzic.c
@@ -0,0 +1,185 @@
+/*
+ * Samsung TZIC Driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+
+#define KMSG_COMPONENT "TZIC"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/platform_device.h>
+#include <linux/debugfs.h>
+#include <linux/cdev.h>
+#include <linux/uaccess.h>
+#include <linux/sched.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/android_pmem.h>
+#include <linux/io.h>
+#include <linux/types.h>
+#include <asm/smc.h>
+
+#define TZIC_DEV "tzic"
+#define SMC_CMD_STORE_BINFO	 (-201)
+
+static int gotoCpu0(void);
+static int gotoAllCpu(void) __attribute__ ((unused));
+
+u32 exynos_smc1(u32 cmd, u32 arg1, u32 arg2, u32 arg3)
+{
+	register u32 reg0 __asm__("r0") = cmd;
+	register u32 reg1 __asm__("r1") = arg1;
+	register u32 reg2 __asm__("r2") = arg2;
+	register u32 reg3 __asm__("r3") = arg3;
+
+	__asm__ volatile (
+		"smc	0\n"
+		: "+r"(reg0), "+r"(reg1), "+r"(reg2), "+r"(reg3)
+	);
+
+	return reg0;
+}
+
+static DEFINE_MUTEX(tzic_mutex);
+static struct class *driver_class;
+static dev_t tzic_device_no;
+static struct cdev tzic_cdev;
+
+#define LOG printk
+
+static long tzic_ioctl(struct file *file, unsigned cmd,
+		unsigned long arg)
+{
+	int ret = 0;
+
+	ret = gotoCpu0();
+	if (0 != ret) {
+		LOG(KERN_INFO "changing core failed!");
+		return -1;
+	}
+
+	LOG(KERN_INFO "set_fuse");
+	exynos_smc1(SMC_CMD_STORE_BINFO, 0x80010001, 0, 0);
+	exynos_smc1(SMC_CMD_STORE_BINFO, 0x00000001, 0, 0);
+
+	gotoAllCpu();
+
+	return 0;
+}
+
+static const struct file_operations tzic_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = tzic_ioctl,
+};
+
+static int __init tzic_init(void)
+{
+	int rc;
+	struct device *class_dev;
+
+	rc = alloc_chrdev_region(&tzic_device_no, 0, 1, TZIC_DEV);
+	if (rc < 0) {
+		LOG(KERN_INFO "alloc_chrdev_region failed %d", rc);
+		return rc;
+	}
+
+	driver_class = class_create(THIS_MODULE, TZIC_DEV);
+	if (IS_ERR(driver_class)) {
+		rc = -ENOMEM;
+		LOG(KERN_INFO "class_create failed %d", rc);
+		goto unregister_chrdev_region;
+	}
+
+	class_dev = device_create(driver_class, NULL, tzic_device_no, NULL,
+			TZIC_DEV);
+	if (!class_dev) {
+		LOG(KERN_INFO "class_device_create failed %d", rc);
+		rc = -ENOMEM;
+		goto class_destroy;
+	}
+
+	cdev_init(&tzic_cdev, &tzic_fops);
+	tzic_cdev.owner = THIS_MODULE;
+
+	rc = cdev_add(&tzic_cdev, MKDEV(MAJOR(tzic_device_no), 0), 1);
+	if (rc < 0) {
+		LOG(KERN_INFO "cdev_add failed %d", rc);
+		goto class_device_destroy;
+	}
+
+	return 0;
+
+class_device_destroy:
+	device_destroy(driver_class, tzic_device_no);
+class_destroy:
+	class_destroy(driver_class);
+unregister_chrdev_region:
+	unregister_chrdev_region(tzic_device_no, 1);
+	return rc;
+}
+
+static void __exit tzic_exit(void)
+{
+	device_destroy(driver_class, tzic_device_no);
+	class_destroy(driver_class);
+	unregister_chrdev_region(tzic_device_no, 1);
+}
+
+static int gotoCpu0(void)
+{
+	int		ret = 0;
+	struct cpumask	mask =  CPU_MASK_CPU0;
+
+	LOG(KERN_INFO "System has %d CPU's, we are on CPU #%d\n"
+		  "\tBinding this process to CPU #0.\n"
+		  "\tactive mask is %lx, setting it to mask=%lx\n",
+		  nr_cpu_ids,
+		  raw_smp_processor_id(),
+		  cpu_active_mask->bits[0],
+		  mask.bits[0]);
+	ret = set_cpus_allowed_ptr(current, &mask);
+	if (0 != ret)
+		LOG(KERN_INFO "set_cpus_allowed_ptr=%d.\n", ret);
+	LOG(KERN_INFO "And now we are on CPU #%d",
+		  raw_smp_processor_id());
+
+	return ret;
+}
+
+static int gotoAllCpu(void)
+{
+	int		ret = 0;
+	struct cpumask	mask =  CPU_MASK_ALL;
+
+	LOG(KERN_INFO "System has %d CPU's, we are on CPU #%d\n"
+		  "\tBinding this process to CPU #0.\n"
+		  "\tactive mask is %lx, setting it to mask=%lx\n",
+		  nr_cpu_ids,
+		  raw_smp_processor_id(),
+		  cpu_active_mask->bits[0],
+		  mask.bits[0]);
+	ret = set_cpus_allowed_ptr(current, &mask);
+	if (0 != ret)
+		LOG(KERN_INFO "set_cpus_allowed_ptr=%d.\n", ret);
+	LOG(KERN_INFO "And now we are on CPU #%d",
+		  raw_smp_processor_id());
+
+	return ret;
+}
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Samsung TZIC Driver");
+MODULE_VERSION("1.00");
+
+module_init(tzic_init);
+module_exit(tzic_exit);
+
diff --git a/drivers/misc/uart_select.c b/drivers/misc/uart_select.c
new file mode 100644
index 0000000..22d1439
--- /dev/null
+++ b/drivers/misc/uart_select.c
@@ -0,0 +1,156 @@
+/*
+ * uart_sel.c - UART Selection Driver
+ *
+ * Copyright (C) 2009 Samsung Electronics
+ * Kim Kyuwon <q1.kim@samsung.com>
+ *
+ * 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.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/uart_select.h>
+
+struct uart_select {
+	struct uart_select_platform_data	*pdata;
+	struct rw_semaphore			rwsem;
+};
+
+static int uart_saved_state = UART_SW_PATH_NA;
+
+static ssize_t uart_select_show_state(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct uart_select *uart_sel =
+			platform_get_drvdata(to_platform_device(dev));
+	int ret;
+	int path;
+
+	path = uart_sel->pdata->get_uart_switch();
+
+	down_read(&uart_sel->rwsem);
+	uart_saved_state = path;
+	if (path == UART_SW_PATH_NA)
+		ret = sprintf(buf, "NA\n");
+	else if (path == UART_SW_PATH_CP)
+		ret = sprintf(buf, "CP\n");
+	else
+		ret = sprintf(buf, "AP\n");
+	up_read(&uart_sel->rwsem);
+
+	return ret;
+}
+
+static ssize_t uart_select_store_state(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct uart_select *uart_sel =
+			platform_get_drvdata(to_platform_device(dev));
+	struct uart_select_platform_data *pdata = uart_sel->pdata;
+	int path;
+
+	if (!count)
+		return -EINVAL;
+
+	down_write(&uart_sel->rwsem);
+	if (!strncmp(buf, "CP", 2))
+		path = UART_SW_PATH_CP;
+	else if (!strncmp(buf, "AP", 2))
+		path = UART_SW_PATH_AP;
+	else {
+		up_write(&uart_sel->rwsem);
+		dev_err(dev, "Invalid cmd !!\n");
+		return -EINVAL;
+	}
+	pdata->set_uart_switch(path);
+	uart_saved_state = path;
+	up_write(&uart_sel->rwsem);
+
+	return count;
+}
+
+static struct device_attribute uart_select_attr = {
+	.attr = {
+		.name = "path",
+		.mode = 0644,
+	},
+	.show = uart_select_show_state,
+	.store = uart_select_store_state,
+};
+
+/* Used in uart isr to avoid triggering sysrq when uart is not in AP */
+int uart_sel_get_state(void)
+{
+	if (uart_saved_state < 0)
+		return -EPERM;
+	else
+		return uart_saved_state;
+}
+EXPORT_SYMBOL(uart_sel_get_state);
+
+static int __devinit uart_select_probe(struct platform_device *pdev)
+{
+	struct uart_select *uart_sel;
+	struct uart_select_platform_data *pdata = pdev->dev.platform_data;
+	int ret;
+
+	uart_sel = kzalloc(sizeof(struct uart_select), GFP_KERNEL);
+	if (!uart_sel) {
+		dev_err(&pdev->dev, "failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, uart_sel);
+	uart_sel->pdata = pdata;
+	init_rwsem(&uart_sel->rwsem);
+
+	uart_saved_state = pdata->get_uart_switch();
+
+	ret = device_create_file(&pdev->dev, &uart_select_attr);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to crreate device file\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devexit uart_select_remove(struct platform_device *pdev)
+{
+	device_remove_file(&pdev->dev, &uart_select_attr);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver uart_select_driver = {
+	.probe		= uart_select_probe,
+	.remove		= __devexit_p(uart_select_remove),
+	.driver		= {
+		.name	= "uart-select",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init uart_select_init(void)
+{
+	return platform_driver_register(&uart_select_driver);
+}
+module_init(uart_select_init);
+
+static void __exit uart_select_exit(void)
+{
+	platform_driver_unregister(&uart_select_driver);
+}
+module_exit(uart_select_exit);
+
+MODULE_AUTHOR("Kim Kyuwon <q1.kim@samsung.com>");
+MODULE_DESCRIPTION("UART Selection Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/misc/uid_stat.c b/drivers/misc/uid_stat.c
new file mode 100644
index 0000000..2141124
--- /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/usb3503.c b/drivers/misc/usb3503.c
new file mode 100644
index 0000000..c7b88d7
--- /dev/null
+++ b/drivers/misc/usb3503.c
@@ -0,0 +1,519 @@
+/*
+ * drivers/misc/usb3503.c - usb3503 usb hub driver
+ *
+ * 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/i2c.h>
+#include <linux/platform_device.h>
+#include <linux/platform_data/usb3503.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+
+static int usb3503_register_write(struct i2c_client *i2c_dev, char reg,
+	char data)
+{
+	int ret;
+	char buf[2];
+	struct i2c_msg msg[] = {
+		{
+			.addr = i2c_dev->addr,
+			.flags = 0,
+			.len = 2,
+			.buf = buf,
+		},
+	};
+
+	buf[0] = reg;
+	buf[1] = data;
+
+	ret = i2c_transfer(i2c_dev->adapter, msg, 1);
+	if (ret < 0)
+		pr_err(HUB_TAG "%s: reg: %x data: %x write failed\n",
+		__func__, reg, data);
+
+	return ret;
+}
+
+static int usb3503_register_read(struct i2c_client *i2c_dev, char reg,
+	char *data)
+{
+	int ret;
+	struct i2c_msg msgs[] = {
+		{
+			.addr = i2c_dev->addr,
+			.flags = 0,
+			.len = 1,
+			.buf = &reg,
+		},
+		{
+			.addr = i2c_dev->addr,
+			.flags = I2C_M_RD,
+			.len = 1,
+			.buf = data,
+		},
+	};
+
+	ret = i2c_transfer(i2c_dev->adapter, msgs, 2);
+	if (ret < 0)
+		pr_err(HUB_TAG "%s: reg: %x read failed\n", __func__, reg);
+
+	return ret;
+}
+
+void s5pv210_hsic_port1_power(int enable)
+{
+	/*TODO:*/
+}
+
+static int reg_write(struct i2c_client *i2c_dev, char reg, char req, int retry)
+{
+	int cnt = retry, err;
+	char data = 0;
+
+	pr_debug(HUB_TAG "%s: write %02X, data: %02x\n", __func__, reg, req);
+	do {
+		err = usb3503_register_write(i2c_dev, reg, req);
+		if (err < 0) {
+			pr_err(HUB_TAG "%s: usb3503_register_write failed"
+					" - retry(%d)", __func__, cnt);
+			continue;
+		}
+
+		err = usb3503_register_read(i2c_dev, reg, &data);
+		if (err < 0)
+			pr_err(HUB_TAG "%s: usb3503_register_read failed"
+					" - retry(%d)", __func__, cnt);
+	} while (data != req && cnt--);
+exit:
+	pr_info(HUB_TAG "%s: write %02X, req:%02x, val:%02x\n", __func__, reg,
+		req, data);
+
+	return err;
+}
+
+static int reg_update(struct i2c_client *i2c_dev, char reg, char req, int retry)
+{
+	int cnt = retry, err;
+	char data;
+
+	pr_debug(HUB_TAG "%s: update %02X, data: %02x\n", __func__, reg, req);
+	do {
+		err = usb3503_register_read(i2c_dev, reg, &data);
+		if (err < 0) {
+			pr_err(HUB_TAG "%s: usb3503_register_read failed"
+					" - retry(%d)", __func__, cnt);
+			continue;
+		}
+
+		pr_debug(HUB_TAG "%s: read %02X, data: %02x\n", __func__, reg,
+			data);
+		if ((data & req) == req) {
+			pr_debug(HUB_TAG "%s: aleady set data: %02x\n",
+				__func__, data);
+			break;
+		}
+		err = usb3503_register_write(i2c_dev, reg, data | req);
+		if (err < 0)
+			pr_err(HUB_TAG "%s: usb3503_register_write failed"
+					" - retry(%d)", __func__, cnt);
+	} while (cnt--);
+exit:
+	pr_info(HUB_TAG "%s: update %02X, req:%02x, val:%02x\n", __func__, reg,
+		req, data);
+	return err;
+}
+
+static int reg_clear(struct i2c_client *i2c_dev, char reg, char req, int retry)
+{
+	int cnt = retry, err;
+	char data;
+
+	pr_debug(HUB_TAG "%s: clear %X, data %x\n", __func__, reg, req);
+	do {
+		err = usb3503_register_read(i2c_dev, reg, &data);
+		if (err < 0)
+			goto exit;
+		pr_debug(HUB_TAG "%s: read %02X, data %02x\n", __func__, reg,
+			data);
+		if (!(data & req)) {
+			pr_err(HUB_TAG "%s: aleady cleared data = %02x\n",
+				__func__, data);
+			break;
+		}
+		err = usb3503_register_write(i2c_dev, reg, data & ~req);
+		if (err < 0)
+			goto exit;
+	} while (cnt--);
+exit:
+	pr_info(HUB_TAG "%s: clear %02X, req:%02x, val:%02x\n", __func__, reg,
+		req, data);
+	return err;
+}
+
+static int usb3503_set_mode(struct usb3503_hubctl *hc, int mode)
+{
+	int err = 0;
+	struct i2c_client *i2c_dev = hc->i2c_dev;
+
+	pr_info(HUB_TAG "%s: mode = %d\n", __func__, mode);
+
+	switch (mode) {
+	case USB3503_MODE_HUB:
+		hc->reset_n(1);
+
+		/* SP_ILOCK: set connect_n, config_n for config */
+		err = reg_write(i2c_dev, SP_ILOCK_REG,
+			(SPILOCK_CONNECT_N | SPILOCK_CONFIG_N), 3);
+		if (err < 0) {
+			pr_err(HUB_TAG "SP_ILOCK write fail err = %d\n", err);
+			goto exit;
+		}
+#ifdef USB3503_ES_VER
+/* ES version issue
+ * USB3503 can't PLL power up under cold circumstance, so enable
+ * the Force suspend clock bit
+ */
+		err = reg_update(i2c_dev, CFGP_REG, CFGP_CLKSUSP, 1);
+		if (err < 0) {
+			pr_err(HUB_TAG "CFGP update fail err = %d\n", err);
+			goto exit;
+		}
+#endif
+		/* PDS : Port2,3 Disable For Self Powered Operation */
+		err = reg_update(i2c_dev, PDS_REG, (PDS_PORT2 | PDS_PORT3), 1);
+		if (err < 0) {
+			pr_err(HUB_TAG "PDS update fail err = %d\n", err);
+			goto exit;
+		}
+		/* CFG1 : SELF_BUS_PWR -> Self-Powerd operation */
+		err = reg_update(i2c_dev, CFG1_REG, CFG1_SELF_BUS_PWR, 1);
+		if (err < 0) {
+			pr_err(HUB_TAG "CFG1 update fail err = %d\n", err);
+			goto exit;
+		}
+		/* SP_LOCK: clear connect_n, config_n for hub connect */
+		err = reg_clear(i2c_dev, SP_ILOCK_REG,
+			(SPILOCK_CONNECT_N | SPILOCK_CONFIG_N), 1);
+		if (err < 0) {
+			pr_err(HUB_TAG "SP_ILOCK clear err = %d\n", err);
+			goto exit;
+		}
+		hc->mode = mode;
+
+		/* Should be enable the HSIC port1 */
+
+		break;
+
+	case USB3503_MODE_STANDBY:
+		hc->reset_n(0);
+		hc->mode = mode;
+		break;
+
+	default:
+		pr_err(HUB_TAG "%s: Invalid mode %d\n", __func__, mode);
+		err = -EINVAL;
+		goto exit;
+		break;
+	}
+exit:
+	return err;
+}
+
+/* sysfs for control */
+static ssize_t mode_show(struct device *dev, struct device_attribute *attr,
+	char *buf)
+{
+	struct usb3503_hubctl *hc = dev_get_drvdata(dev);
+
+	if (hc->mode == USB3503_MODE_HUB)
+		return sprintf(buf, "%s", "hub");
+	else if (hc->mode == USB3503_MODE_STANDBY)
+		return sprintf(buf, "%s", "standby");
+
+	return 0;
+}
+
+static ssize_t mode_store(
+		struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t size)
+{
+	struct usb3503_hubctl *hc = dev_get_drvdata(dev);
+
+	if (!strncmp(buf, "hub", 3)) {
+		/*usb3503_set_mode(hc, USB3503_MODE_HUB);*/
+		if (hc->port_enable)
+			hc->port_enable(2, 1);
+		pr_debug(HUB_TAG "mode set to hub\n");
+	} else if (!strncmp(buf, "standby", 7)) {
+		/*usb3503_set_mode(hc, USB3503_MODE_STANDBY);*/
+		if (hc->port_enable)
+			hc->port_enable(2, 0);
+		pr_debug(HUB_TAG "mode set to standby\n");
+	}
+	return size;
+}
+static DEVICE_ATTR(mode, 0664, mode_show, mode_store);
+
+#ifdef USB3503_SYSFS_DEBUG
+static ssize_t read_store(
+		struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t size)
+{
+	unsigned addr;
+	char data;
+	int err;
+	struct usb3503_hubctl *hc = dev_get_drvdata(dev);
+
+	err = sscanf(buf, "%x", &addr);
+
+	err = usb3503_register_read(hc->i2c_dev, addr, &data);
+	if (err < 0) {
+		pr_err(HUB_TAG "register read fail\n");
+		goto exit;
+	}
+	pr_info(HUB_TAG "%s: read 0x%x = 0x%x\n", __func__, addr, data);
+exit:
+	return size;
+}
+static DEVICE_ATTR(read, 0664, NULL, read_store);
+
+static ssize_t write_store(
+		struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t size)
+{
+	unsigned addr, data;
+	int err;
+	struct usb3503_hubctl *hc = dev_get_drvdata(dev);
+
+	err = sscanf(buf, "%x %x", &addr, &data);
+	pr_debug(HUB_TAG "%s: addr=%x, data=%x\n", __func__, addr, data);
+
+	err = usb3503_register_write(hc->i2c_dev, addr, data);
+	if (err < 0) {
+		pr_err(HUB_TAG "register write fail\n");
+		goto exit;
+	}
+
+	err = usb3503_register_read(hc->i2c_dev, addr, (char *)&data);
+	if (err < 0) {
+		pr_err(HUB_TAG "register read fail\n");
+		goto exit;
+	}
+	pr_info(HUB_TAG "%s: write 0x%x = 0x%x\n", __func__, addr, data);
+exit:
+	return size;
+}
+static DEVICE_ATTR(write, 0664, NULL, write_store);
+
+static ssize_t reset_store(
+		struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t size)
+{
+	unsigned val;
+	int err;
+	struct usb3503_hubctl *hc = dev_get_drvdata(dev);
+
+	err = sscanf(buf, "%x", &val);
+	pr_info(HUB_TAG "%s: val=%x\n", __func__, val);
+
+	hc->reset_n(val);
+
+	return size;
+}
+static DEVICE_ATTR(reset, 0664, NULL, reset_store);
+#endif /* end of USB3503_SYSFS_DEBUG */
+
+int usb3503_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	struct usb3503_hubctl *hc = i2c_get_clientdata(client);
+#if defined(CONFIG_MACH_C1)
+	struct regulator *regulator;
+#endif
+
+	/* Should be disable the HSIC port1 */
+
+	hc->reset_n(0);
+	pr_info(HUB_TAG "suspended\n");
+
+#if defined(CONFIG_MACH_C1)
+
+	if (system_rev >= 0x6) {
+		regulator = regulator_get(NULL, "vusbhub_osc_1.8v");
+		if (IS_ERR(regulator)) {
+			pr_err(HUB_TAG "%s:Get VUSBHUBOSC Fail\n", __func__);
+			return 0;
+		}
+		regulator_disable(regulator);
+		regulator_put(regulator);
+	}
+#endif
+
+	return 0;
+}
+
+int usb3503_resume(struct i2c_client *client)
+{
+	struct usb3503_hubctl *hc = i2c_get_clientdata(client);
+
+#if defined(CONFIG_MACH_M0_CTC)
+	return 0;
+#endif
+
+#if defined(CONFIG_MACH_C1)
+
+	struct regulator *regulator;
+
+	if (system_rev >= 0x6) {
+		regulator = regulator_get(NULL, "vusbhub_osc_1.8v");
+		if (IS_ERR(regulator)) {
+			pr_err(HUB_TAG "%s:Get VUSBHUBOSC Fail\n", __func__);
+			return 0;
+		}
+		regulator_enable(regulator);
+		regulator_put(regulator);
+
+		mdelay(3);
+	}
+#endif
+
+	if (hc->mode == USB3503_MODE_HUB)
+		usb3503_set_mode(hc, USB3503_MODE_HUB);
+
+	pr_info(HUB_TAG "resume mode=%s", (hc->mode == USB3503_MODE_HUB) ?
+		"hub" : "standny");
+
+	return 0;
+}
+
+int usb3503_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	int err = 0;
+	struct usb3503_hubctl *hc;
+	struct usb3503_platform_data *pdata;
+
+#if defined(CONFIG_MACH_C1)
+
+	struct regulator *regulator;
+
+	if (system_rev >= 0x6) {
+		regulator = regulator_get(NULL, "vusbhub_osc_1.8v");
+		if (IS_ERR(regulator)) {
+			pr_err(HUB_TAG "%s:Get VUSBHUBOSC Fail\n", __func__);
+			return 0;
+		}
+		regulator_enable(regulator);
+		regulator_put(regulator);
+	}
+#endif
+
+	pr_info(HUB_TAG "%s:%d\n", __func__, __LINE__);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		err = -ENODEV;
+		goto exit;
+	}
+
+	pdata = client->dev.platform_data;
+	if (pdata == NULL) {
+		pr_err(HUB_TAG "device's platform data is NULL!\n");
+		err = -ENODEV;
+		goto exit;
+	}
+
+	hc = kzalloc(sizeof(struct usb3503_hubctl), GFP_KERNEL);
+	if (!hc) {
+		pr_err(HUB_TAG "private data alloc fail\n");
+		err = -ENOMEM;
+		goto exit;
+	}
+	hc->i2c_dev = client;
+	hc->reset_n = pdata->reset_n;
+	hc->port_enable = pdata->port_enable;
+	if (pdata->initial_mode) {
+		usb3503_set_mode(hc, pdata->initial_mode);
+		hc->mode = pdata->initial_mode;
+	}
+	/* For HSIC to USB brige with CMC221
+	 * export the hub_set_mode and private data to board modem
+	 * it will be handled by PM scenaio.
+	 */
+	if (pdata->register_hub_handler)
+		pdata->register_hub_handler((void (*)(void))usb3503_set_mode,
+			(void *)hc);
+
+	i2c_set_clientdata(client, hc);
+
+	err = device_create_file(&client->dev, &dev_attr_mode);
+#ifdef USB3503_SYSFS_DEBUG
+	err = device_create_file(&client->dev, &dev_attr_read);
+	err = device_create_file(&client->dev, &dev_attr_write);
+	err = device_create_file(&client->dev, &dev_attr_reset);
+#endif
+	pr_info(HUB_TAG "%s: probed on  %s mode\n", __func__,
+		(hc->mode == USB3503_MODE_HUB) ? "hub" : "standby");
+exit:
+	return err;
+}
+
+static int usb3503_remove(struct i2c_client *client)
+{
+	struct usb3503_hubctl *hc = i2c_get_clientdata(client);
+
+	pr_debug(HUB_TAG "%s\n", __func__);
+	kfree(hc);
+
+	return 0;
+}
+
+static const struct i2c_device_id usb3503_id[] = {
+	{ USB3503_I2C_NAME, 0 },
+	{ }
+};
+
+static void usb3503_shutdown(struct i2c_client *client)
+{
+	struct usb3503_hubctl *hc = i2c_get_clientdata(client);
+
+	pr_err(HUB_TAG "%s:\n", __func__);
+	mdelay(10);
+	usb3503_set_mode(hc, USB3503_MODE_STANDBY);
+}
+
+static struct i2c_driver usb3503_driver = {
+	.probe = usb3503_probe,
+	.remove = usb3503_remove,
+	.suspend = usb3503_suspend,
+	.resume = usb3503_resume,
+	.shutdown = usb3503_shutdown,
+	.id_table = usb3503_id,
+	.driver = {
+		.name = USB3503_I2C_NAME,
+	},
+};
+
+static int __init usb3503_init(void)
+{
+	pr_info(HUB_TAG "USB HUB driver init\n");
+	return i2c_add_driver(&usb3503_driver);
+}
+
+static void __exit usb3503_exit(void)
+{
+	pr_info(HUB_TAG "USB HUB driver exit\n");
+	i2c_del_driver(&usb3503_driver);
+}
+module_init(usb3503_init);
+module_exit(usb3503_exit);
+
+MODULE_DESCRIPTION("USB3503 USB HUB driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/wl127x-rfkill.c b/drivers/misc/wl127x-rfkill.c
new file mode 100644
index 0000000..f5b9515
--- /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");