IKXCLOCK-1703 Add M4 Pre-Flash Callback for RTC

Added a pre-flash callback system for drivers with timing restrictions like RTC
Updated the RTC driver to save and use set time requests until M4 is ready
Fixed a bug where uninitialized memory was preventing the time being set
Minor clean up of driver files to meet checkpatch style requirements

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

4 files changed, 320 insertions, 91 deletions

diff --git a/drivers/mfd/m4sensorhub-core.c b/drivers/mfd/m4sensorhub-core.c
index d93432d63f1..a1ae596bf4c 100644
--- a/drivers/mfd/m4sensorhub-core.c
+++ b/drivers/mfd/m4sensorhub-core.c
@@ -60,6 +60,7 @@ struct init_call {
 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;
 
@@ -69,8 +70,8 @@ MODULE_PARM_DESC(force_upgrade, "Force FW download ignoring version check");
 
 unsigned short debug_level;
 module_param(debug_level, short, 0644);
-MODULE_PARM_DESC(debug_level, "Set debug level 1 (CRITICAL) to "
-				"7 (VERBOSE_DEBUG)");
+MODULE_PARM_DESC(debug_level,
+	"Set debug level 1 (CRITICAL) to 7 (VERBOSE_DEBUG)");
 
 /* -------------- Global Functions ----------------- */
 struct m4sensorhub_data *m4sensorhub_client_get_drvdata(void)
@@ -366,6 +367,89 @@ void m4sensorhub_unregister_initcall(int(*initfunc)(struct init_calldata *))
 	}
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_unregister_initcall);
+
+int m4sensorhub_register_preflash_callback(
+		int(*initfunc)(struct init_calldata *), void *pdata)
+{
+	struct init_call *inc = NULL;
+
+	inc = kzalloc(sizeof(struct init_call), GFP_KERNEL);
+	if (inc == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: Unable to allocate init call mem\n",
+			__func__);
+		return -ENOMEM;
+	}
+
+	inc->initcb = initfunc;
+	inc->pdata = pdata;
+	/* add it to the list */
+	if (preflash_head == NULL)
+		inc->next = NULL;
+	else
+		inc->next = preflash_head;
+
+	preflash_head = inc;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(m4sensorhub_register_preflash_callback);
+
+void m4sensorhub_unregister_preflash_callback(
+		int(*initfunc)(struct init_calldata *))
+{
+	struct init_call *node = preflash_head;
+	struct init_call *prev;
+
+	for (node = preflash_head, prev = NULL;
+		node != NULL;
+		prev = node, node = node->next) {
+		if (node->initcb == initfunc) {
+			/* remove this node */
+			if (node == preflash_head)
+				preflash_head = node->next;
+			else
+				prev->next = node->next;
+			kfree(node);
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(m4sensorhub_unregister_preflash_callback);
+
+void m4sensorhub_call_preflash_callbacks(void)
+{
+	struct init_calldata arg;
+	struct init_call *inc = NULL;
+	struct init_call *prev = NULL;
+	int err = 0;
+
+	/* Call any registered preflash callbacks */
+	inc = preflash_head;
+	arg.p_m4sensorhub_data = &m4sensorhub_misc_data;
+	prev = NULL;
+	while (inc) {
+		arg.p_data = inc->pdata;
+		err = inc->initcb(&arg);
+		if (err < 0) {
+			KDEBUG(M4SH_ERROR,
+				"%s: Callback failed with error code %d\n",
+				__func__, err);
+		}
+		prev = inc;
+		inc = inc->next;
+		kfree(prev);
+	}
+
+	return;
+}
+EXPORT_SYMBOL_GPL(m4sensorhub_call_preflash_callbacks);
+
+bool m4sensorhub_preflash_callbacks_exist(void)
+{
+	if (preflash_head != NULL)
+		return true;
+	else
+		return false;
+}
+EXPORT_SYMBOL_GPL(m4sensorhub_preflash_callbacks_exist);
 /* END BOARD FILE FUNCTIONS */
 
 /* Downloads m4 firmware and also initializes all m4 drivers */
@@ -420,9 +504,8 @@ static void m4sensorhub_initialize(const struct firmware *firmware,
 		err = inc->initcb(&arg);
 		if (err < 0) {
 			KDEBUG(M4SH_ERROR,
-				"%s: Callback failed with error code %d %s\n",
-				__func__, err, "(dumping stack)");
-			dump_stack();
+				"%s: Callback failed with error code %d\n",
+				__func__, err);
 		}
 		prev = inc;
 		inc = inc->next;
@@ -439,7 +522,7 @@ static ssize_t m4sensorhub_set_dbg(struct device *dev,
 {
 	unsigned long debug;
 
-	if ((strict_strtol(buf, 10, &debug) < 0) ||
+	if ((kstrtol(buf, 10, &debug) < 0) ||
 	    (debug < M4SH_NODEBUG) || (debug > M4SH_VERBOSE_DEBUG))
 		return -EINVAL;
 
@@ -463,8 +546,8 @@ static ssize_t m4sensorhub_get_loglevel(struct device *dev,
 	KDEBUG(M4SH_INFO, "M4 loglevel = %llx", loglevel);
 	return sprintf(buf, "%llu\n", loglevel);
 }
-void ParseAndUpdateLogLevels(char *tag, char *level,
-			unsigned long long *logLevels)
+void m4sensorhub_update_loglevels(char *tag, char *level,
+			unsigned long long *log_levels)
 {
 	int i;
 	int levelindex = -1;
@@ -490,10 +573,10 @@ void ParseAndUpdateLogLevels(char *tag, char *level,
 
 	/*Clear the revelant bits*/
 	mask = 0x03;
-	*logLevels &= ~(mask << (tagindex * 2));
+	*log_levels &= ~(mask << (tagindex * 2));
 	/*set debug level for the relevant bits*/
-	*logLevels |= (levelindex << (tagindex * 2));
-	KDEBUG(M4SH_INFO, "New M4 log levels = 0x%llx\n", *logLevels);
+	*log_levels |= (levelindex << (tagindex * 2));
+	KDEBUG(M4SH_INFO, "New M4 log levels = 0x%llx\n", *log_levels);
 }
 
 /* Usage: adb shell into the directory of sysinterface log_level and
@@ -501,12 +584,12 @@ void ParseAndUpdateLogLevels(char *tag, char *level,
 static ssize_t m4sensorhub_set_loglevel(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
-	unsigned long long currentLogLevels;
+	unsigned long long cur_loglevels;
 	char *tag, *level;
 	char **logbuf = (char **) &buf;
 
 	m4sensorhub_reg_read(&m4sensorhub_misc_data,
-		M4SH_REG_LOG_LOGENABLE, (char *)&currentLogLevels);
+		M4SH_REG_LOG_LOGENABLE, (char *)&cur_loglevels);
 	while (1) {
 		tag = strsep(logbuf, "=,\n ");
 		if (tag == NULL)
@@ -514,11 +597,11 @@ static ssize_t m4sensorhub_set_loglevel(struct device *dev,
 		level = strsep(logbuf, "=,\n ");
 		if (level == NULL)
 			break;
-		ParseAndUpdateLogLevels(tag, level, &currentLogLevels);
+		m4sensorhub_update_loglevels(tag, level, &cur_loglevels);
 	}
 
 	return m4sensorhub_reg_write(&m4sensorhub_misc_data,
-		M4SH_REG_LOG_LOGENABLE, (char *)&currentLogLevels,
+		M4SH_REG_LOG_LOGENABLE, (char *)&cur_loglevels,
 		m4sh_no_mask);
 }
 
@@ -656,10 +739,11 @@ static ssize_t m4sensorhub_raw_i2c(struct device *dev,
 		buffer[1] = (addr & 0xFF);
 		ret = m4sensorhub_i2c_write_read(&m4sensorhub_misc_data,
 						 buffer, 2, len);
-		if (ret != len)
-			KDEBUG(M4SH_ERROR, "Failed to read from bank 0x%x \
-			       addr 0x%x", bank, addr);
-		else {
+		if (ret != len) {
+			KDEBUG(M4SH_ERROR,
+				"Failed to read from bank 0x%x addr 0x%x",
+				bank, addr);
+		} else {
 			for (i = 0; i < len; i++)
 				KDEBUG(M4SH_INFO, "0x%x ",
 				       (unsigned char)buffer[i]);
@@ -673,9 +757,11 @@ static ssize_t m4sensorhub_raw_i2c(struct device *dev,
 		buffer[2] = (len & 0xFF);
 		ret = m4sensorhub_i2c_write_read(&m4sensorhub_misc_data,
 						 buffer, 3, 0);
-		if (ret != 1)
-			KDEBUG(M4SH_ERROR, "Failed to write 0x%x from bank \
-			       0x%x addr 0x%x", len, bank, addr);
+		if (ret != 1) {
+			KDEBUG(M4SH_ERROR,
+				"Failed to write 0x%x from bank 0x%x addr 0x%x",
+				len, bank, addr);
+		}
 	} else {
 		KDEBUG(M4SH_ERROR, "Unknown operation = %c", operation);
 	}
diff --git a/drivers/mfd/m4sensorhub-stm32_401-fw.c b/drivers/mfd/m4sensorhub-stm32_401-fw.c
index 73ae3a31519..2998546a3a8 100644
--- a/drivers/mfd/m4sensorhub-stm32_401-fw.c
+++ b/drivers/mfd/m4sensorhub-stm32_401-fw.c
@@ -190,6 +190,11 @@ int m4sensorhub_401_load_firmware(struct m4sensorhub_data *m4sensorhub,
 				barker_read_from_device, BARKER_NUMBER);
 			KDEBUG(M4SH_NOTICE,
 				"forcing firmware update from file\n");
+			/*
+			 * This is likely a blank flash factory case, so
+			 * skip doing any driver pre-flash callbacks.
+			 */
+			goto m4sensorhub_401_load_firmware_erase_flash;
 		} else {
 			/* Read firmware version from device */
 			if (m4sensorhub_bl_rm(m4sensorhub, VERSION_ADDRESS,
@@ -223,9 +228,41 @@ int m4sensorhub_401_load_firmware(struct m4sensorhub_data *m4sensorhub,
 	} else {
 		KDEBUG(M4SH_NOTICE, "Version of firmware on file is 0x%04x\n",
 			fw_version_file);
+		/*
+		 * Currently no code uses the force_upgrade path,
+		 * but in case it is used for some recovery (and because
+		 * no numbers or error checking is done), we will skip
+		 * trying to call any driver pre-flash callbacks.
+		 */
+		goto m4sensorhub_401_load_firmware_erase_flash;
+	}
+
+	/* Boot M4, execute any pre-flash callbacks, then reset for BL mode */
+	if (m4sensorhub_preflash_callbacks_exist()) {
+		KDEBUG(M4SH_ERROR, "%s: Booting M4 to execute callbacks...\n",
+			__func__); /* Not an error (see similar above) */
+		ret = m4sensorhub_jump_to_user(m4sensorhub);
+		if (ret < 0) {
+			KDEBUG(M4SH_ERROR, "%s: %s %s %d.\n", __func__,
+				"Failed to boot M4 for callbacks",
+				"with error code", ret);
+			/*
+			 * Since we don't know the status of M4 at this point,
+			 * we will skip any callbacks, reset the IC to a known
+			 * state, and go ahead with a reflash.
+			 */
+			m4sensorhub_hw_reset(m4sensorhub);
+			goto m4sensorhub_401_load_firmware_erase_flash;
+		}
+
+		m4sensorhub_call_preflash_callbacks();
+		KDEBUG(M4SH_ERROR, "%s: Callbacks complete, flashing M4...\n",
+			__func__); /* Not an error (see similar above) */
+		m4sensorhub_hw_reset(m4sensorhub);
 	}
 
 	/* The flash memory to update has to be erased before updating */
+m4sensorhub_401_load_firmware_erase_flash:
 	ret = m4sensorhub_bl_erase_fw(m4sensorhub);
 	if (ret < 0) {
 		pr_err("%s: erase failed\n", __func__);
diff --git a/drivers/rtc/rtc-sensorhub.c b/drivers/rtc/rtc-sensorhub.c
index 4939ff8874f..c57d7ea95a5 100644
--- a/drivers/rtc/rtc-sensorhub.c
+++ b/drivers/rtc/rtc-sensorhub.c
@@ -22,9 +22,14 @@
 #define DRIVER_NAME "rtc-sensorhub"
 
 struct rtc_sensorhub_private_data {
-	struct rtc_device *p_rtc;
-	struct m4sensorhub_data *p_m4sensorhub_data;
-	struct rtc_wkalrm next_alarm_set;
+	struct rtc_device         *p_rtc;
+	struct m4sensorhub_data   *p_m4sensorhub_data;
+	struct rtc_wkalrm         next_alarm_set;
+
+	unsigned long   m4_seconds_cached;
+	unsigned long   m4_boot_cached;
+	unsigned long   sys_seconds_cached;
+	unsigned long   sys_boot_cached;
 };
 
 static int rtc_sensorhub_rtc_alarm_irq_enable(struct device *p_dev,
@@ -38,19 +43,22 @@ static int rtc_sensorhub_rtc_alarm_irq_enable(struct device *p_dev,
 	dev_dbg(p_dev, "enable is %u\n", enable);
 
 	if (!(p_priv_data->p_m4sensorhub_data)) {
-		dev_err(p_dev, "RTC hardware not ready yet\n");
+		dev_err(p_dev, "Cannot %s alarm--RTC hardware not ready yet\n",
+			(enable ? "set" : "clear"));
 		return -EIO;
 	}
 
-	if (enable == 1)
+	if (enable == 1) {
 		err = m4sensorhub_irq_enable(p_priv_data->p_m4sensorhub_data,
 				M4SH_IRQ_AP_ALARM_EXPIRED);
-	else
+		if (err < 0)
+			dev_err(p_dev, "Enabling IRQ failed (%d)\n", err);
+	} else {
 		err = m4sensorhub_irq_disable(p_priv_data->p_m4sensorhub_data,
 				M4SH_IRQ_AP_ALARM_EXPIRED);
-
-	if (err < 0)
-		dev_err(p_dev, "couldn't enable irq\n");
+		if (err < 0)
+			dev_err(p_dev, "Disabling IRQ failed (%d)\n", err);
+	}
 
 	return err;
 }
@@ -63,13 +71,10 @@ static int rtc_sensorhub_rtc_read_alarm(struct device *p_dev,
 						platform_get_drvdata(p_platdev);
 	struct rtc_time rtc = p_alrm->time;
 
-	memcpy(
-			p_alrm, &(p_priv_data->next_alarm_set),
-			sizeof(struct rtc_wkalrm)
-		);
+	memcpy(p_alrm, &(p_priv_data->next_alarm_set),
+			sizeof(struct rtc_wkalrm));
 
-	dev_info(p_dev, "alarm read for "
-		"%d-%02d-%02d %02d:%02d:%02d UTC\n",
+	dev_info(p_dev, "alarm read for %d-%02d-%02d %02d:%02d:%02d UTC\n",
 		rtc.tm_year + 1900, rtc.tm_mon + 1, rtc.tm_mday,
 		rtc.tm_hour, rtc.tm_min, rtc.tm_sec);
 
@@ -90,8 +95,7 @@ static int rtc_sensorhub_rtc_set_alarm(struct device *p_dev,
 	unsigned long requested_time, time_diff;
 	int ret;
 
-	dev_info(p_dev, "alarm requested for "
-		"%d-%02d-%02d %02d:%02d:%02d UTC\n",
+	dev_info(p_dev, "alarm requested for %d-%02d-%02d %02d:%02d:%02d UTC\n",
 		rtc.tm_year + 1900, rtc.tm_mon + 1, rtc.tm_mday,
 		rtc.tm_hour, rtc.tm_min, rtc.tm_sec);
 
@@ -111,18 +115,16 @@ static int rtc_sensorhub_rtc_set_alarm(struct device *p_dev,
 
 	time_diff = requested_time - tv_current.tv_sec;
 	if (time_diff >= SECONDS_IN_DAY || time_diff <= 0) {
-		dev_err(p_dev, "requested alarm out of range, rejecting alarm\n");
+		dev_err(p_dev,
+			"requested alarm out of range, rejecting alarm\n");
 		return -EINVAL;
 	}
 
-	if (m4sensorhub_reg_getsize(
-				p_m4_drvdata,
-				M4SH_REG_GENERAL_APALARM
-			) != m4sensorhub_reg_write(
-		p_m4_drvdata, M4SH_REG_GENERAL_APALARM,
+	if (m4sensorhub_reg_getsize(p_m4_drvdata, M4SH_REG_GENERAL_APALARM) !=
+		m4sensorhub_reg_write(p_m4_drvdata, M4SH_REG_GENERAL_APALARM,
 		(char *)&time_diff, m4sh_no_mask)) {
-			dev_err(p_dev, "Failed to set M4 alarm!\n");
-			return -EIO;
+		dev_err(p_dev, "Failed to set M4 alarm!\n");
+		return -EIO;
 	}
 
 	ret = rtc_sensorhub_rtc_alarm_irq_enable(p_dev, p_alrm->enabled);
@@ -132,23 +134,20 @@ static int rtc_sensorhub_rtc_set_alarm(struct device *p_dev,
 	}
 
 	/* Store the info abt this alarm in our local datastructure */
-	memcpy(
-			&(p_priv_data->next_alarm_set), p_alrm,
-						sizeof(struct rtc_wkalrm)
-		);
+	memcpy(&(p_priv_data->next_alarm_set), p_alrm,
+		sizeof(struct rtc_wkalrm));
 	return 0;
 }
 
 static int rtc_sensorhub_get_rtc_from_m4(struct rtc_time *p_tm,
 			struct m4sensorhub_data *p_m4_drvdata)
 {
-	u32 seconds;
+	u32 seconds = 0;
 
-	if (m4sensorhub_reg_getsize(p_m4_drvdata,
-		M4SH_REG_GENERAL_UTC) != m4sensorhub_reg_read(
-		p_m4_drvdata, M4SH_REG_GENERAL_UTC,
+	if (m4sensorhub_reg_getsize(p_m4_drvdata, M4SH_REG_GENERAL_UTC) !=
+		m4sensorhub_reg_read(p_m4_drvdata, M4SH_REG_GENERAL_UTC,
 		(char *)&seconds)) {
-		pr_err("%s: Failed get M4 clock!\n", DRIVER_NAME);
+		pr_err("%s: Failed to get M4 clock!\n", DRIVER_NAME);
 		return -EIO;
 	}
 
@@ -159,16 +158,26 @@ static int rtc_sensorhub_get_rtc_from_m4(struct rtc_time *p_tm,
 static int rtc_sensorhub_rtc_read_time(struct device *p_dev,
 	struct rtc_time *p_tm)
 {
-	int err;
+	int err = 0;
 	struct platform_device *p_platdev = to_platform_device(p_dev);
 	struct rtc_sensorhub_private_data *p_priv_data =
 						platform_get_drvdata(p_platdev);
 
 	if (!(p_priv_data->p_m4sensorhub_data)) {
-		dev_err(p_dev, "read time, but RTC hardware not ready\n");
-		/* M4 driver is not yet ready, just give the time since boot
-		and treat boot as start of epoch */
-		rtc_time_to_tm(get_seconds(), p_tm);
+		if ((p_priv_data->sys_seconds_cached != 0) &&
+		(p_priv_data->sys_boot_cached != 0)) {
+			dev_err(p_dev,
+				"Using saved set RTC time (hardware not ready)\n");
+			/* Use saved settime request (will go to M4) */
+			rtc_time_to_tm(p_priv_data->sys_seconds_cached +
+				get_seconds() - p_priv_data->sys_boot_cached, p_tm);
+		} else {
+			dev_err(p_dev,
+				"Using cached M4 RTC time (hardware not ready)\n");
+			/* Use cached time from M4 */
+			rtc_time_to_tm(p_priv_data->m4_seconds_cached +
+				get_seconds() - p_priv_data->m4_boot_cached, p_tm);
+		}
 		return 0;
 	}
 
@@ -181,36 +190,37 @@ static int rtc_sensorhub_rtc_read_time(struct device *p_dev,
 static int rtc_sensorhub_rtc_set_time(struct device *p_dev,
 	struct rtc_time *p_tm)
 {
-	unsigned long sec;
+	unsigned long sec = 0;
 	struct platform_device *p_platdev = to_platform_device(p_dev);
 	struct rtc_sensorhub_private_data *p_priv_data =
 						platform_get_drvdata(p_platdev);
 	struct m4sensorhub_data *p_m4_drvdata =
 			p_priv_data->p_m4sensorhub_data;
 
-	if (!(p_m4_drvdata)) {
-		dev_err(p_dev, "set time, but M4 not ready, ignore func call\n");
-		return 0;
-	}
-
 	/* M4 expects the UTC time in seconds from Jan 1, 1970,
 	basically epoch_time in seconds */
 	rtc_tm_to_time(p_tm, &sec);
 
+	if (!(p_m4_drvdata)) {
+		dev_err(p_dev,
+			"Saving set time request (hardware not ready)\n");
+		p_priv_data->sys_seconds_cached = sec;
+		p_priv_data->sys_boot_cached = get_seconds();
+		return 0;
+	}
+
 	/* M4 accepts time as u32*/
-	if (m4sensorhub_reg_getsize(p_m4_drvdata,
-		M4SH_REG_GENERAL_UTC) != m4sensorhub_reg_write(
-		p_m4_drvdata, M4SH_REG_GENERAL_UTC,
+	if (m4sensorhub_reg_getsize(p_m4_drvdata, M4SH_REG_GENERAL_UTC) !=
+		m4sensorhub_reg_write(p_m4_drvdata, M4SH_REG_GENERAL_UTC,
 		(char *)&sec, m4sh_no_mask)) {
-			dev_err(p_dev, "set time, but failed to set M4 clock!\n");
+			dev_err(p_dev,
+				"set time, but failed to set M4 clock!\n");
 			return -EIO;
 	}
 
 	return 0;
 }
 
-
-
 static const struct rtc_class_ops rtc_sensorhub_rtc_ops = {
 	.read_time = rtc_sensorhub_rtc_read_time,
 	.set_time = rtc_sensorhub_rtc_set_time,
@@ -219,8 +229,55 @@ static const struct rtc_class_ops rtc_sensorhub_rtc_ops = {
 	.alarm_irq_enable = rtc_sensorhub_rtc_alarm_irq_enable,
 };
 
+static int rtc_sensorhub_preflash(struct init_calldata *p_arg)
+{
+	int err = 0;
+	uint32_t seconds = 0;
+	int size = 0;
+	struct rtc_sensorhub_private_data *rtcpd = NULL;
+	struct m4sensorhub_data *m4 = NULL;
+
+	if (p_arg == NULL) {
+		pr_err("%s: No callback data received.\n", __func__);
+		err = -ENODATA;
+		goto rtc_sensorhub_preflash_fail;
+	} else if (p_arg->p_data == NULL) {
+		pr_err("%s: No private data received.\n", __func__);
+		err = -ENODATA;
+		goto rtc_sensorhub_preflash_fail;
+	} else if (p_arg->p_m4sensorhub_data == NULL) {
+		pr_err("%s: M4 data is NULL.\n", __func__);
+		err = -ENODATA;
+		goto rtc_sensorhub_preflash_fail;
+	}
+
+	/*
+	 * NOTE: We don't want to save the M4 data struct;
+	 *       M4 will be reflashed after this callback returns
+	 *       (thus the struct will become invalid but we have
+	 *        no way to communicate that to the rest of the driver).
+	 */
+	rtcpd = p_arg->p_data;
+	m4 = p_arg->p_m4sensorhub_data;
+	size = m4sensorhub_reg_getsize(m4, M4SH_REG_GENERAL_UTC);
+	err = m4sensorhub_reg_read(m4, M4SH_REG_GENERAL_UTC, (char *)&seconds);
+	if (err < 0) {
+		pr_err("%s: Failed to read RTC seconds from M4.\n", __func__);
+		goto rtc_sensorhub_preflash_fail;
+	} else if (err != size) {
+		pr_err("%s: Read %d bytes instead of %d.\n",
+			__func__, err, size);
+	}
+
+	rtcpd->m4_seconds_cached = seconds;
+	rtcpd->m4_boot_cached = get_seconds();
+
+rtc_sensorhub_preflash_fail:
+	return err;
+}
+
 static void rtc_handle_sensorhub_irq(enum m4sensorhub_irqs int_event,
-						void *p_data)
+	void *p_data)
 {
 	struct rtc_sensorhub_private_data *p_priv_data =
 			(struct rtc_sensorhub_private_data *)(p_data);
@@ -236,34 +293,56 @@ static int rtc_sensorhub_init(struct init_calldata *p_arg)
 	struct timespec tv;
 	struct rtc_sensorhub_private_data *p_priv_data =
 			(struct rtc_sensorhub_private_data *)(p_arg->p_data);
+	uint32_t seconds = 0;
 
 	p_priv_data->p_m4sensorhub_data = p_arg->p_m4sensorhub_data;
 
-	/* read RTC time from M4 and set the system time */
-	err = rtc_sensorhub_get_rtc_from_m4(&rtc,
-				p_priv_data->p_m4sensorhub_data);
-	if (err) {
-		pr_err("%s: get_rtc failed\n", DRIVER_NAME);
-		return 0;
+	if ((p_priv_data->sys_seconds_cached != 0) &&
+		(p_priv_data->sys_boot_cached != 0)) {
+		pr_err("%s: Setting M4 to a saved time request\n", __func__);
+		seconds = p_priv_data->sys_seconds_cached +
+			get_seconds() - p_priv_data->sys_boot_cached;
+		if (m4sensorhub_reg_getsize(p_priv_data->p_m4sensorhub_data,
+			M4SH_REG_GENERAL_UTC) != m4sensorhub_reg_write(
+			p_priv_data->p_m4sensorhub_data, M4SH_REG_GENERAL_UTC,
+			(char *)&seconds, m4sh_no_mask)) {
+				pr_err("%s: Failed to set M4 RTC\n",
+					__func__);
+				return 0;
+		}
+		/*
+		 * We don't write directly to tv_sec here because we want
+		 * to print below the system clock we are setting,
+		 * which means we need to populate rtc anyway.
+		 */
+		rtc_time_to_tm(seconds, &rtc);
+	} else {
+		/* read RTC time from M4 and set the system time */
+		err = rtc_sensorhub_get_rtc_from_m4(&rtc,
+					p_priv_data->p_m4sensorhub_data);
+		if (err) {
+			pr_err("%s: get_rtc failed\n", DRIVER_NAME);
+			return 0;
+		}
 	}
 
+	tv.tv_nsec = 0;  /* Initialize variable or do_settimeofday will fail */
 	rtc_tm_to_time(&rtc, &tv.tv_sec);
 
 	err = do_settimeofday(&tv);
 	if (err)
-		pr_err("%s: settimeofday failed\n", DRIVER_NAME);
+		pr_err("%s: settimeofday failed (err=%d)\n", DRIVER_NAME, err);
 
-	pr_info("setting system clock to "
-		"%d-%02d-%02d %02d:%02d:%02d UTC (%u)\n",
+	pr_info("%s %d-%02d-%02d %02d:%02d:%02d UTC (%u)\n",
+		"setting system clock to",
 		rtc.tm_year + 1900, rtc.tm_mon + 1, rtc.tm_mday,
 		rtc.tm_hour, rtc.tm_min, rtc.tm_sec,
 		(unsigned int) tv.tv_sec);
 
 	/* register an irq handler*/
 	err = m4sensorhub_irq_register(p_priv_data->p_m4sensorhub_data,
-						M4SH_IRQ_AP_ALARM_EXPIRED,
-						rtc_handle_sensorhub_irq,
-						p_priv_data, 1);
+		M4SH_IRQ_AP_ALARM_EXPIRED, rtc_handle_sensorhub_irq,
+		p_priv_data, 1);
 
 	if (err < 0)
 		pr_err("%s: irq register failed\n", DRIVER_NAME);
@@ -296,7 +375,7 @@ static int rtc_sensorhub_probe(struct platform_device *p_platdev)
 	}
 
 	p_rtc = devm_rtc_device_register(&p_platdev->dev, "rtc_sensorhub",
-				&rtc_sensorhub_rtc_ops, THIS_MODULE);
+		&rtc_sensorhub_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(p_rtc)) {
 		err = PTR_ERR(p_rtc);
@@ -305,14 +384,24 @@ static int rtc_sensorhub_probe(struct platform_device *p_platdev)
 
 	p_priv_data->p_rtc = p_rtc;
 
+	err = m4sensorhub_register_preflash_callback(rtc_sensorhub_preflash,
+		p_priv_data);
+	if (err < 0) {
+		dev_err(&(p_platdev->dev),
+			"Failed to register M4 preflash callback\n");
+		goto err_unregister_rtc;
+	}
+
 	err = m4sensorhub_register_initcall(rtc_sensorhub_init, p_priv_data);
 	if (err) {
 		dev_err(&(p_platdev->dev), "can't register init with m4\n");
-		goto err_unregister_rtc;
+		goto err_unregister_preflash_callback;
 	}
 
 	return 0;
 
+err_unregister_preflash_callback:
+	m4sensorhub_unregister_preflash_callback(rtc_sensorhub_preflash);
 err_unregister_rtc:
 	devm_rtc_device_unregister(&p_platdev->dev, p_rtc);
 	kfree(p_rtc);
diff --git a/include/linux/m4sensorhub.h b/include/linux/m4sensorhub.h
index 0559eb980f0..2e0de82aa1b 100644
--- a/include/linux/m4sensorhub.h
+++ b/include/linux/m4sensorhub.h
@@ -31,10 +31,10 @@ extern char m4sensorhub_debug;
 #define M4SENSORHUB_DRIVER_NAME     "m4sensorhub"
 #define M4SENSORHUB_I2C_ADDR        0x18
 
-#define KDEBUG(i, format, s...) 			\
-	do {						\
+#define KDEBUG(i, format, s...)                         \
+	do {                                            \
 		if (m4sensorhub_debug >= i)             \
-			printk(KERN_CRIT format, ##s);	\
+			pr_crit(format, ##s);           \
 	} while (0)
 
 enum m4sensorhub_debug_level {
@@ -222,6 +222,23 @@ int m4sensorhub_register_initcall(int(*initfunc)(struct init_calldata *),
 void m4sensorhub_unregister_initcall(
 		int(*initfunc)(struct init_calldata *));
 
+/*
+ * Some M4 drivers (e.g., RTC) require reading data on boot, even if M4
+ * needs a firmware update.  These functions allow drivers to register
+ * callbacks with the core to take care of small maintenance tasks before
+ * M4 is reflashed (e.g., caching the system time).
+ *
+ * NOTE:  Drivers should not rely on this call for normal operation.
+ *        Reflashing M4 is an uncommon event, and most of the time,
+ *        especially in production, these callbacks will never be used.
+ */
+int m4sensorhub_register_preflash_callback(
+		int(*initfunc)(struct init_calldata *), void *pdata);
+void m4sensorhub_unregister_preflash_callback(
+		int(*initfunc)(struct init_calldata *));
+void m4sensorhub_call_preflash_callbacks(void); /* For FW flash core */
+bool m4sensorhub_preflash_callbacks_exist(void); /* For FW flash core */
+
 int m4sensorhub_irq_disable_all(struct m4sensorhub_data *m4sensorhub);
 
 #endif /* __KERNEL__ */