IKXCLOCK-26: Add support for STM401

Change-Id: I60489dfee479c9994792aaa82295a960f3965ae7
Reviewed-on: http://gerrit.pcs.mot.com/587826
SLTApproved: Slta Waiver <sltawvr@motorola.com>
Tested-by: Jira Key <jirakey@motorola.com>
Reviewed-by: Douglas Zobel <dzobel1@motorola.com>
Submit-Approved: Jira Key <jirakey@motorola.com>

11 files changed, 809 insertions, 31 deletions

diff --git a/arch/arm/boot/dts/omap3-casper-p1.dts b/arch/arm/boot/dts/omap3-casper-p1.dts
index 898da9be9b9..95fa27c6a76 100644
--- a/arch/arm/boot/dts/omap3-casper-p1.dts
+++ b/arch/arm/boot/dts/omap3-casper-p1.dts
@@ -13,3 +13,25 @@
 	mmi,revision = "p1";
 	mmi,hwrev = <0xFE>;
 };
+
+&i2c2 {
+    m4sensorhub@39 {
+		status = "disabled";
+    };
+};
+
+&i2c2 {
+    m4sensorhub@18 {
+        compatible = "mot,m4sensorhub";
+        reg = <0x18>;
+
+        mot,irq-gpio    = <&gpio2 27 1>; /* gpio-059 */
+        mot,reset-gpio  = <&gpio5  1 2>; /* gpio-129 */
+        mot,wake-gpio   = <&gpio4 30 0>; /* gpio-126 */
+        mot,boot0-gpio  = <&gpio1 21 0>; /* gpio-021 */
+        mot,boot1-gpio  = <&gpio1 24 0>; /* gpio-024 */
+        mot,enable-gpio = <&gpio1 28 0>; /* gpio-028 */
+	mot,fw-filename = "m4sensorhub-p1_casper.bin";
+    };
+};
+
diff --git a/arch/arm/boot/dts/omap3-minnow.dtsi b/arch/arm/boot/dts/omap3-minnow.dtsi
index 562205e34dd..5997b6cdb2c 100644
--- a/arch/arm/boot/dts/omap3-minnow.dtsi
+++ b/arch/arm/boot/dts/omap3-minnow.dtsi
@@ -112,9 +112,10 @@
 };
 
 &i2c2 {
-	m4sensorhub@18 {
+	m4sensorhub@39 {
 		compatible = "mot,m4sensorhub";
-		reg = <0x18>;
+		status = "ok";
+		reg = <0x39>;
 
 		mot,irq-gpio    = <&gpio2 27 1>; /* gpio-059 */
 		mot,reset-gpio  = <&gpio5  1 2>; /* gpio-129 */
@@ -206,6 +207,7 @@
 	wkup_pins: pinmux_board_pins {
 		pinctrl-single,pins = <
 			0x01a 0x10c     /* SYS_CLKOUT1, MODE4 | INPUT_PULLDOWN */
+			0x05a 0x004     /* GPIO_129, MODE4 | OUTPUT */
 		>;
 	};
 };
diff --git a/arch/arm/mach-omap2/board-minnow.c b/arch/arm/mach-omap2/board-minnow.c
index ef21cfb8956..72d43c88d1a 100644
--- a/arch/arm/mach-omap2/board-minnow.c
+++ b/arch/arm/mach-omap2/board-minnow.c
@@ -17,11 +17,11 @@
 #include <linux/usb/musb.h>
 #include <linux/usb/phy.h>
 #include <linux/usb/nop-usb-xceiv.h>
-#include "board-minnow.h"
 #include "mux.h"
 #include "common.h"
 #include "dss-common.h"
 #include "board-minnow.h"
+#include "control.h"
 
 #include "sdram-toshiba-hynix-numonyx.h"
 
@@ -46,6 +46,7 @@ static void __init minnow_init(void)
 	of_platform_populate(NULL, omap_dt_match_table, NULL, NULL);
 
 	omap_sdrc_init(JEDEC_JESD209A_sdrc_params, JEDEC_JESD209A_sdrc_params);
+	omap3_enable_usim_buffer(); /* Needed for GPIOs in USIM block */
 	omap_minnow_display_init();
 	minnow_spi_init();
 	minnow_bt_init();
diff --git a/arch/arm/mach-omap2/control.c b/arch/arm/mach-omap2/control.c
index 2adb2683f07..67dd881c23b 100644
--- a/arch/arm/mach-omap2/control.c
+++ b/arch/arm/mach-omap2/control.c
@@ -235,6 +235,22 @@ void omap3_ctrl_write_boot_mode(u8 bootmode)
 	__raw_writel(l, OMAP2_L4_IO_ADDRESS(OMAP343X_SCRATCHPAD + 4));
 }
 
+/**
+ * omap3_enable_usim_IO - enable USIM block input/output buffers
+ *
+ * Enable USIM block input/output buffers.  By default the buffers are
+ * not enabled.  Once vdds is stable, they can be enabled.  They must
+ * be enabled before using any GPIOs from this block.
+ */
+void omap3_enable_usim_buffer(void)
+{
+	u32 reg;
+
+	reg = omap_ctrl_readl(OMAP343X_CONTROL_WKUP_CTRL);
+	reg |= OMAP343X_USIM_IO_PWRDNZ;
+	omap_ctrl_writel(reg, OMAP343X_CONTROL_WKUP_CTRL);
+}
+
 #endif
 
 /**
diff --git a/arch/arm/mach-omap2/control.h b/arch/arm/mach-omap2/control.h
index e6c328128a0..8698aed1e84 100644
--- a/arch/arm/mach-omap2/control.h
+++ b/arch/arm/mach-omap2/control.h
@@ -243,6 +243,9 @@
 #define OMAP343X_CONTROL_WKUP_DEBOBS3 (OMAP343X_CONTROL_GENERAL_WKUP + 0x014)
 #define OMAP343X_CONTROL_WKUP_DEBOBS4 (OMAP343X_CONTROL_GENERAL_WKUP + 0x018)
 
+/* 34xx USIM register offsets */
+#define OMAP343X_CONTROL_WKUP_CTRL    (OMAP2_CONTROL_INTERFACE + 0x0A5C)
+
 /* 36xx-only RTA - Retention till Access control registers and bits */
 #define OMAP36XX_CONTROL_MEM_RTA_CTRL	0x40C
 #define OMAP36XX_RTA_DISABLE		0x0
@@ -323,6 +326,9 @@
 #define OMAP343X_SCRATCHPAD_REGADDR(reg)	OMAP2_L4_IO_ADDRESS(\
 						OMAP343X_SCRATCHPAD + reg)
 
+/* CONTROL_WKUP bits */
+#define OMAP343X_USIM_IO_PWRDNZ         (1 << 6)
+
 /* AM35XX_CONTROL_IPSS_CLK_CTRL bits */
 #define AM35XX_USBOTG_VBUSP_CLK_SHIFT	0
 #define AM35XX_CPGMAC_VBUSP_CLK_SHIFT	1
@@ -390,6 +396,7 @@
 #define		FEAT_NEON_NONE		1
 
 
+
 #ifndef __ASSEMBLY__
 #ifdef CONFIG_ARCH_OMAP2PLUS
 extern void __iomem *omap_ctrl_base_get(void);
@@ -417,6 +424,8 @@ extern void omap3630_ctrl_disable_rta(void);
 extern int omap3_ctrl_save_padconf(void);
 extern void omap2_set_globals_control(void __iomem *ctrl,
 				      void __iomem *ctrl_pad);
+/* called from board-minnow.c to enable USIM GPIOs */
+extern void omap3_enable_usim_buffer(void);
 #else
 #define omap_ctrl_base_get()		0
 #define omap_ctrl_readb(x)		0
@@ -429,6 +438,5 @@ extern void omap2_set_globals_control(void __iomem *ctrl,
 #define omap4_ctrl_pad_writel(x, y)	WARN_ON(1)
 #endif
 #endif	/* __ASSEMBLY__ */
-
 #endif /* __ARCH_ARM_MACH_OMAP2_CONTROL_H */
 
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index c4aa99058c7..b11b8bf992f 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -170,7 +170,8 @@ m4sensorhub-objs                := m4sensorhub-core.o \
                                    m4sensorhub-reg.o \
                                    m4sensorhub-irq.o \
                                    m4sensorhub-panic.o \
-                                   m4sensorhub-stm32-fw.o
+                                   m4sensorhub-stm32-fw.o \
+                                   m4sensorhub-stm32_401-fw.o
 
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub.o
 
diff --git a/drivers/mfd/m4sensorhub-core.c b/drivers/mfd/m4sensorhub-core.c
index 14bc2d26071..cb8bad9743f 100644
--- a/drivers/mfd/m4sensorhub-core.c
+++ b/drivers/mfd/m4sensorhub-core.c
@@ -119,25 +119,37 @@ int m4sensorhub_set_bootmode(struct m4sensorhub_data *m4sensorhub,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(m4sensorhub_set_bootmode);
 
-static void minnow_m4sensorhub_hw_reset(struct m4sensorhub_data *m4sensorhub)
+void m4sensorhub_hw_reset(struct m4sensorhub_data *m4sensorhub)
 {
 	if (!m4sensorhub) {
 		printk(KERN_ERR "m4sensorhub_hw_reset: invalid pointer\n");
 		return;
 	}
 
-	m4sensorhub_set_bootmode(m4sensorhub, BOOTMODE00);
-	gpio_set_value(m4sensorhub->hwconfig.reset_gpio, 1);
-	msleep(5);
-	gpio_set_value(m4sensorhub->hwconfig.reset_gpio, 0);
-	msleep(5);
-	gpio_set_value(m4sensorhub->hwconfig.reset_gpio, 1);
+
+	if (m4sensorhub->i2c_client->addr == 0x39) {
+		m4sensorhub_set_bootmode(m4sensorhub, BOOTMODE01);
+		usleep_range(5000, 10000);
+		gpio_set_value(m4sensorhub->hwconfig.reset_gpio, 0);
+		usleep_range(10000, 12000);
+		gpio_set_value(m4sensorhub->hwconfig.reset_gpio, 1);
+		msleep(400);
+	} else {
+		m4sensorhub_set_bootmode(m4sensorhub, BOOTMODE00);
+		gpio_set_value(m4sensorhub->hwconfig.reset_gpio, 1);
+		usleep_range(5000, 10000);
+		gpio_set_value(m4sensorhub->hwconfig.reset_gpio, 0);
+		usleep_range(5000, 10000);
+		gpio_set_value(m4sensorhub->hwconfig.reset_gpio, 1);
+	}
 }
+EXPORT_SYMBOL_GPL(m4sensorhub_hw_reset);
 
 
 /* callback from driver to initialize hardware on probe */
-static int minnow_m4sensorhub_hw_init(struct m4sensorhub_data *m4sensorhub,
+static int m4sensorhub_hw_init(struct m4sensorhub_data *m4sensorhub,
 		struct device_node *node)
 {
 	int gpio;
@@ -223,7 +235,7 @@ static int minnow_m4sensorhub_hw_init(struct m4sensorhub_data *m4sensorhub,
 	gpio_direction_output(gpio, 0);
 	m4sensorhub->hwconfig.mpu_9150_en_gpio = gpio;
 
-	minnow_m4sensorhub_hw_reset(m4sensorhub);
+	m4sensorhub_hw_reset(m4sensorhub);
 
 	return 0;
 
@@ -248,7 +260,7 @@ error:
 }
 
 /* callback from driver to free hardware on shutdown */
-static void minnow_m4sensorhub_hw_free(struct m4sensorhub_data *m4sensorhub)
+static void m4sensorhub_hw_free(struct m4sensorhub_data *m4sensorhub)
 {
 
 	if (!m4sensorhub) {
@@ -343,15 +355,22 @@ static void m4sensorhub_initialize(const struct firmware *firmware,
 	}
 
 	/* initiate m4 firmware download */
-	firmware_download_status = m4sensorhub_load_firmware(
-					&m4sensorhub_misc_data,
-					force_upgrade,
-					firmware);
-
+	KDEBUG(M4SH_CRITICAL, "Starting firmware download with force_upgrade "
+		"= %d\n", force_upgrade);
+	if (m4sensorhub_misc_data.i2c_client->addr == 0x39)
+		firmware_download_status = m4sensorhub_401_load_firmware(
+						&m4sensorhub_misc_data,
+						force_upgrade,
+						firmware);
+	else
+		firmware_download_status = m4sensorhub_load_firmware(
+						&m4sensorhub_misc_data,
+						force_upgrade,
+						firmware);
 	if (firmware_download_status < 0) {
 		KDEBUG(M4SH_ERROR, "Failed to load M4 firmware = %d\n", err);
 		/* Since firmware download failed, put m4 back into boot mode*/
-		minnow_m4sensorhub_hw_reset(&m4sensorhub_misc_data);
+		m4sensorhub_hw_reset(&m4sensorhub_misc_data);
 		return;
 	}
 
@@ -494,15 +513,19 @@ static int m4sensorhub_probe(struct i2c_client *client,
 		err = -ENODEV;
 		goto err_unload;
 	}
-	err = minnow_m4sensorhub_hw_init(m4sensorhub, node);
-	if (err)
+
+	/* link m4sensorhub to i2c_client, hw_init uses i2c_client */
+	m4sensorhub->i2c_client = client;
+
+	err = m4sensorhub_hw_init(m4sensorhub, node);
+	if (err) {
 		printk(KERN_ERR "%s: hw_init Failed!", __func__);
+		goto done;
+	}
 
 	/* link i2c_client to m4sensorhub */
 	i2c_set_clientdata(client, m4sensorhub);
 
-	/* link m4sensorhub to i2c_client */
-	m4sensorhub->i2c_client = client;
 
 	err = misc_register(&m4sensorhub_misc_device);
 	if (err < 0) {
@@ -560,7 +583,7 @@ err_deregister:
 err_hw_free:
 	m4sensorhub->i2c_client = NULL;
 	i2c_set_clientdata(client, NULL);
-	minnow_m4sensorhub_hw_free(m4sensorhub);
+	m4sensorhub_hw_free(m4sensorhub);
 	m4sensorhub = NULL;
 err_unload:
 done:
@@ -577,11 +600,11 @@ static int __exit m4sensorhub_remove(struct i2c_client *client)
 	m4sensorhub_reg_shutdown(m4sensorhub);
 	device_remove_file(&client->dev, &dev_attr_log_level);
 	device_remove_file(&client->dev, &dev_attr_debug_level);
-	minnow_m4sensorhub_hw_reset(m4sensorhub);
+	m4sensorhub_hw_reset(m4sensorhub);
 	misc_deregister(&m4sensorhub_misc_device);
 	m4sensorhub->i2c_client = NULL;
 	i2c_set_clientdata(client, NULL);
-	minnow_m4sensorhub_hw_free(m4sensorhub);
+	m4sensorhub_hw_free(m4sensorhub);
 	m4sensorhub = NULL;
 
 	return 0;
diff --git a/drivers/mfd/m4sensorhub-panic.c b/drivers/mfd/m4sensorhub-panic.c
index 6ce982b3f0c..f1cc373b12f 100644
--- a/drivers/mfd/m4sensorhub-panic.c
+++ b/drivers/mfd/m4sensorhub-panic.c
@@ -225,7 +225,12 @@ void m4sensorhub_panic_process(struct m4sensorhub_data *m4sensorhub)
 	KDEBUG(M4SH_ERROR, "m4sensorhub_panic: Detected M4 panic, reset M4!\n");
 	m4sensorhub->pdev->hw_reset(m4sensorhub);
 	msleep(100);
-	ret = m4sensorhub_load_firmware(m4sensorhub, 0, NULL);
+	if (m4sensorhub->i2c_client->addr == 0x39) {
+		ret = m4sensorhub_401_load_firmware(m4sensorhub, 0, NULL);
+	} else {
+		ret = m4sensorhub_load_firmware(m4sensorhub, 0, NULL);
+	}
+
 	if (ret < 0) {
 		KDEBUG(M4SH_ERROR, "m4sensorhub_panic: "\
 			"Failed to restart M4, ret = %d\n", ret);
diff --git a/drivers/mfd/m4sensorhub-stm32-fw.c b/drivers/mfd/m4sensorhub-stm32-fw.c
index 811ec70c4b8..bf5597840b4 100644
--- a/drivers/mfd/m4sensorhub-stm32-fw.c
+++ b/drivers/mfd/m4sensorhub-stm32-fw.c
@@ -128,8 +128,6 @@ int m4sensorhub_load_firmware(struct m4sensorhub_data *m4sensorhub,
 		ret = request_firmware(&firmware, m4sensorhub->filename,
 			&m4sensorhub->i2c_client->dev);
 	}
-	KDEBUG(M4SH_INFO, "Firmware = %s\n", m4sensorhub->filename);
-
 	if (ret < 0) {
 		KDEBUG(M4SH_ERROR, "%s: request_firmware failed for %s\n",
 			__func__,  m4sensorhub->filename);
@@ -137,6 +135,7 @@ int m4sensorhub_load_firmware(struct m4sensorhub_data *m4sensorhub,
 		ret = 0;
 		goto done;
 	}
+	KDEBUG(M4SH_INFO, "%s:Firmware = %s\n", m4sensorhub->filename, __func__);
 
 	if (firmware->size > MAX_FILE_SIZE) {
 		KDEBUG(M4SH_ERROR, "%s: firmware file size is too big.\n",
diff --git a/drivers/mfd/m4sensorhub-stm32_401-fw.c b/drivers/mfd/m4sensorhub-stm32_401-fw.c
new file mode 100644
index 00000000000..293e2816e4b
--- /dev/null
+++ b/drivers/mfd/m4sensorhub-stm32_401-fw.c
@@ -0,0 +1,695 @@
+/*
+ * Copyright (C) 2013 Motorola Mobility LLC.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
+ * 02111-1307, USA
+ */
+
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/m4sensorhub.h>
+#include <linux/slab.h>
+/* --------------- Global Declarations -------------- */
+
+/* ------------ Local Function Prototypes ----------- */
+static int m4sensorhub_bl_ack(struct m4sensorhub_data *m4sensorhub);
+static int m4sensorhub_bl_rm(struct m4sensorhub_data *m4sensorhub,
+	int start_address, u8 *data, int len);
+static int m4sensorhub_bl_go(struct m4sensorhub_data *m4sensorhub,
+	int start_address);
+static int m4sensorhub_bl_wm(struct m4sensorhub_data *m4sensorhub,
+	int start_address, u8 *data, int len);
+static int m4sensorhub_bl_erase_fw(struct m4sensorhub_data *m4sensorhub);
+static int m4sensorhub_jump_to_user(struct m4sensorhub_data *m4sensorhub);
+
+/* --------------- Local Declarations -------------- */
+
+#define ACK		((uint8_t) (0x79))
+#define NACK	((uint8_t) (0x1F))
+#define BUSY    ((uint8_t) (0x76))
+
+/* LIST OF OPCODES FOR BOOTLOADER */
+/* opcode_get */
+#define OPC_GET	((uint8_t) (0x00))
+/* opcode_get_version */
+#define OPC_GV	((uint8_t) (0x01))
+/* opcode_get_id */
+#define OPC_GID	((uint8_t) (0x02))
+/* opcode_read_memory */
+#define OPC_RM	((uint8_t) (0x11))
+/* opcode_go */
+#define OPC_GO	((uint8_t) (0x21))
+/* opcode_write_memory */
+#define OPC_WM	((uint8_t) (0x31))
+/* opcode_no_stretch_write_memory */
+#define OPC_NO_STRETCH_WM	((uint8_t) (0x32))
+/* opcode_erase */
+#define OPC_ER	((uint8_t) (0x44))
+/* opcode_no_stretch_erase */
+#define OPC_NO_STRETCH_ER	((uint8_t) (0x45))
+/* opcode_write_protect */
+#define OPC_WP	((uint8_t) (0x63))
+/* opcode_write_unprotect */
+#define OPC_WU	((uint8_t) (0x73))
+/* opcode_readout_protect */
+#define OPC_RP	((uint8_t) (0x82))
+/* opcode_readout_unprotect */
+#define OPC_RU	((uint8_t) (0x92))
+/* opcode_no_stretch_readout_unprotect */
+#define OPC_NO_STRETCH_RU	((uint8_t) (0x93))
+
+/* We flash code in sector 4 and sector 5 */
+#define USER_FLASH_FIRST_PAGE_ADDRESS	0x08010000
+/* TO CHECK : Is it a valid assumption that the version number is
+0x200 bytes into the file*/
+#define VERSION_OFFSET  0x200
+#define VERSION_ADDRESS (USER_FLASH_FIRST_PAGE_ADDRESS + VERSION_OFFSET)
+
+/* Sector 5 ends at 0x0803FFFF, and we put 4 byte barker */
+/* at the end of Sector 5, giving BARKER_ADDRESS as (0x0803FFFF - 0x4 + 0x1) */
+#define BARKER_ADDRESS	0x0803FFFC
+#define BARKER_NUMBER	0xACEC0DE
+/* The MAX_FILE_SIZE is the size of sectors 4 and 5  where the firmware code
+ * will reside (minus the barker size).
+
+ * From the Flash Programming Manual:
+  Sector  Start Address End Address Size
+  ------  ------------- ----------------
+  0       0x0800 0000   0x0800 3FFF 16 Kbytes
+  1       0x0800 4000   0x0800 7FFF 16 Kbytes
+  2       0x0800 8000   0x8000 BFFF 16 Kbytes
+  3       0x0800 C000   0x0800 FFFF 16 Kbytes
+  4       0x0801 0000   0x0801 FFFF 64 Kbytes
+  5       0x0802 0000   0x0803 FFFF 128 Kbytes
+
+ */
+/* Max file size is 64Kb + 128 Kb - 4 bytes for barker */
+#define MAX_FILE_SIZE	(64*1024+128*1024-4) /* bytes */
+/* Transfer is done in 256 bytes, or if data is less than 256 bytes, then
+data is 16 bit aligned and sent out */
+#define MAX_TRANSFER_SIZE	256 /* bytes */
+/* If M4 returns back BUSY, then we give it 40 chances to execute the command
+each attempt to check status is delayed by 100 ms, thus 40 attempts give it a
+window of about 4 seconds. */
+#define MAX_ATTEMPTS 40
+/* We are flashing/erasing 2 sectors */
+#define NUM_FLASH_TO_ERASE 2
+#define SECTOR_TO_ERASE_1 4
+#define SECTOR_TO_ERASE_2 5
+int page_to_erase[2] = {SECTOR_TO_ERASE_1, SECTOR_TO_ERASE_2};
+/* -------------- Local Data Structures ------------- */
+
+
+/* -------------- Global Functions ----------------- */
+
+/* m4sensorhub_401_load_firmware()
+   This function uses I2C bootloader version 1.1 on M4
+   Check firmware and load if different from what's already on the M4.
+   Then jump to user code on M4.
+
+   Returns 0 on success or negative error code on failure
+
+     m4sensorhub - pointer to the main m4sensorhub data struct
+*/
+
+int m4sensorhub_401_load_firmware(struct m4sensorhub_data *m4sensorhub,
+	unsigned short force_upgrade, const struct firmware *fm)
+{
+	const struct firmware *firmware = fm;
+	int ret = 0;
+	int bytes_left, bytes_to_write;
+	int address_to_write;
+	u8 *buf_to_read, *buf = NULL;
+	u16 fw_version_file, fw_version_device;
+	u32 barker_read_from_device;
+
+	m4sensorhub_hw_reset(m4sensorhub);
+
+	buf = kzalloc(MAX_TRANSFER_SIZE, GFP_KERNEL);
+	if (!buf) {
+		ret = -ENOMEM;
+		goto done;
+	}
+
+	if (firmware == NULL) {
+		ret = request_firmware(&firmware, m4sensorhub->filename,
+			&m4sensorhub->i2c_client->dev);
+	}
+	if (ret < 0) {
+		KDEBUG(M4SH_ERROR, "%s: request_firmware failed for %s\n",
+				__func__, m4sensorhub->filename);
+		KDEBUG(M4SH_ERROR, "Trying to run firmware already on hw.\n");
+		ret = 0;
+		goto done;
+	}
+	 KDEBUG(M4SH_INFO, "%s: Filename = %s",
+		__func__, m4sensorhub->filename);
+
+	if (firmware->size > MAX_FILE_SIZE) {
+		KDEBUG(M4SH_ERROR, "%s: firmware file size is too big.\n",
+			__func__);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	fw_version_file = *(u16 *)(firmware->data +
+		VERSION_ADDRESS - USER_FLASH_FIRST_PAGE_ADDRESS);
+
+	if (!force_upgrade) {
+		/* Verify Barker number from device */
+		if (m4sensorhub_bl_rm(m4sensorhub, BARKER_ADDRESS,
+				(u8 *)&barker_read_from_device,
+				sizeof(barker_read_from_device)) != 0) {
+			KDEBUG(M4SH_ERROR, "%s : %d : error reading Barker\n",
+				__func__, __LINE__);
+			ret = -EINVAL;
+			goto done;
+		}
+
+		if (barker_read_from_device != BARKER_NUMBER) {
+			KDEBUG(M4SH_NOTICE,
+				"Barker Number read 0x%8x does not match 0x%8x\n",
+				barker_read_from_device, BARKER_NUMBER);
+			KDEBUG(M4SH_NOTICE,
+				"forcing firmware update from file\n");
+		} else {
+			/* Read firmware version from device */
+			if (m4sensorhub_bl_rm(m4sensorhub, VERSION_ADDRESS,
+					(u8 *)&fw_version_device,
+					sizeof(fw_version_device)) != 0) {
+				KDEBUG(M4SH_ERROR, "%s : %d : Read err\n",
+					__func__, __LINE__);
+				ret = -EINVAL;
+				goto done;
+			}
+
+			if (fw_version_file == fw_version_device) {
+				KDEBUG(M4SH_NOTICE,
+					"Version of firmware on device is 0x%4x\n",
+					fw_version_device);
+				KDEBUG(M4SH_NOTICE,
+					"Firmware on device same as file, not loading firmware.\n");
+				goto done;
+			}
+			/* Print statement below isn't really an ERROR, but
+			 * this ensures it is always printed */
+			KDEBUG(M4SH_ERROR,
+				"Version of firmware on device is 0x%4x\n",
+				fw_version_device);
+			KDEBUG(M4SH_ERROR,
+				"Version of firmware on file is 0x%4x\n",
+				fw_version_file);
+			KDEBUG(M4SH_ERROR,
+				"Firmware on device different from file, updating...\n");
+		}
+	} else {
+		KDEBUG(M4SH_NOTICE, "Version of firmware on file is 0x%4x\n",
+			fw_version_file);
+	}
+
+	/* The flash memory to update has to be erased before updating */
+	ret = m4sensorhub_bl_erase_fw(m4sensorhub);
+	if (ret < 0) {
+		pr_err("erase failed\n");
+		/* TODO : Not sure if this is critical error
+		goto done; */
+	}
+
+	bytes_left = firmware->size;
+	address_to_write = USER_FLASH_FIRST_PAGE_ADDRESS;
+	buf_to_read = (u8 *)firmware->data;
+
+	KDEBUG(M4SH_DEBUG, "%s: %d bytes to be written\n", __func__,
+		firmware->size);
+
+	while (bytes_left) {
+		if (bytes_left > MAX_TRANSFER_SIZE)
+			bytes_to_write = MAX_TRANSFER_SIZE;
+		else
+			bytes_to_write = bytes_left;
+
+		if (m4sensorhub_bl_wm(m4sensorhub, address_to_write,
+				buf_to_read, bytes_to_write) != 0) {
+			KDEBUG(M4SH_ERROR, "%s : %d : error writing\n",
+				__func__, __LINE__);
+			ret = -EINVAL;
+			goto done;
+		}
+
+		/* Relying on CRC to take care of errors */
+		address_to_write += bytes_to_write;
+		buf_to_read += bytes_to_write;
+		bytes_left -= bytes_to_write;
+	}
+
+	/* Write barker number when firmware successfully written */
+	buf[0] = BARKER_NUMBER & 0xFF;
+	buf[1] = (BARKER_NUMBER >> 8) & 0xFF;
+	buf[2] = (BARKER_NUMBER >> 16) & 0xFF;
+	buf[3] = (BARKER_NUMBER >> 24) & 0xFF;
+	if (m4sensorhub_bl_wm(m4sensorhub, BARKER_ADDRESS,
+		buf, 4) != 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : error writing\n",
+			__func__, __LINE__);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	KDEBUG(M4SH_NOTICE, "%s: %d bytes written successfully\n",
+		__func__, firmware->size);
+
+	ret = 0;
+
+done:
+	release_firmware(firmware);
+
+	/* If ret is invalid, then we don't try to jump to user code */
+	if (ret >= 0 && m4sensorhub_jump_to_user(m4sensorhub) < 0)
+		/* If jump to user code fails, return failure */
+		ret = -EINVAL;
+
+	kfree(buf);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(m4sensorhub_401_load_firmware);
+
+
+/* -------------- Local Functions ----------------- */
+/* m4sensorhub_bl_ack()
+
+   Receive next byte from M4 and wait to get either NACK or ACK.
+   if M4 sent back ACK, then return 1, else return 0.
+
+     m4sensorhub - pointer to the main m4sensorhub data struct
+*/
+static int m4sensorhub_bl_ack(struct m4sensorhub_data *m4sensorhub)
+{
+	u8 buf_ack = 0;
+	int ret = 0;
+	int timeout = 0;
+
+	for (timeout = 0; timeout <= MAX_ATTEMPTS; timeout++) {
+		if (m4sensorhub_i2c_write_read(m4sensorhub,
+				&buf_ack, 0, 1) < 0) {
+			KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+				__func__, __LINE__);
+			break;
+		}
+		/* Poll for ACK or NACK if M4 says its BUSY */
+		if (buf_ack == BUSY) {
+			KDEBUG(M4SH_DEBUG, "%s : %d : got BUSY from M4\n",
+				__func__, __LINE__);
+			msleep(100);
+		} else if (buf_ack == ACK) {
+			KDEBUG(M4SH_DEBUG, "%s : %d : got ACK from M4\n",
+				__func__, __LINE__);
+			ret = 1;
+			break;
+		} else if (buf_ack == NACK) {
+			break;
+		} else {
+			KDEBUG(M4SH_ERROR, "%s : %d : unexpected response\n",
+				__func__, __LINE__);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+/* m4sensorhub_bl_rm()
+
+   Read data from M4 FLASH.
+
+   Returns 0 on success or negative error code on failure
+
+     m4sensorhub - pointer to the main m4sensorhub data struct
+     start_address - address to read data from
+     data - pointer of buffer to write read data
+     len - length of data to read
+*/
+static int m4sensorhub_bl_rm(struct m4sensorhub_data *m4sensorhub,
+	int start_address, u8 *data, int len)
+{
+	u8 buf[5] = {0};
+	int num_bytes = 0;
+
+	if ((data == NULL) || (len < 0) || (len > MAX_TRANSFER_SIZE)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : Invalid inputs\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	buf[num_bytes++] = OPC_RM;
+	buf[num_bytes++] = ~(OPC_RM);
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf, num_bytes, 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, command invalid\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	num_bytes = 0;
+	buf[num_bytes++] = (start_address >> 24) & 0xFF;
+	buf[num_bytes++] = (start_address >> 16) & 0xFF;
+	buf[num_bytes++] = (start_address >> 8) & 0xFF;
+	buf[num_bytes++] = start_address & 0xFF;
+	buf[num_bytes++] = buf[0] ^ buf[1] ^ buf[2] ^ buf[3];
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf, num_bytes, 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, address invalid\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	num_bytes = 0;
+	buf[num_bytes++] = (len - 1);
+	buf[num_bytes++] = (len - 1) ^ 0xFF;
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf, num_bytes, 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, data, 0, len) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	return 0;
+}
+
+/* m4sensorhub_bl_go()
+
+   Cause M4 to jump to new address and start execution fresh.
+
+   Returns 0 on success or negative error code on failure
+
+     m4sensorhub - pointer to the main m4sensorhub data struct
+     start_address - address to jump M4 code to
+*/
+static int m4sensorhub_bl_go(struct m4sensorhub_data *m4sensorhub,
+	int start_address)
+{
+	u8 buf[5] = {0};
+	int num_bytes = 0;
+
+	buf[num_bytes++] = OPC_GO;
+	buf[num_bytes++] = ~(OPC_GO);
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf, num_bytes, 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, command invalid\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	num_bytes = 0;
+	buf[num_bytes++] = (start_address >> 24) & 0xFF;
+	buf[num_bytes++] = (start_address >> 16) & 0xFF;
+	buf[num_bytes++] = (start_address >> 8) & 0xFF;
+	buf[num_bytes++] = start_address & 0xFF;
+	buf[num_bytes++] = buf[0] ^ buf[1] ^ buf[2] ^ buf[3];
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf, num_bytes, 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, address invalid\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	return 0;
+}
+
+/* m4sensorhub_bl_wm()
+
+   Write data to M4 FLASH.
+
+   Returns 0 on success or negative error code on failure
+
+     m4sensorhub - pointer to the main m4sensorhub data struct
+     start_address - address to write data to
+     data - pointer of data to write
+     len - length of data to write, must be 16-bit aligned
+*/
+static int m4sensorhub_bl_wm(struct m4sensorhub_data *m4sensorhub,
+	int start_address, u8 *data, int len)
+{
+	u8 buf[5] = {0};
+	u8 *temp_buf = NULL;
+	u8 checksum = 0;
+	int i, retval = -1;
+	int malloc_size = 0;
+	int num_bytes = 0;
+
+	if ((data == NULL) || (len < 0) || (len > MAX_TRANSFER_SIZE)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : Invalid inputs\n",
+			__func__, __LINE__);
+		goto done;
+	}
+
+	/* add 2 bytes for data, one for length and last for checksum
+	as per spec, data is 16 bit aligned, padding of 0xFF if needed */
+	if (len % 2)
+		malloc_size = len + 1 + 2;
+	else
+		malloc_size = len + 2;
+
+
+	temp_buf = kzalloc(malloc_size, GFP_KERNEL);
+	if (!temp_buf) {
+		pr_err("Can't allocate memory %s\n", __func__);
+		retval = -ENOMEM;
+		goto done;
+	}
+
+	buf[num_bytes++] = OPC_WM;
+	buf[num_bytes++] = ~(OPC_WM);
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf, num_bytes, 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		goto done;
+	}
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, command invalid\n",
+			__func__, __LINE__);
+		goto done;
+	}
+
+	num_bytes = 0;
+	checksum = 0;
+
+	buf[num_bytes++] = (start_address >> 24) & 0xFF;
+	checksum = checksum ^ ((start_address >> 24) & 0xFF);
+	buf[num_bytes++] = (start_address >> 16) & 0xFF;
+	checksum = checksum ^ ((start_address >> 16) & 0xFF);
+	buf[num_bytes++] = (start_address >> 8) & 0xFF;
+	checksum = checksum ^ ((start_address >> 8) & 0xFF);
+	buf[num_bytes++] = start_address & 0xFF;
+	checksum = checksum ^ ((start_address) & 0xFF);
+	buf[num_bytes++] = checksum;
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf, num_bytes, 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		goto done;
+	}
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, address invalid\n",
+			__func__, __LINE__);
+		goto done;
+	}
+
+
+	checksum = 0;
+	num_bytes = 0;
+	/* (malloc_size -2 - 1) is the number of data bytes -1 to be sent */
+	checksum = checksum ^ (malloc_size - 2 - 1);
+	/* Calculate checksum of data */
+	for (i = 0; i < len; i++)
+		checksum ^= data[i];
+
+	if (len % 2)
+		checksum ^= 0xFF;
+
+	temp_buf[num_bytes++] = malloc_size - 2 - 1;
+	memcpy(&temp_buf[num_bytes], data, len);
+	if (len % 2)
+		temp_buf[len + 1] = 0xFF;
+	temp_buf[malloc_size-1] = checksum;
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, temp_buf,
+			malloc_size, 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		goto done;
+	}
+
+	usleep_range(10000, 12000);
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, data invalid\n",
+			__func__, __LINE__);
+		goto done;
+	}
+
+	retval = 0;
+
+done:
+	kfree(temp_buf);
+
+	return retval;
+}
+
+/* m4sensorhub_bl_erase_fw()
+
+   Erases the M4 FLASH that is to be used for the firmware.
+
+   Returns 0 on success or negative error code on failure
+
+     m4sensorhub - pointer to the main m4sensorhub data struct
+*/
+static int m4sensorhub_bl_erase_fw(struct m4sensorhub_data *m4sensorhub)
+{
+	u8 buf[2*NUM_FLASH_TO_ERASE + 1] = {0};
+	u8 checksum = 0;
+	int i;
+	int num_bytes = 0;
+
+	buf[num_bytes++] = OPC_ER;
+	buf[num_bytes++] = ~(OPC_ER);
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf, num_bytes, 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, command invalid\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	num_bytes = 0;
+	buf[num_bytes++] = ((NUM_FLASH_TO_ERASE - 1) >> 8) & 0xFF;
+	buf[num_bytes++] = ((NUM_FLASH_TO_ERASE - 1) & 0xFF);
+	buf[num_bytes++] = buf[0] ^ buf[1];
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf, num_bytes,
+			0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, command invalid\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	checksum = 0;
+	for (i = 0; i < NUM_FLASH_TO_ERASE; i++) {
+		buf[2*i] = (page_to_erase[i] >> 8) & 0xFF;
+		buf[2*i+1] = page_to_erase[i] & 0xFF;
+		checksum ^= buf[2*i];
+		checksum ^= buf[2*i+1];
+	}
+	buf[2*NUM_FLASH_TO_ERASE] = checksum;
+
+	if (m4sensorhub_i2c_write_read(m4sensorhub, buf,
+			(2*NUM_FLASH_TO_ERASE + 1), 0) < 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : I2C transfer error\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	/* TODO : this sleep isn't needed with v1.1 protocol*/
+	msleep(2000);
+
+	if (!m4sensorhub_bl_ack(m4sensorhub)) {
+		KDEBUG(M4SH_ERROR, "%s : %d : NACK received, command invalid\n",
+			__func__, __LINE__);
+		return -1;
+	}
+
+	return 0;
+}
+
+/* m4sensorhub_jump_to_user()
+
+   Jump to user code loaded in M4 FLASH.
+
+   Returns 0 on success or negative error code on failure
+
+     m4sensorhub - pointer to the main m4sensorhub data struct
+*/
+static int m4sensorhub_jump_to_user(struct m4sensorhub_data *m4sensorhub)
+{
+	int ret = -1;
+	u32 barker_read_from_device = 0;
+
+	if (m4sensorhub_bl_rm(m4sensorhub, BARKER_ADDRESS,
+	    (u8 *)&barker_read_from_device, 4) != 0) {
+		KDEBUG(M4SH_ERROR, "%s : %d : error reading from device\n",
+		       __func__, __LINE__);
+	}
+
+	if (barker_read_from_device == BARKER_NUMBER) {
+		m4sensorhub_bl_go(m4sensorhub, USER_FLASH_FIRST_PAGE_ADDRESS);
+		KDEBUG(M4SH_NOTICE, "Executing M4 code\n");
+		ret = 0;
+	} else {
+		KDEBUG(M4SH_ERROR, "Wrong Barker Number read 0x%8x\n",
+		       barker_read_from_device);
+		KDEBUG(M4SH_ERROR, "*** Not executing M4 code ***\n");
+	}
+
+	return ret;
+}
diff --git a/include/linux/m4sensorhub.h b/include/linux/m4sensorhub.h
index 717e88b37a3..d2a4a094715 100644
--- a/include/linux/m4sensorhub.h
+++ b/include/linux/m4sensorhub.h
@@ -170,6 +170,12 @@ int m4sensorhub_load_firmware(struct m4sensorhub_data *m4sensorhub,
 	unsigned short force_upgrade,
 	const struct firmware *firmware);
 
+int m4sensorhub_401_load_firmware(struct m4sensorhub_data *m4sensorhub,
+	unsigned short force_upgrade,
+	const struct firmware *fm);
+
+void m4sensorhub_hw_reset(struct m4sensorhub_data *m4sensorhub);
+
 /* Interrupt handler */
 int m4sensorhub_irq_init(struct m4sensorhub_data *m4sensorhub);
 void m4sensorhub_irq_shutdown(struct m4sensorhub_data *m4sensorhub);