IKXCLOCK-2835 Fix M4 Panic Recovery

Reduced the retry logic to only do a reboot and jump-to-user.
Updated the panic handler to use the same call as m4sensorhub_initialize.
Removed an unused static variable in the M4 core.

Change-Id: I246e220540a4c02939c0ac435b670529d85c8f27
Signed-off-by: Eric Tashakkor <w36098@motorola.com>

4 files changed, 82 insertions, 47 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()
diff --git a/include/linux/m4sensorhub.h b/include/linux/m4sensorhub.h
index ff41f8dc33f..bd4cf0c24e6 100644
--- a/include/linux/m4sensorhub.h
+++ b/include/linux/m4sensorhub.h
@@ -170,14 +170,19 @@ void m4sensorhub_reg_access_unlock(void);
 int m4sensorhub_i2c_write_read(struct m4sensorhub_data *m4sensorhub,
 				      u8 *buf, int writelen, int readlen);
 
+/*
+ * Functions for re-initializing M4
+ *
+ * In general, only m4sensorhub_test_m4_reboot() should ever
+ *   be called directly.
+ */
+int m4sensorhub_test_m4_reboot(struct m4sensorhub_data *m4, bool reboot_first);
 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 */
@@ -198,6 +203,7 @@ int m4sensorhub_irq_enable_get(struct m4sensorhub_data *m4sensorhub,
 void m4sensorhub_irq_pm_dbg_suspend(void);
 void m4sensorhub_irq_pm_dbg_resume(void);
 
+/* M4 Panic Calls */
 int m4sensorhub_panic_init(struct m4sensorhub_data *m4sensorhub);
 void m4sensorhub_panic_shutdown(struct m4sensorhub_data *m4sensorhub);
 int m4sensorhub_panic_register(struct m4sensorhub_data *m4sensorhub,
@@ -207,6 +213,7 @@ int m4sensorhub_panic_register(struct m4sensorhub_data *m4sensorhub,
 int m4sensorhub_panic_unregister(struct m4sensorhub_data *m4sensorhub,
 				enum m4sensorhub_panichdl_index index);
 void m4sensorhub_panic_process(struct m4sensorhub_data *m4sensorhub);
+
 /* all M4 based drivers need to register an init call with the core,
  this callback will be executed once M4 core has properly set up FW
  on M4. For registration, a callback and a void* is passed in. When