3 files changed, 73 insertions, 45 deletions

diff --git a/drivers/mfd/m4sensorhub-core.c b/drivers/mfd/m4sensorhub-core.c
index 9053bfbba45..3e7cad67d3c 100644
--- a/drivers/mfd/m4sensorhub-core.c
+++ b/drivers/mfd/m4sensorhub-core.c
@@ -61,7 +61,6 @@ static struct m4sensorhub_data m4sensorhub_misc_data;
 static DEFINE_MUTEX(m4sensorhub_driver_lock);
 static struct init_call *inithead;
 static struct init_call *preflash_head;
-static int firmware_download_status = -1;
 static char tcmd_exec_status;
 
 unsigned short force_upgrade;
@@ -459,8 +458,6 @@ static void m4sensorhub_initialize(const struct firmware *firmware,
 	int err = 0;
 	struct init_call *inc, *prev;
 	struct init_calldata arg;
-	int i;
-	uint16_t version;
 
 	if (firmware == NULL) {
 		KDEBUG(M4SH_ERROR, "%s: No firmware data recieved\n",
@@ -468,46 +465,30 @@ static void m4sensorhub_initialize(const struct firmware *firmware,
 		return;
 	}
 
-	/* initiate m4 firmware download */
-	for (i = 0; i < 3; i++) {
-		KDEBUG(M4SH_CRITICAL, "%s: Starting M4 download %s = %d\n",
-			__func__, "with force_upgrade", 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, "%s: %s = %d\n",
-				__func__, "Failed to load M4 firmware", err);
-			/* Since download failed, return M4 to boot mode */
-			m4sensorhub_hw_reset(&m4sensorhub_misc_data);
-			return;
-		}
-
-		/* Wait progressively longer for M4 to become ready */
-		if (i > 0)
-			msleep(i * 100);
+	/* Initiate M4 firmware download */
+	KDEBUG(M4SH_CRITICAL, "%s: Starting M4 download %s = %d\n",
+		__func__, "with force_upgrade", force_upgrade);
+	if (m4sensorhub_misc_data.i2c_client->addr == 0x39)
+		err = m4sensorhub_401_load_firmware(&m4sensorhub_misc_data,
+			force_upgrade, firmware);
+	else
+		err = m4sensorhub_load_firmware(&m4sensorhub_misc_data,
+			force_upgrade, firmware);
 
-		/* Read M4 register to test if M4 is ready */
-		err = m4sensorhub_reg_read(&m4sensorhub_misc_data,
-			M4SH_REG_GENERAL_VERSION, (char *)&version);
-		if (err < 0)
-			KDEBUG(M4SH_ERROR, "%s: %s (retries=%d).\n", __func__,
-				"Failed initial I2C read", i);
-		else  /* No failure */
-			break;
+	if (err < 0) {
+		KDEBUG(M4SH_ERROR, "%s: %s = %d\n",
+			__func__, "Failed to load M4 firmware", err);
+		/* Since download failed, return M4 to boot mode */
+		m4sensorhub_hw_reset(&m4sensorhub_misc_data);
+		return;
 	}
 
-	/* Check if we actually finished the loop */
-	if (i >= 3)
-		panic("%s: M4 has failed--forcing panic...\n",
-			__func__);
+	err = m4sensorhub_test_m4_reboot(&m4sensorhub_misc_data, false);
+	if (err < 0) {
+		/* Getting here is unlikely because failures normally panic */
+		KDEBUG(M4SH_ERROR, "%s: Testing M4 reboot failed.\n", __func__);
+		return;
+	}
 
 	err = m4sensorhub_irq_init(&m4sensorhub_misc_data);
 	if (err < 0) {
diff --git a/drivers/mfd/m4sensorhub-panic.c b/drivers/mfd/m4sensorhub-panic.c
index 5609492a485..b127adeba5c 100644
--- a/drivers/mfd/m4sensorhub-panic.c
+++ b/drivers/mfd/m4sensorhub-panic.c
@@ -294,11 +294,8 @@ void m4sensorhub_panic_process(struct m4sensorhub_data *m4sensorhub)
 	}
 
 	KDEBUG(M4SH_ERROR, "%s: Detected M4 panic, reset M4!\n", __func__);
-	msleep(100);
-	if (m4sensorhub->i2c_client->addr == 0x39)
-		ret = m4sensorhub_401_load_firmware(m4sensorhub, 0, NULL);
-	else
-		ret = m4sensorhub_load_firmware(m4sensorhub, 0, NULL);
+	/* Passing "true" will reset M4 before trying to communicate */
+	ret = m4sensorhub_test_m4_reboot(m4sensorhub, true);
 	if (ret < 0) {
 		KDEBUG(M4SH_ERROR, "%s: Failed to restart M4, ret = %d\n",
 			__func__, ret);
diff --git a/drivers/mfd/m4sensorhub-stm32_401-fw.c b/drivers/mfd/m4sensorhub-stm32_401-fw.c
index 2998546a3a8..61926c29068 100644
--- a/drivers/mfd/m4sensorhub-stm32_401-fw.c
+++ b/drivers/mfd/m4sensorhub-stm32_401-fw.c
@@ -333,6 +333,56 @@ done:
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_401_load_firmware);
 
+/* TODO: Restructure reflash code so that this function can go in the core */
+int m4sensorhub_test_m4_reboot(struct m4sensorhub_data *m4, bool reboot_first)
+{
+	int err = 0;
+	int i;
+	uint16_t version;
+
+	if (m4 == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is missing.\n", __func__);
+		err = -ENODATA;
+		goto m4sensorhub_test_m4_reboot_exit;
+	}
+
+	for (i = 0; i < 3; i++) {
+		if ((i > 0) || (reboot_first)) {
+			m4sensorhub_hw_reset(m4);
+			err = m4sensorhub_jump_to_user(m4);
+			if (err < 0) {
+				KDEBUG(M4SH_ERROR,
+					"%s: M4 reboot failed (retries=%d)\n",
+					__func__, i);
+				continue;
+			}
+		}
+
+		/* Wait progressively longer for M4 to become ready */
+		if (i > 0)
+			msleep(i * 100);
+
+		/* Read M4 register to test if M4 is ready */
+		err = m4sensorhub_reg_read(m4, M4SH_REG_GENERAL_VERSION,
+			(char *)&version);
+		if (err < 0) {
+			KDEBUG(M4SH_ERROR, "%s: %s (retries=%d).\n", __func__,
+				"Failed initial I2C read", i);
+			continue;
+		} else {
+			/* No failure so break loop */
+			err = 0;
+			break;
+		}
+	}
+
+	if (err < 0)
+		panic("%s: M4 has failed--forcing panic...\n", __func__);
+
+m4sensorhub_test_m4_reboot_exit:
+	return err;
+}
+EXPORT_SYMBOL_GPL(m4sensorhub_test_m4_reboot);
 
 /* -------------- Local Functions ----------------- */
 /* m4sensorhub_bl_ack()