IKXCLOCK-1349 Fix Kernel Panics in M4 Core

Fixed several panics caused by missing NULL checks in exported core functions
  (e.g., M4 fails to init but an outside process calls the sysfs interface)
Fixed/added error messages to check for bad conditions earlier
Fixed a bug in m4sensorhub-irq where IRQ book-keeping was not being done
Added missing callback string to m4sensorhub-panic.c

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

7 files changed, 629 insertions, 273 deletions

diff --git a/drivers/mfd/m4sensorhub-core.c b/drivers/mfd/m4sensorhub-core.c
index 168b59f7bd0..8a55d5beb3b 100644
--- a/drivers/mfd/m4sensorhub-core.c
+++ b/drivers/mfd/m4sensorhub-core.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 Motorola, Inc.
+ * Copyright (C) 2012-2014 Motorola, Inc.
  *
  * 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
@@ -94,8 +94,8 @@ static ssize_t m4sensorhub_get_dbg(struct device *dev,
 int m4sensorhub_set_bootmode(struct m4sensorhub_data *m4sensorhub,
 			 enum m4sensorhub_bootmode bootmode)
 {
-	if (!m4sensorhub) {
-		printk(KERN_ERR "set_bootmode: invalid pointer\n");
+	if (m4sensorhub == NULL) {
+		pr_err("%s: M4 data is NULL\n", __func__);
 		return -EINVAL;
 	}
 
@@ -125,27 +125,45 @@ EXPORT_SYMBOL_GPL(m4sensorhub_set_bootmode);
 
 void m4sensorhub_hw_reset(struct m4sensorhub_data *m4sensorhub)
 {
-	if (!m4sensorhub) {
-		printk(KERN_ERR "m4sensorhub_hw_reset: invalid pointer\n");
-		return;
-	}
+	int err = 0;
 
+	if (m4sensorhub == NULL) {
+		pr_err("%s: M4 data is NULL\n", __func__);
+		err = -ENODATA;
+		goto m4sensorhub_hw_reset_fail;
+	} else if (m4sensorhub->i2c_client == NULL) {
+		pr_err("%s: I2C client is missing\n", __func__);
+		err = -ENODATA;
+		goto m4sensorhub_hw_reset_fail;
+	}
 
 	if (m4sensorhub->i2c_client->addr == 0x39) {
-		m4sensorhub_set_bootmode(m4sensorhub, BOOTMODE01);
+		err = m4sensorhub_set_bootmode(m4sensorhub, BOOTMODE01);
+		if (err < 0) {
+			pr_err("%s: Failed to enter bootmode 01\n", __func__);
+			goto m4sensorhub_hw_reset_fail;
+		}
 		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);
+		err = m4sensorhub_set_bootmode(m4sensorhub, BOOTMODE00);
+		if (err < 0) {
+			pr_err("%s: Failed to enter bootmode 00\n", __func__);
+			goto m4sensorhub_hw_reset_fail;
+		}
 		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);
 	}
+
+m4sensorhub_hw_reset_fail:
+	if (err < 0)
+		pr_err("%s: Failed with error code %d", __func__, err);
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_hw_reset);
 
@@ -158,20 +176,20 @@ static int m4sensorhub_hw_init(struct m4sensorhub_data *m4sensorhub,
 	int err = -EINVAL;
 	const char *fp = NULL;
 
-	if (!m4sensorhub) {
-		printk(KERN_ERR "m4sensorhub_hw_init: invalid pointer\n");
-		err = -EINVAL;
+	if (m4sensorhub == NULL) {
+		pr_err("%s: M4 data is NULL\n", __func__);
+		err = -ENODATA;
 		goto error;
-	}
-	if (node == NULL) {
-		printk(KERN_ERR "m4sensorhub_hw_init: node null\n");
-		err = -EINVAL;
+	} else if (node == NULL) {
+		pr_err("%s: Device node is missing\n", __func__);
+		err = -ENODATA;
 		goto error;
 	}
 
 	of_property_read_string(node, "mot,fw-filename", &fp);
 	if (fp == NULL) {
-		pr_err("Missing M4 sensorhub firmware filename\n");
+		pr_err("%s: Missing M4 sensorhub firmware filename\n",
+			__func__);
 		err = -EINVAL;
 		goto error;
 	}
@@ -266,8 +284,8 @@ error:
 static void m4sensorhub_hw_free(struct m4sensorhub_data *m4sensorhub)
 {
 
-	if (!m4sensorhub) {
-		printk(KERN_ERR "hw_free: invalid pointer\n");
+	if (m4sensorhub == NULL) {
+		pr_err("%s: M4 data is NULL\n", __func__);
 		return;
 	}
 
@@ -311,9 +329,11 @@ int m4sensorhub_register_initcall(int(*initfunc)(struct init_calldata *),
 
 	inc = kzalloc(sizeof(struct init_call), GFP_KERNEL);
 	if (inc == NULL) {
-		KDEBUG(M4SH_ERROR, "Unable to allocate for init call\n");
+		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 */
@@ -321,6 +341,7 @@ int m4sensorhub_register_initcall(int(*initfunc)(struct init_calldata *),
 		inc->next = NULL;
 	else
 		inc->next = inithead;
+
 	inithead = inc;
 	return 0;
 }
@@ -356,13 +377,14 @@ static void m4sensorhub_initialize(const struct firmware *firmware,
 	struct init_calldata arg;
 
 	if (firmware == NULL) {
-		KDEBUG(M4SH_ERROR, "%s:No firmware data recieved\n", __func__);
+		KDEBUG(M4SH_ERROR, "%s: No firmware data recieved\n",
+			__func__);
 		return;
 	}
 
 	/* initiate m4 firmware download */
-	KDEBUG(M4SH_CRITICAL, "Starting firmware download with force_upgrade "
-		"= %d\n", force_upgrade);
+	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,
@@ -373,8 +395,10 @@ static void m4sensorhub_initialize(const struct firmware *firmware,
 						&m4sensorhub_misc_data,
 						force_upgrade,
 						firmware);
+
 	if (firmware_download_status < 0) {
-		KDEBUG(M4SH_ERROR, "Failed to load M4 firmware = %d\n", err);
+		KDEBUG(M4SH_ERROR, "%s: Failed to load M4 firmware = %d\n",
+			__func__, err);
 		/* Since firmware download failed, put m4 back into boot mode*/
 		m4sensorhub_hw_reset(&m4sensorhub_misc_data);
 		return;
@@ -382,7 +406,8 @@ static void m4sensorhub_initialize(const struct firmware *firmware,
 
 	err = m4sensorhub_irq_init(&m4sensorhub_misc_data);
 	if (err < 0) {
-		KDEBUG(M4SH_ERROR, "M4sensorhub irq init failed\n");
+		KDEBUG(M4SH_ERROR, "%s: m4sensorhub irq init failed (err=%d)\n",
+			__func__, err);
 		return;
 	}
 
@@ -393,8 +418,12 @@ static void m4sensorhub_initialize(const struct firmware *firmware,
 	while (inc) {
 		arg.p_data = inc->pdata;
 		err = inc->initcb(&arg);
-		if (err < 0)
+		if (err < 0) {
+			KDEBUG(M4SH_ERROR,
+				"%s: Callback failed with error code %d %s\n",
+				__func__, err, "(dumping stack)");
 			dump_stack();
+		}
 		prev = inc;
 		inc = inc->next;
 		kfree(prev);
@@ -415,8 +444,8 @@ static ssize_t m4sensorhub_set_dbg(struct device *dev,
 		return -EINVAL;
 
 	m4sensorhub_debug = debug;
-	KDEBUG(M4SH_CRITICAL, "M4 Sensor Hub debug level = %d\n",
-		m4sensorhub_debug);
+	KDEBUG(M4SH_CRITICAL, "%s: M4 Sensor Hub debug level = %d\n",
+		__func__, m4sensorhub_debug);
 
 	return count;
 }
@@ -464,7 +493,7 @@ void ParseAndUpdateLogLevels(char *tag, char *level,
 	*logLevels &= ~(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);
+	KDEBUG(M4SH_INFO, "New M4 log levels = 0x%llx\n", *logLevels);
 }
 
 /* Usage: adb shell into the directory of sysinterface log_level and
@@ -567,7 +596,8 @@ static ssize_t m4sensorhub_disable_interrupts(struct device *dev,
 
 	ret = m4sensorhub_irq_disable_all(&m4sensorhub_misc_data);
 	if (ret < 0) {
-		KDEBUG(M4SH_ERROR, "Unable to disable all m4 interrupts\n");
+		KDEBUG(M4SH_ERROR, "%s: Unable to disable all m4 interrupts\n",
+			__func__);
 		return ret;
 	}
 	return count;
@@ -596,7 +626,6 @@ static int m4sensorhub_probe(struct i2c_client *client,
 	struct device_node *node = client->dev.of_node;
 	int err = -EINVAL;
 
-
 	/* Set debug based on module argument if set, otherwise use
 	   default logging rate based on build type */
 	if (debug_level)
@@ -610,15 +639,16 @@ static int m4sensorhub_probe(struct i2c_client *client,
 		m4sensorhub_debug = M4SH_ERROR;
 #endif
 	}
+
 	/* Enabling detailed level M4 logs for all builds*/
 	m4sensorhub_debug = M4SH_INFO;
-	KDEBUG(M4SH_ERROR, "Initializing M4 Sensor Hub debug=%d\n",
-			m4sensorhub_debug);
+	KDEBUG(M4SH_ERROR, "%s: Initializing M4 Sensor Hub debug=%d\n",
+			__func__, m4sensorhub_debug);
 
 	m4sensorhub->mode = UNINITIALIZED;
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
-		KDEBUG(M4SH_ERROR, "client not i2c capable\n");
+		KDEBUG(M4SH_ERROR, "%s: client not i2c capable\n", __func__);
 		err = -ENODEV;
 		goto err_unload;
 	}
@@ -627,36 +657,41 @@ static int m4sensorhub_probe(struct i2c_client *client,
 	m4sensorhub->i2c_client = client;
 
 	err = m4sensorhub_hw_init(m4sensorhub, node);
-	if (err) {
-		printk(KERN_ERR "%s: hw_init Failed!", __func__);
+	if (err < 0) {
+		KDEBUG(M4SH_ERROR, "%s: hw_init failed!", __func__);
 		goto done;
 	}
 
 	/* link i2c_client to m4sensorhub */
 	i2c_set_clientdata(client, m4sensorhub);
 
-
 	err = misc_register(&m4sensorhub_misc_device);
 	if (err < 0) {
-		KDEBUG(M4SH_ERROR, "misc register failed: %d\n", err);
+		KDEBUG(M4SH_ERROR, "%s: misc_register failed: %d\n",
+			__func__, err);
 		goto err_hw_free;
 	}
 
 	err = sysfs_create_group(&client->dev.kobj, &m4sensorhub_control_group);
-	if (err)
+	if (err < 0) {
+		KDEBUG(M4SH_ERROR, "%s: Failed to create sysfs group\n",
+			__func__);
 		goto err_deregister;
+	}
 
 	if (m4sensorhub->hwconfig.irq_gpio >= 0)
 		client->irq = gpio_to_irq(m4sensorhub->hwconfig.irq_gpio);
 	else {
-		KDEBUG(M4SH_ERROR, "Error: No IRQ configured\n");
+		KDEBUG(M4SH_ERROR, "%s: No IRQ configured\n", __func__);
 		err = -ENODEV;
 		goto err_unregister_control_group;
 	}
 
 	err = m4sensorhub_panic_init(m4sensorhub);
-	if (err < 0)
+	if (err < 0) {
+		KDEBUG(M4SH_ERROR, "%s: Panic init failed\n", __func__);
 		goto err_reg_shutdown;
+	}
 
 	err = request_firmware_nowait(THIS_MODULE,
 			FW_ACTION_HOTPLUG, m4sensorhub->filename,
@@ -664,7 +699,8 @@ static int m4sensorhub_probe(struct i2c_client *client,
 			GFP_KERNEL, m4sensorhub,
 			m4sensorhub_initialize);
 	if (err < 0) {
-		KDEBUG(M4SH_ERROR, "request_firmware_nowait failed: %d\n", err);
+		KDEBUG(M4SH_ERROR, "%s: request_firmware_nowait failed: %d\n",
+			__func__, err);
 		goto err_panic_shutdown;
 	}
 	KDEBUG(M4SH_NOTICE, "Registered M4 Sensor Hub\n");
@@ -686,6 +722,11 @@ err_hw_free:
 	m4sensorhub = NULL;
 err_unload:
 done:
+	if (err < 0) {
+		KDEBUG(M4SH_ERROR, "%s: Probe failed with error code %d\n",
+			__func__, err);
+	}
+
 	return err;
 }
 
@@ -694,6 +735,9 @@ static int __exit m4sensorhub_remove(struct i2c_client *client)
 	struct m4sensorhub_data *m4sensorhub = i2c_get_clientdata(client);
 	KDEBUG(M4SH_INFO, "Removing M4 Sensor Hub Driver\n");
 
+	if (m4sensorhub == NULL)
+		return 0;
+
 	m4sensorhub_irq_shutdown(m4sensorhub);
 	m4sensorhub_panic_shutdown(m4sensorhub);
 	m4sensorhub_reg_shutdown(m4sensorhub);
diff --git a/drivers/mfd/m4sensorhub-irq.c b/drivers/mfd/m4sensorhub-irq.c
index a2b39a3c0d3..b7cdba1188e 100644
--- a/drivers/mfd/m4sensorhub-irq.c
+++ b/drivers/mfd/m4sensorhub-irq.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 Motorola, Inc.
+ * Copyright (C) 2012-2014 Motorola, Inc.
  *
  * 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
@@ -51,19 +51,19 @@ static void irq_work_func(struct work_struct *work);
 #ifdef CONFIG_DEBUG_FS
 static int m4sensorhub_dbg_irq_open(struct inode *inode, struct file *file);
 #endif
-static void m4sensorhub_irq_restore(struct m4sensorhub_data *m4sensorhub,\
-					void *data);
+static void m4sensorhub_irq_restore(struct m4sensorhub_data *m4sensorhub,
+	void *data);
 
 /* ---------------- Local Declarations -------------- */
 
 static const char *irq_name[] = {
 	[M4SH_IRQ_TMP_DATA_READY]         = "TMP_DATA_READY",
-	[M4SH_IRQ_PRESSURE_DATA_READY]	  = "PRES_DATA_READY",
-	[M4SH_IRQ_GYRO_DATA_READY]	  = "GYRO_DATA_READY",
+	[M4SH_IRQ_PRESSURE_DATA_READY]    = "PRES_DATA_READY",
+	[M4SH_IRQ_GYRO_DATA_READY]        = "GYRO_DATA_READY",
 	[M4SH_IRQ_PEDOMETER_DATA_READY]   = "PEDO_DATA_READY",
-	[M4SH_IRQ_COMPASS_DATA_READY]	  = "COMPASS_DATA_READY",
-	[M4SH_IRQ_FUSION_DATA_READY]	  = "FUSION_DATA_READY",
-	[M4SH_IRQ_ACCEL_DATA_READY]	  = "ACCEL_DATA_READY",
+	[M4SH_IRQ_COMPASS_DATA_READY]     = "COMPASS_DATA_READY",
+	[M4SH_IRQ_FUSION_DATA_READY]      = "FUSION_DATA_READY",
+	[M4SH_IRQ_ACCEL_DATA_READY]       = "ACCEL_DATA_READY",
 	[M4SH_IRQ_GESTURE_DETECTED]       = "GESTURE_DETECTED",
 	[M4SH_IRQ_STILL_DETECTED]         = "STILL_DETECTED",
 	[M4SH_IRQ_MOTION_DETECTED]        = "MOTION_DETECTED",
@@ -164,41 +164,54 @@ static const struct file_operations debug_fops = {
 
      m4sensorhub - pointer to the main m4sensorhub data struct
 */
-
 int m4sensorhub_irq_init(struct m4sensorhub_data *m4sensorhub)
 {
-	int retval;
-	struct i2c_client *i2c = m4sensorhub->i2c_client;
-	struct m4sensorhub_irqdata *data;
+	int retval = 0;
+	struct i2c_client *i2c = NULL;
+	struct m4sensorhub_irqdata *data = NULL;
+
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto done;
+	} else if (m4sensorhub->i2c_client == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: I2C client is missing\n", __func__);
+		retval = -ENODATA;
+		goto done;
+	}
+
+	i2c = m4sensorhub->i2c_client;
 
 	data = kzalloc(sizeof(struct m4sensorhub_irqdata), GFP_KERNEL);
-	if (!data) {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: Memory error in irq_init\n");
+	if (data == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: Failed to allocat irqdata\n", __func__);
 		retval = -ENOMEM;
 		goto done;
 	}
-	KDEBUG(
-		M4SH_INFO,
-		"m4sensorhub: %u IRQs with valid_bits %02X%02X%02X\n",
-		M4SH_IRQ__NUM, int_registers[2].valid_bits,
-		int_registers[1].valid_bits, int_registers[0].valid_bits
-		);
+
+	mutex_init(&data->lock);
+
+	data->m4sensorhub = m4sensorhub;
+	m4sensorhub->irqdata = data;
+
+	KDEBUG(M4SH_INFO, "%s: %u IRQs with valid_bits %02X%02X%02X\n",
+		__func__, M4SH_IRQ__NUM, int_registers[2].valid_bits,
+		int_registers[1].valid_bits, int_registers[0].valid_bits);
 	retval = m4sensorhub_irq_disable_all(m4sensorhub);
-	if (retval) {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: Failed disable all irqs\n");
+	if (retval < 0) {
+		KDEBUG(M4SH_ERROR, "%s: Failed disable all irqs\n", __func__);
 		goto err_free;
 	}
 
 	data->workqueue = create_workqueue("m4sensorhub_irq");
 	if (data->workqueue == NULL) {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: IRQ Workqueue init failure\n");
+		KDEBUG(M4SH_ERROR, "%s: IRQ Workqueue init failure\n",
+			__func__);
 		retval = -ENOMEM;
 		goto err_free;
 	}
 	INIT_WORK(&data->work, irq_work_func);
 
-	mutex_init(&data->lock);
-
 	wake_lock_init(&data->wake_lock, WAKE_LOCK_SUSPEND, "m4sensorhub-irq");
 	wake_lock_init(&data->tm_wake_lock, WAKE_LOCK_SUSPEND,
 		       "m4sensorhub-timed-irq");
@@ -206,16 +219,13 @@ int m4sensorhub_irq_init(struct m4sensorhub_data *m4sensorhub)
 	retval = request_irq(i2c->irq, event_isr, IRQF_TRIGGER_HIGH,
 		"m4sensorhub-irq", data);
 	if (retval) {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: Failed requesting irq.\n");
+		KDEBUG(M4SH_ERROR, "%s: Failed requesting irq.\n", __func__);
 		goto err_destroy_wq;
 	}
 
-	data->m4sensorhub = m4sensorhub;
-	m4sensorhub->irqdata = data;
-
 	retval = enable_irq_wake(i2c->irq);
 	if (retval) {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: Failed enabling irq wake.\n");
+		KDEBUG(M4SH_ERROR, "%s: Failed enabling irq wake.\n", __func__);
 		goto err_free_irq;
 	}
 
@@ -223,14 +233,15 @@ int m4sensorhub_irq_init(struct m4sensorhub_data *m4sensorhub)
 	data->debugfs = debugfs_create_file("m4sensorhub-irq", S_IRUGO, NULL,
 					    data, &debug_fops);
 	if (data->debugfs == NULL) {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: Error creating debufs\n");
+		KDEBUG(M4SH_ERROR, "%s: Error creating debufs\n", __func__);
 		retval = -EINVAL;
 		goto err_disa_irq;
 	}
 #endif
-	m4sensorhub_panic_register(m4sensorhub, PANICHDL_IRQ_RESTORE,\
-					m4sensorhub_irq_restore, data);
-	KDEBUG(M4SH_INFO, "m4sensorhub IRQ subsystem initialized\n");
+	m4sensorhub_panic_register(m4sensorhub, PANICHDL_IRQ_RESTORE,
+		m4sensorhub_irq_restore, data);
+	KDEBUG(M4SH_INFO, "%s: m4sensorhub IRQ subsystem initialized\n",
+		__func__);
 	retval = 0;
 	goto done;
 
@@ -240,14 +251,14 @@ err_disa_irq:
 	disable_irq_wake(i2c->irq);
 err_free_irq:
 	free_irq(i2c->irq, data);
-	m4sensorhub->irqdata = NULL;
-	data->m4sensorhub = NULL;
 err_destroy_wq:
 	wake_lock_destroy(&data->wake_lock);
 	wake_lock_destroy(&data->tm_wake_lock);
-	mutex_destroy(&data->lock);
 	destroy_workqueue(data->workqueue);
 err_free:
+	mutex_destroy(&data->lock);
+	m4sensorhub->irqdata = NULL;
+	data->m4sensorhub = NULL;
 	kfree(data);
 done:
 	return retval;
@@ -262,10 +273,25 @@ EXPORT_SYMBOL_GPL(m4sensorhub_irq_init);
 */
 void m4sensorhub_irq_shutdown(struct m4sensorhub_data *m4sensorhub)
 {
-	struct i2c_client *i2c = m4sensorhub->i2c_client;
-	struct m4sensorhub_irqdata *data = m4sensorhub->irqdata;
+	struct i2c_client *i2c = NULL;
+	struct m4sensorhub_irqdata *data = NULL;
 
-	KDEBUG(M4SH_INFO, "shutdown m4sensorhub IRQ subsystem\n");
+	KDEBUG(M4SH_INFO, "%s: shutdown m4sensorhub IRQ subsystem\n",
+		__func__);
+
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return;
+	} else if (m4sensorhub->i2c_client) {
+		KDEBUG(M4SH_ERROR, "%s: I2C client is NULL\n", __func__);
+		return;
+	} else if (m4sensorhub->irqdata) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ data is NULL\n", __func__);
+		return;
+	}
+
+	i2c = m4sensorhub->i2c_client;
+	data = m4sensorhub->irqdata;
 
 	m4sensorhub_panic_unregister(m4sensorhub, PANICHDL_IRQ_RESTORE);
 
@@ -311,23 +337,36 @@ EXPORT_SYMBOL_GPL(m4sensorhub_irq_shutdown);
      cb_func - IRQ handler function to execute on inturrupt
      data - pointer to data for IRQ handler function
 */
-
 int m4sensorhub_irq_register(struct m4sensorhub_data *m4sensorhub,
 			     enum m4sensorhub_irqs irq,
 			     void (*cb_func) (enum m4sensorhub_irqs, void *),
 			     void *data, uint8_t enable_timed_wakelock)
 {
-	struct m4sensorhub_irqdata *irqdata;
+	struct m4sensorhub_irqdata *irqdata = NULL;
 	int retval = 0;
 
-	if ((!m4sensorhub) || (irq >= M4SH_IRQ__NUM) || (!cb_func))
-		return -EINVAL;
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_register_exit;
+	} else if (irq >= M4SH_IRQ__NUM) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ %d exceeds maximum %d\n",
+			__func__, irq, M4SH_IRQ__NUM);
+		retval = -EINVAL;
+		goto m4sensorhub_irq_register_exit;
+	} else if (cb_func == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: Handler missing for IRQ %d\n",
+			__func__, irq);
+		retval = -ENOSYS;
+		goto m4sensorhub_irq_register_exit;
+	}
 
 	irqdata = m4sensorhub->irqdata;
-
 	if (irqdata == NULL) {
-		KDEBUG(M4SH_ERROR, "irqdata null for caller = %d\n", irq);
-		return -EINVAL;
+		KDEBUG(M4SH_ERROR, "%s: Caller irqdata is NULL (irq=%d)\n",
+			__func__, irq);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_register_exit;
 	}
 
 	mutex_lock(&irqdata->lock);
@@ -337,16 +376,18 @@ int m4sensorhub_irq_register(struct m4sensorhub_data *m4sensorhub,
 		irqdata->irq_info[irq].tm_wakelock = enable_timed_wakelock;
 		irqdata->event_handler[irq].func = cb_func;
 		irqdata->event_handler[irq].data = data;
-		KDEBUG(M4SH_NOTICE, "m4sensorhub: %s IRQ registered\n",
-			irq_name[irq]);
+		KDEBUG(M4SH_NOTICE, "%s: %s IRQ registered\n",
+			__func__, irq_name[irq]);
 	} else {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: %s IRQ registration failed\n",
-			irq_name[irq]);
+		KDEBUG(M4SH_ERROR, "%s: %s IRQ registration failed\n",
+			__func__, irq_name[irq]);
 		retval = -EPERM;
+		goto m4sensorhub_irq_register_fail;
 	}
 
+m4sensorhub_irq_register_fail:
 	mutex_unlock(&irqdata->lock);
-
+m4sensorhub_irq_register_exit:
 	return retval;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_irq_register);
@@ -363,14 +404,31 @@ EXPORT_SYMBOL_GPL(m4sensorhub_irq_register);
 int m4sensorhub_irq_unregister(struct m4sensorhub_data *m4sensorhub,
 			       enum m4sensorhub_irqs irq)
 {
-	struct m4sensorhub_irqdata *data = m4sensorhub->irqdata;
-	int retval;
+	struct m4sensorhub_irqdata *data = NULL;
+	int retval = 0;
 
-	if ((irq >= M4SH_IRQ__NUM) || (m4sensorhub == NULL) ||
-			(m4sensorhub->irqdata == NULL))
-		return -EINVAL;
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_unregister_exit;
+	} else if (irq >= M4SH_IRQ__NUM) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ %d exceeds maximum %d\n",
+			__func__, irq, M4SH_IRQ__NUM);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_unregister_exit;
+	} else if (m4sensorhub->irqdata == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_unregister_exit;
+	}
 
+	data = m4sensorhub->irqdata;
 	retval = m4sensorhub_irq_disable(m4sensorhub, irq);
+	if (retval < 0) {
+		KDEBUG(M4SH_ERROR, "%s: Failed to disable IRQ %d\n",
+			__func__, irq);
+		goto m4sensorhub_irq_unregister_exit;
+	}
 
 	mutex_lock(&data->lock);
 	data->event_handler[irq].func = NULL;
@@ -378,9 +436,10 @@ int m4sensorhub_irq_unregister(struct m4sensorhub_data *m4sensorhub,
 	data->irq_info[irq].registered = 0;
 	mutex_unlock(&data->lock);
 
-	KDEBUG(M4SH_NOTICE, "m4sensorhub: %s IRQ un-registered\n",
-			    irq_name[irq]);
+	KDEBUG(M4SH_NOTICE, "%s: %s IRQ un-registered\n",
+			    __func__, irq_name[irq]);
 
+m4sensorhub_irq_unregister_exit:
 	return retval;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_irq_unregister);
@@ -395,16 +454,31 @@ EXPORT_SYMBOL_GPL(m4sensorhub_irq_unregister);
      m4sensorhub - pointer to the main m4sensorhub data struct
      irq - M4 Sensor Hub interupt to check
 */
-
 int m4sensorhub_irq_enable_get(struct m4sensorhub_data *m4sensorhub,
 		       enum m4sensorhub_irqs irq)
 {
-	struct m4sensorhub_irqdata *data = m4sensorhub->irqdata;
+	struct m4sensorhub_irqdata *data = NULL;
 	int retval = -EINVAL;
 
-	if (irq < M4SH_IRQ__NUM)
-		return data->irq_info[irq].enabled;
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_enable_get_fail;
+	} else if (irq >= M4SH_IRQ__NUM) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ %d exceeds maximum %d\n",
+			__func__, irq, M4SH_IRQ__NUM);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_enable_get_fail;
+	} else if (m4sensorhub->irqdata == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_enable_get_fail;
+	}
 
+	data = m4sensorhub->irqdata;
+	retval = data->irq_info[irq].enabled;
+
+m4sensorhub_irq_enable_get_fail:
 	return retval;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_irq_enable_get);
@@ -418,23 +492,57 @@ EXPORT_SYMBOL_GPL(m4sensorhub_irq_enable_get);
      m4sensorhub - pointer to the main m4sensorhub data struct
      irq - M4 Sensor Hub interupt to disable
 */
-
 int m4sensorhub_irq_disable(struct m4sensorhub_data *m4sensorhub,
 		   enum m4sensorhub_irqs irq)
 {
-	struct m4sensorhub_irqdata *data = m4sensorhub->irqdata;
+	struct m4sensorhub_irqdata *data = NULL;
 	int retval = -EINVAL;
+	bool enabled = true;
 
-	if (irq < M4SH_IRQ__NUM) {
-		mutex_lock(&data->lock);
-		data->irq_info[irq].enabled = 0;
-		mutex_unlock(&data->lock);
-		retval = m4sensorhub_reg_write_1byte(m4sensorhub,
-				get_enable_reg(irq), 0, EVENT_MASK(irq));
-		retval = CHECK_REG_ACCESS_RETVAL(m4sensorhub, retval,
-						 get_enable_reg(irq));
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_disable_fail;
+	} else if (irq >= M4SH_IRQ__NUM) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ %d exceeds maximum %d\n",
+			__func__, irq, M4SH_IRQ__NUM);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_disable_fail;
+	} else if (m4sensorhub->irqdata == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_disable_fail;
 	}
 
+	data = m4sensorhub->irqdata;
+	mutex_lock(&data->lock);
+	if (data->irq_info[irq].enabled == 0)
+		enabled = false;
+	mutex_unlock(&data->lock);
+
+	/* Checking if the IRQ was previously disabled only to print an error */
+	if (!enabled) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ %d is already disabled\n",
+			__func__, irq);
+		goto m4sensorhub_irq_disable_fail;
+	}
+
+	retval = m4sensorhub_reg_write_1byte(m4sensorhub,
+		get_enable_reg(irq), 0, EVENT_MASK(irq));
+	if (retval < 0) {
+		KDEBUG(M4SH_ERROR, "%s: Register write failed\n", __func__);
+		goto m4sensorhub_irq_disable_fail;
+	} else if (retval != 1) {
+		KDEBUG(M4SH_ERROR, "%s: Wrote %d bytes instead of 1\n",
+			__func__, retval);
+		goto m4sensorhub_irq_disable_fail;
+	}
+
+	mutex_lock(&data->lock);
+	data->irq_info[irq].enabled = 0;
+	mutex_unlock(&data->lock);
+
+m4sensorhub_irq_disable_fail:
 	return retval;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_irq_disable);
@@ -448,24 +556,57 @@ EXPORT_SYMBOL_GPL(m4sensorhub_irq_disable);
      m4sensorhub - pointer to the main m4sensorhub data struct
      irq - M4 Sensor Hub interupt to enable
 */
-
 int m4sensorhub_irq_enable(struct m4sensorhub_data *m4sensorhub,
 		     enum m4sensorhub_irqs irq)
 {
-	struct m4sensorhub_irqdata *data = m4sensorhub->irqdata;
+	struct m4sensorhub_irqdata *data = NULL;
 	int retval = -EINVAL;
+	bool disabled = true;
 
-	if (irq < M4SH_IRQ__NUM) {
-		mutex_lock(&data->lock);
-		data->irq_info[irq].enabled = 1;
-		mutex_unlock(&data->lock);
-		retval = m4sensorhub_reg_write_1byte(m4sensorhub,
-				get_enable_reg(irq), EVENT_MASK(irq),
-				EVENT_MASK(irq));
-		retval = CHECK_REG_ACCESS_RETVAL(m4sensorhub, retval,
-						 get_enable_reg(irq));
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_enable_fail;
+	} else if (irq >= M4SH_IRQ__NUM) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ %d exceeds maximum %d\n",
+			__func__, irq, M4SH_IRQ__NUM);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_enable_fail;
+	} else if (m4sensorhub->irqdata == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_enable_fail;
+	}
+
+	data = m4sensorhub->irqdata;
+	mutex_lock(&data->lock);
+	if (data->irq_info[irq].enabled == 1)
+		disabled = false;
+	mutex_unlock(&data->lock);
+
+	/* Checking if the IRQ was previously enabled only to print an error */
+	if (!disabled) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ %d is already enabled\n",
+			__func__, irq);
+		goto m4sensorhub_irq_enable_fail;
+	}
+
+	retval = m4sensorhub_reg_write_1byte(m4sensorhub,
+		get_enable_reg(irq), EVENT_MASK(irq), EVENT_MASK(irq));
+	if (retval < 0) {
+		KDEBUG(M4SH_ERROR, "%s: Register write failed\n", __func__);
+		goto m4sensorhub_irq_enable_fail;
+	} else if (retval != 1) {
+		KDEBUG(M4SH_ERROR, "%s: Wrote %d bytes instead of 1\n",
+			__func__, retval);
+		goto m4sensorhub_irq_enable_fail;
 	}
 
+	mutex_lock(&data->lock);
+	data->irq_info[irq].enabled = 1;
+	mutex_unlock(&data->lock);
+
+m4sensorhub_irq_enable_fail:
 	return retval;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_irq_enable);
@@ -475,7 +616,6 @@ EXPORT_SYMBOL_GPL(m4sensorhub_irq_enable);
    Called by core to track suspend state and wakeup cause
 
 */
-
 void m4sensorhub_irq_pm_dbg_suspend(void)
 {
 	irq_dbg_info.suspend = 1;
@@ -488,7 +628,6 @@ EXPORT_SYMBOL_GPL(m4sensorhub_irq_pm_dbg_suspend);
    Called by core to print interupt source on M4SH wakeup
 
 */
-
 void m4sensorhub_irq_pm_dbg_resume(void)
 {
 	char buffer[DBG_BUF_LINE_LEN];
@@ -514,18 +653,31 @@ void m4sensorhub_irq_pm_dbg_resume(void)
 				/* find the first set bit */
 				index = (unsigned char)
 					(ffs(irq_dbg_info.en_ints[i]) - 1);
-				if (index >= M4SH_IRQ__NUM)
+				if (index >= M4SH_IRQ__NUM) {
+					KDEBUG(M4SH_NOTICE,
+							"%s: No set bits found\n",
+							__func__);
 					goto error;
+				}
 
 				/* clear the bit */
 				irq_dbg_info.en_ints[i] &= ~(1 << index);
 				/* find the event that occurred */
 				index += M4SH_IRQ__START +
 					 (i * NUM_INTS_PER_REG);
-				if (index >= M4SH_IRQ__NUM)
+				if (index >= M4SH_IRQ__NUM) {
+					KDEBUG(M4SH_NOTICE,
+							"%s: IRQ index is %d\n",
+							__func__, index);
 					goto error;
+				}
 
-				KDEBUG(M4SH_NOTICE, "\t%s\n", irq_name[index]);
+				if (index <= ARRAY_SIZE(irq_name))
+					KDEBUG(M4SH_NOTICE, "\t%s\n",
+						irq_name[index]);
+				else
+					KDEBUG(M4SH_NOTICE,
+						"\tIRQ %d\n", index);
 			}
 		}
 	}
@@ -533,6 +685,55 @@ error:
 	return;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_irq_pm_dbg_resume);
+
+/* m4sensorhub_irq_disable_all()
+
+   Disables all M4 IRQs (bypasses m4sensorhub_irq_disable())
+
+*/
+int m4sensorhub_irq_disable_all(struct m4sensorhub_data *m4sensorhub)
+{
+	int retval = 0;
+	int i = 0;
+	struct m4sensorhub_irqdata *data = NULL;
+
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_disable_all_fail;
+	} else if (m4sensorhub->irqdata == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: IRQ data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_irq_disable_all_fail;
+	}
+
+	for (i = 0; i < NUM_INT_REGS; i++) {
+		retval = m4sensorhub_reg_write_1byte(m4sensorhub,
+			int_registers[i].enable_reg, 0,
+			int_registers[i].valid_bits);
+		if (retval < 0) {
+			KDEBUG(M4SH_ERROR, "%s: Failed to disable INT%d\n",
+				__func__, i);
+			goto m4sensorhub_irq_disable_all_fail;
+		} else if (retval != 1) {
+			KDEBUG(M4SH_ERROR, "%s: Wrote %d bytes instead of 1\n",
+				__func__, retval);
+			retval = -EINVAL;
+			goto m4sensorhub_irq_disable_all_fail;
+		}
+	}
+
+	data = m4sensorhub->irqdata;
+	mutex_lock(&data->lock);
+	for (i = 0; i < M4SH_IRQ__NUM; i++)
+		data->irq_info[i].enabled = 0;
+	mutex_unlock(&data->lock);
+
+m4sensorhub_irq_disable_all_fail:
+	return retval;
+}
+EXPORT_SYMBOL_GPL(m4sensorhub_irq_disable_all);
+
 /* --------------- Local Functions ----------------- */
 
 static unsigned short get_enable_reg(enum m4sensorhub_irqs event)
@@ -553,23 +754,6 @@ static unsigned short get_enable_reg(enum m4sensorhub_irqs event)
 	return ret;
 }
 
-int m4sensorhub_irq_disable_all(struct m4sensorhub_data *m4sensorhub)
-{
-	int i;
-
-	for (i = 0; i < NUM_INT_REGS; i++) {
-		if (1 != m4sensorhub_reg_write_1byte(m4sensorhub,
-				int_registers[i].enable_reg, 0,
-				int_registers[i].valid_bits)) {
-			KDEBUG(M4SH_ERROR, "m4sensorhub_irq: "
-					"Failed disabling INT%d\n", i);
-			return -EFAULT;
-		}
-	}
-	return 0;
-}
-EXPORT_SYMBOL_GPL(m4sensorhub_irq_disable_all);
-
 static void irq_work_func(struct work_struct *work)
 {
 	unsigned short en_ints[NUM_INT_REGS] = { 0 };
@@ -685,8 +869,8 @@ static int m4sensorhub_dbg_irq_open(struct inode *inode, struct file *file)
    Callback Handler is called by Panic after M4 has been restarted
 
 */
-static void m4sensorhub_irq_restore(\
-	struct m4sensorhub_data *m4sensorhub, void *data)
+static void m4sensorhub_irq_restore(struct m4sensorhub_data *m4sensorhub,
+	void *data)
 {
 	int i;
 	unsigned short en_ints[NUM_INT_REGS] = {0};
@@ -700,13 +884,13 @@ static void m4sensorhub_irq_restore(\
 	mutex_unlock(&((struct m4sensorhub_irqdata *)data)->lock);
 
 	for (i = 0; i < NUM_INT_REGS; i++) {
-		KDEBUG(M4SH_INFO, "m4sensorhub_irq: Reseting INT%d-%02X\n",\
-					i, en_ints[i]);
+		KDEBUG(M4SH_INFO, "%s: Reseting INT%d-%02X\n", __func__,
+			i, en_ints[i]);
 		if (1 != m4sensorhub_reg_write_1byte(m4sensorhub,
 				int_registers[i].enable_reg, en_ints[i],
 				int_registers[i].valid_bits)) {
-			KDEBUG(M4SH_ERROR, "m4sensorhub_irq: "
-					"Failed reseting INT%d\n", i);
+			KDEBUG(M4SH_ERROR, "%s: Failed reseting INT%d\n",
+				__func__, i);
 		}
 	}
 }
diff --git a/drivers/mfd/m4sensorhub-panic.c b/drivers/mfd/m4sensorhub-panic.c
index f1cc373b12f..725c2ff80c0 100644
--- a/drivers/mfd/m4sensorhub-panic.c
+++ b/drivers/mfd/m4sensorhub-panic.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 Motorola, Inc.
+ * Copyright (C) 2012-2014 Motorola, Inc.
  *
  * 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
@@ -23,7 +23,6 @@
 #include <linux/slab.h>
 
 
-
 /* --------------- Global Declarations -------------- */
 #define PANIC_BANK              0xFF /* Reserved for Panic bank */
 #define PANIC_CMD_CHECK         0xCD /* Panic Handoff command */
@@ -33,6 +32,7 @@
 
 /* --------------- Local Declarations -------------- */
 static const char *callback_name[PANICHDL_MAX] = {
+	[PANICHDL_DISPLAY_RESTORE] = "display_restore",
 	[PANICHDL_IRQ_RESTORE] = "irq_restore",
 };
 
@@ -71,14 +71,32 @@ int m4sensorhub_panic_init(struct m4sensorhub_data *m4sensorhub)
 	int retval = 0;
 	struct m4sensorhub_panicdata *data;
 
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_panic_init_fail;
+	}
+
+	if (m4sensorhub->panicdata != NULL) {
+		KDEBUG(M4SH_ERROR,
+			"%s: Trying to overwrite previous panic data\n",
+			__func__);
+		retval = -EPERM;
+		goto m4sensorhub_panic_init_fail;
+	}
+
 	data = kzalloc(sizeof(struct m4sensorhub_panicdata), GFP_KERNEL);
-	if (data) {
-		mutex_init(&data->lock);
-		m4sensorhub->panicdata = data;
-	} else {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: Memory error in panic_init\n");
+	if (data == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: Memory error in panic_init\n",
+			__func__);
 		retval = -ENOMEM;
+		goto m4sensorhub_panic_init_fail;
 	}
+
+	mutex_init(&data->lock);
+	m4sensorhub->panicdata = data;
+
+m4sensorhub_panic_init_fail:
 	return retval;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_panic_init);
@@ -91,54 +109,86 @@ EXPORT_SYMBOL_GPL(m4sensorhub_panic_init);
 */
 void m4sensorhub_panic_shutdown(struct m4sensorhub_data *m4sensorhub)
 {
-	if (m4sensorhub && m4sensorhub->panicdata) {
-		struct m4sensorhub_panicdata *data = m4sensorhub->panicdata;
+	struct m4sensorhub_panicdata *data = NULL;
+
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return;
+	}
+
+	data = m4sensorhub->panicdata;
+	if (data != NULL) {
 		m4sensorhub->panicdata = NULL;
 		if (mutex_is_locked(&data->lock))
 			mutex_unlock(&data->lock);
 		mutex_destroy(&data->lock);
+		/*
+		 * Callback and data pointers were passed to us, so
+		 * we leave it to the registering functions to
+		 * clean up their own memory.
+		 */
 		kfree(data);
 	}
+
+	return;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_panic_shutdown);
 
 /* m4sensorhub_panic_register()
 
-   Register an panic handler to monitor M4 panic reset
+   Register a panic handler to monitor M4 panic reset
 
    Returns 0 on success or negative error code on failure
 
      m4sensorhub - pointer to the main m4sensorhub data struct
-     index - M4 Sensor Hub panic handler to resiter for
+     index - M4 Sensor Hub panic handler to register for
      cb_func - panic handler function to execute after M4 reset
      data - pointer to data for panic handler function
 */
-
 int m4sensorhub_panic_register(struct m4sensorhub_data *m4sensorhub,
 	enum m4sensorhub_panichdl_index index,
 	void (*cb_func) (struct m4sensorhub_data *, void *),
 	void *data)
 {
-	struct m4sensorhub_panicdata *panicdata;
+	struct m4sensorhub_panicdata *panicdata = NULL;
 	int retval = 0;
 
-	if (!m4sensorhub || (index >= PANICHDL_MAX) || !cb_func)
-		return -EINVAL;
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_panic_register_exit;
+	} else if (cb_func == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: Callback is NULL\n", __func__);
+		retval = -ENOSYS;
+		goto m4sensorhub_panic_register_exit;
+	} else if (index >= PANICHDL_MAX) {
+		KDEBUG(M4SH_ERROR, "%s: Number of panic handlers exceeded\n",
+			__func__);
+		retval = -EOVERFLOW;
+		goto m4sensorhub_panic_register_exit;
+	} else if (m4sensorhub->panicdata == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: Panic data is missing\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_panic_register_exit;
+	}
 
 	panicdata = (struct m4sensorhub_panicdata *)m4sensorhub->panicdata;
 	mutex_lock(&panicdata->lock);
 	if (panicdata->funcs[index].callback == NULL) {
 		panicdata->funcs[index].callback = cb_func;
 		panicdata->funcs[index].data = data;
-		KDEBUG(M4SH_NOTICE, "m4sensorhub: %s callback registered\n",
-			callback_name[index]);
+		KDEBUG(M4SH_NOTICE, "%s: %s callback registered\n",
+			__func__, callback_name[index]);
 	} else {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: %s callback"\
-			" registration failed\n", callback_name[index]);
+		KDEBUG(M4SH_ERROR, "%s: %s %s", __func__,
+			callback_name[index], "callback registration failed\n");
 		retval = -EPERM;
+		goto m4sensorhub_panic_register_fail;
 	}
-	mutex_unlock(&panicdata->lock);
 
+m4sensorhub_panic_register_fail:
+	mutex_unlock(&panicdata->lock);
+m4sensorhub_panic_register_exit:
 	return retval;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_panic_register);
@@ -155,20 +205,34 @@ EXPORT_SYMBOL_GPL(m4sensorhub_panic_register);
 int m4sensorhub_panic_unregister(struct m4sensorhub_data *m4sensorhub,
 	enum m4sensorhub_panichdl_index index)
 {
-	struct m4sensorhub_panicdata *panicdata;
+	int retval = 0;
+	struct m4sensorhub_panicdata *panicdata = NULL;
 
-	if (!m4sensorhub || (index >= PANICHDL_MAX))
-		return -EINVAL;
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_panic_unregister_fail;
+	} else if (index >= PANICHDL_MAX) {
+		KDEBUG(M4SH_ERROR, "%s: Number of panic handlers exceeded\n",
+			__func__);
+		retval = -EOVERFLOW;
+		goto m4sensorhub_panic_unregister_fail;
+	} else if (m4sensorhub->panicdata == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: Panic data is missing\n", __func__);
+		retval = -ENODATA;
+		goto m4sensorhub_panic_unregister_fail;
+	}
 
 	panicdata = (struct m4sensorhub_panicdata *)m4sensorhub->panicdata;
 	mutex_lock(&panicdata->lock);
 	panicdata->funcs[index].callback = NULL;
 	panicdata->funcs[index].data = NULL;
 	mutex_unlock(&panicdata->lock);
-	KDEBUG(M4SH_NOTICE, "m4sensorhub: %s callback un-registered\n",
-			callback_name[index]);
+	KDEBUG(M4SH_NOTICE, "%s: %s callback un-registered\n",
+			__func__, callback_name[index]);
 
-	return 0;
+m4sensorhub_panic_unregister_fail:
+	return retval;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_panic_unregister);
 
@@ -201,9 +265,11 @@ void m4sensorhub_panic_process(struct m4sensorhub_data *m4sensorhub)
 	union panic_buf buf;
 	struct m4sensorhub_panic_callback handler;
 
-	if (!m4sensorhub || !m4sensorhub->panicdata) {
-		KDEBUG(M4SH_ERROR, "m4sensorhub: Invalid parameter in %s!\n",\
-					__func__);
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return;
+	} else if (m4sensorhub->panicdata == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: Panic data is missing\n", __func__);
 		return;
 	}
 
@@ -211,41 +277,38 @@ void m4sensorhub_panic_process(struct m4sensorhub_data *m4sensorhub)
 
 	buf.in.bank = PANIC_BANK;
 	buf.in.cmd = PANIC_CMD_CHECK;
-	ret = m4sensorhub_i2c_write_read(m4sensorhub,\
-			(u8 *)&buf, sizeof(buf.in), sizeof(buf.data));
+	ret = m4sensorhub_i2c_write_read(m4sensorhub,
+		(u8 *)&buf, sizeof(buf.in), sizeof(buf.data));
 	if ((ret != sizeof(buf.data)) || (buf.data != PANIC_RESP_CHECK)) {
 		/* TODO maybe we shall check if M4/OMAP i2c broken */
-		KDEBUG(M4SH_ERROR, "m4sensorhub: Unknown IRQ status! "\
-				"M4 panic handoff ret=%d, data=0x%x\n",\
-				ret, buf.data);
+		KDEBUG(M4SH_ERROR, "m4sensorhub: %s ret=%d, data=0x%x\n",
+			"Unknown IRQ status! M4 panic handoff", ret, buf.data);
 		m4sensorhub_reg_access_unlock();
 		return;
 	}
 
-	KDEBUG(M4SH_ERROR, "m4sensorhub_panic: Detected M4 panic, reset M4!\n");
+	KDEBUG(M4SH_ERROR, "%s: Detected M4 panic, reset M4!\n", __func__);
 	m4sensorhub->pdev->hw_reset(m4sensorhub);
 	msleep(100);
-	if (m4sensorhub->i2c_client->addr == 0x39) {
+	if (m4sensorhub->i2c_client->addr == 0x39)
 		ret = m4sensorhub_401_load_firmware(m4sensorhub, 0, NULL);
-	} else {
+	else
 		ret = m4sensorhub_load_firmware(m4sensorhub, 0, NULL);
-	}
 
 	if (ret < 0) {
-		KDEBUG(M4SH_ERROR, "m4sensorhub_panic: "\
-			"Failed to restart M4, ret = %d\n", ret);
+		KDEBUG(M4SH_ERROR, "%s: Failed to restart M4, ret = %d\n",
+			__func__, ret);
 		BUG();
 	}
 
 	m4sensorhub_reg_access_unlock();
 
 	for (i = 0; i < PANICHDL_MAX; i++) {
-		handler = ((struct m4sensorhub_panicdata *)\
-				(m4sensorhub->panicdata))->funcs[i];
+		handler = ((struct m4sensorhub_panicdata *)
+			(m4sensorhub->panicdata))->funcs[i];
 		if (handler.callback) {
-			KDEBUG(M4SH_NOTICE, "m4sensorhub_panic: "\
-				"Calling %s as M4 restarted!\n",\
-				callback_name[i]);
+			KDEBUG(M4SH_NOTICE, "%s: Calling %s as M4 restarted!\n",
+				__func__, callback_name[i]);
 			handler.callback(m4sensorhub, handler.data);
 		}
 	}
diff --git a/drivers/mfd/m4sensorhub-reg.c b/drivers/mfd/m4sensorhub-reg.c
index d4e052aa752..9291f0d6e0e 100644
--- a/drivers/mfd/m4sensorhub-reg.c
+++ b/drivers/mfd/m4sensorhub-reg.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 Motorola, Inc.
+ * Copyright (C) 2012-2014 Motorola, Inc.
  *
  * 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
@@ -51,9 +51,13 @@ static DEFINE_MUTEX(reg_access);
 
      m4sensorhub - pointer to the main m4sensorhub data struct
 */
-
 int m4sensorhub_reg_init(struct m4sensorhub_data *m4sensorhub)
 {
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return -ENODATA;
+	}
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_reg_init);
@@ -66,9 +70,13 @@ EXPORT_SYMBOL_GPL(m4sensorhub_reg_init);
 
      m4sensorhub - pointer to the main m4sensorhub data struct
 */
-
 int m4sensorhub_reg_shutdown(struct m4sensorhub_data *m4sensorhub)
 {
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return -ENODATA;
+	}
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_reg_shutdown);
@@ -86,27 +94,33 @@ EXPORT_SYMBOL_GPL(m4sensorhub_reg_shutdown);
      value - array to return data.  Needs to be at least register's size
      num - number of bytes to read
 */
-
 int m4sensorhub_reg_read_n(struct m4sensorhub_data *m4sensorhub,
 			   enum m4sensorhub_reg reg, unsigned char *value,
 			   short num)
 {
 	int ret = -EINVAL;
 	u8 stack_buf[M4SH_MAX_STACK_BUF_SIZE];
+	u8 *buf = NULL;
 
-	if (!m4sensorhub || !value || !num) {
-		KDEBUG(M4SH_ERROR, "%s() invalid parameter\n", __func__);
-		return ret;
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return -ENODATA;
+	} else if (value == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: value array is NULL\n", __func__);
+		return -ENODATA;
+	} else if (num == 0) {
+		KDEBUG(M4SH_ERROR, "%s: Bytes requested is 0\n", __func__);
+		return -EINVAL;
 	}
 
-	if ((reg < M4SH_REG__NUM) && num <= M4SH_MAX_REG_SIZE &&\
+	if ((reg < M4SH_REG__NUM) && num <= M4SH_MAX_REG_SIZE &&
 	     register_info_tbl[reg].offset + num <=
 	     m4sensorhub_mapsize(reg)) {
-		u8 *buf = (num > (M4SH_MAX_STACK_BUF_SIZE-2))\
+		buf = (num > (M4SH_MAX_STACK_BUF_SIZE-2))
 			 ? kmalloc(num+2, GFP_KERNEL) : stack_buf;
 		if (!buf) {
-			KDEBUG(M4SH_ERROR, "%s() Failed alloc %d memeory\n"\
-					, __func__, num+2);
+			KDEBUG(M4SH_ERROR, "%s() Failed alloc %d memory\n",
+					__func__, num+2);
 			return -ENOMEM;
 		}
 		buf[0] = register_info_tbl[reg].type;
@@ -115,11 +129,13 @@ int m4sensorhub_reg_read_n(struct m4sensorhub_data *m4sensorhub,
 		mutex_lock(&reg_access);
 		ret = m4sensorhub_i2c_write_read(m4sensorhub, buf, 2, num);
 		mutex_unlock(&reg_access);
-
-		if (ret != num)
-			KDEBUG(M4SH_ERROR, "%s() read failure\n", __func__);
-		else
+		if (ret != num) {
+			KDEBUG(M4SH_ERROR, "%s() read failure (ret=%d)\n",
+				__func__, ret);
+		} else {
 			memcpy(value, buf, num);
+		}
+
 		if (buf != stack_buf)
 			kfree(buf);
 	} else {
@@ -128,6 +144,7 @@ int m4sensorhub_reg_read_n(struct m4sensorhub_data *m4sensorhub,
 			reg, M4SH_REG__NUM, num, M4SH_MAX_REG_SIZE,
 			m4sensorhub_mapsize(reg));
 	}
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_reg_read_n);
@@ -147,27 +164,34 @@ EXPORT_SYMBOL_GPL(m4sensorhub_reg_read_n);
 	    are to be changed, then &m4sh_no_mask can be passed here.
      num - number of bytes to write
 */
-
 int m4sensorhub_reg_write_n(struct m4sensorhub_data *m4sensorhub,
 			    enum m4sensorhub_reg reg, unsigned char *value,
 			    unsigned char *mask, short num)
 {
-	int i, ret = -EINVAL;
+	int i = 0;
+	int ret = -EINVAL;
 	u8 stack_buf[M4SH_MAX_STACK_BUF_SIZE];
+	u8 *buf = NULL;
 
-	if (!m4sensorhub || !value || !num) {
-		KDEBUG(M4SH_ERROR, "%s() invalid parameter\n", __func__);
-		return ret;
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return -ENODATA;
+	} else if (value == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: value array is NULL\n", __func__);
+		return -ENODATA;
+	} else if (num == 0) {
+		KDEBUG(M4SH_ERROR, "%s: Bytes requested is 0\n", __func__);
+		return -EINVAL;
 	}
 
-	if ((reg < M4SH_REG__NUM) && num <= M4SH_MAX_REG_SIZE &&\
+	if ((reg < M4SH_REG__NUM) && num <= M4SH_MAX_REG_SIZE &&
 	     register_info_tbl[reg].offset + num <=
 	     m4sensorhub_mapsize(reg)) {
-		u8 *buf = (num > (M4SH_MAX_STACK_BUF_SIZE-2))\
+		buf = (num > (M4SH_MAX_STACK_BUF_SIZE-2))
 			? kmalloc(num+2, GFP_KERNEL) : stack_buf;
 		if (!buf) {
-			KDEBUG(M4SH_ERROR, "%s() Failed alloc %d memeory\n"\
-					, __func__, num+2);
+			KDEBUG(M4SH_ERROR, "%s() Failed alloc %d memory\n",
+					__func__, num+2);
 			return -ENOMEM;
 		}
 
@@ -180,7 +204,8 @@ int m4sensorhub_reg_write_n(struct m4sensorhub_data *m4sensorhub,
 							 2, num);
 			if (ret != num) {
 				KDEBUG(M4SH_ERROR, "%s() register read"
-						   "failure\n", __func__);
+						   "failure (ret=%d)\n",
+						   __func__, ret);
 				goto error;
 			}
 			/* move data right 2 positions and apply mask and
@@ -200,10 +225,12 @@ int m4sensorhub_reg_write_n(struct m4sensorhub_data *m4sensorhub,
 			KDEBUG(M4SH_ERROR, "%s() register write failure\n",
 			      __func__);
 			ret = -EINVAL;
-		} else
+		} else {
 			ret -= 2;
+		}
 
-error:		mutex_unlock(&reg_access);
+error:
+		mutex_unlock(&reg_access);
 		if (buf != stack_buf)
 			kfree(buf);
 	} else {
@@ -230,11 +257,15 @@ EXPORT_SYMBOL_GPL(m4sensorhub_reg_write_n);
      value - byte of data to write
      mask - mask representing which bits to change in register.
 */
-
 int m4sensorhub_reg_write_1byte(struct m4sensorhub_data *m4sensorhub,
 				enum m4sensorhub_reg reg, unsigned char value,
 				unsigned char mask)
 {
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return -ENODATA;
+	}
+
 	if (register_info_tbl[reg].size == 1) {
 		if (mask == 0xFF) {
 			return m4sensorhub_reg_write(
@@ -245,6 +276,9 @@ int m4sensorhub_reg_write_1byte(struct m4sensorhub_data *m4sensorhub,
 				m4sensorhub, reg, &value, &mask
 				);
 		}
+	} else {
+		KDEBUG(M4SH_ERROR, "%s: Size is %hu instead of 1\n",
+			__func__, register_info_tbl[reg].size);
 	}
 	return -EINVAL;
 }
@@ -260,10 +294,14 @@ EXPORT_SYMBOL_GPL(m4sensorhub_reg_write_1byte);
      m4sensorhub - pointer to the main m4sensorhub data struct
      reg - Register to get size of
 */
-
 int m4sensorhub_reg_getsize(struct m4sensorhub_data *m4sensorhub,
 			    enum m4sensorhub_reg reg)
 {
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return -ENODATA;
+	}
+
 	if (reg < M4SH_REG__NUM)
 		return register_info_tbl[reg].size;
 
@@ -276,7 +314,6 @@ EXPORT_SYMBOL_GPL(m4sensorhub_reg_getsize);
    Lock reg access to avoid broken I2C transmit process
 
 */
-
 void m4sensorhub_reg_access_lock(void)
 {
 	mutex_lock(&reg_access);
@@ -288,7 +325,6 @@ EXPORT_SYMBOL_GPL(m4sensorhub_reg_access_lock);
    Unlock reg access to wake up blocked I2C transmit process
 
 */
-
 void m4sensorhub_reg_access_unlock(void)
 {
 	mutex_unlock(&reg_access);
@@ -298,7 +334,7 @@ EXPORT_SYMBOL_GPL(m4sensorhub_reg_access_unlock);
 /* m4sensorhub_i2c_write_read()
 
    Directly I2C access to communicate with the M4 sensor hub.
-   It always read after write if both write and read length are non-zero
+   Reads are always after writes if both write and read lengths are non-zero
 
    Returns number of bytes write on success if readlen is zero
    Returns number of bytes read on success if readlen is non-zero
@@ -309,29 +345,33 @@ EXPORT_SYMBOL_GPL(m4sensorhub_reg_access_unlock);
      writelen - number of bytes write to
      readlen - number of bytes read from
 */
-
 int m4sensorhub_i2c_write_read(struct m4sensorhub_data *m4sensorhub,
 				      u8 *buf, int writelen, int readlen)
 {
 	int i, msglen, msgstart, err, tries = 0;
 	char buffer[DEBUG_LINE_LENGTH];
-	struct i2c_msg msgs[] = {
-		{
-		.addr = m4sensorhub->i2c_client->addr,
-		.flags = m4sensorhub->i2c_client->flags,
-		.len = writelen,
-		.buf = buf,
-		},
-		{
-		.addr = m4sensorhub->i2c_client->addr,
-		.flags = m4sensorhub->i2c_client->flags | I2C_M_RD,
-		.len = readlen,
-		.buf = buf,
-		},
-	};
+	struct i2c_msg msgs[2];
+
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		return -ENODATA;
+	} else if (buf == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: Buffer is NULL\n", __func__);
+		return -ENODATA;
+	} else if ((writelen == 0) && (readlen == 0)) {
+		KDEBUG(M4SH_ERROR, "%s: Lengths are both 0\n", __func__);
+		return -EINVAL;
+	}
+
+	msgs[0].addr = m4sensorhub->i2c_client->addr;
+	msgs[0].flags = m4sensorhub->i2c_client->flags;
+	msgs[0].len = writelen;
+	msgs[0].buf = buf;
 
-	if (buf == NULL || (writelen == 0 && readlen == 0))
-		return -EFAULT;
+	msgs[1].addr = m4sensorhub->i2c_client->addr;
+	msgs[1].flags = m4sensorhub->i2c_client->flags | I2C_M_RD;
+	msgs[1].len = readlen;
+	msgs[1].buf = buf;
 
 	/* Offset and size in msgs array depending on msg type */
 	msglen = (writelen && readlen) ? 2 : 1;
@@ -352,11 +392,19 @@ int m4sensorhub_i2c_write_read(struct m4sensorhub_data *m4sensorhub,
 	do {
 		err = i2c_transfer(m4sensorhub->i2c_client->adapter,
 				   &msgs[msgstart], msglen);
-		if (err != msglen)
+		if (err != msglen) {
+			/* TODO: Print on try 0 after M4 stops NACKing */
+			if (tries != 0) {
+				KDEBUG(M4SH_ERROR,
+					"%s: Fail on try %d (err=%d)\n",
+					__func__, tries, err);
+			}
 			usleep_range(I2C_RETRY_DELAY_MIN, I2C_RETRY_DELAY_MAX);
+		}
 	} while ((err != msglen) && (++tries < I2C_RETRIES));
+
 	if (err != msglen) {
-		dev_err(&m4sensorhub->i2c_client->dev, "i2c transfer error; "
+		dev_err(&m4sensorhub->i2c_client->dev, "i2c transfer error: "
 			"type=%d offset=%d\n", buf[0], buf[1]);
 		err = -EIO;
 	} else {
@@ -376,6 +424,7 @@ int m4sensorhub_i2c_write_read(struct m4sensorhub_data *m4sensorhub,
 			KDEBUG(M4SH_VERBOSE_DEBUG, "%s\n", buffer);
 		}
 	}
+
 	return err;
 }
 EXPORT_SYMBOL_GPL(m4sensorhub_i2c_write_read);
diff --git a/drivers/mfd/m4sensorhub-stm32-fw.c b/drivers/mfd/m4sensorhub-stm32-fw.c
index bf5597840b4..3b3a366828c 100644
--- a/drivers/mfd/m4sensorhub-stm32-fw.c
+++ b/drivers/mfd/m4sensorhub-stm32-fw.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 Motorola, Inc.
+ * Copyright (C) 2012-2014 Motorola, Inc.
  *
  * 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
@@ -119,8 +119,15 @@ int m4sensorhub_load_firmware(struct m4sensorhub_data *m4sensorhub,
 	u32 barker_read_from_device;
 	int j = 0;
 
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		ret = -ENODATA;
+		goto done;
+	}
+
 	buf = kzalloc(MAX_TRANSFER_SIZE+8, GFP_KERNEL);
 	if (!buf) {
+		KDEBUG(M4SH_ERROR, "%s: Failed to allocate buf\n", __func__);
 		ret = -ENOMEM;
 		goto done;
 	}
@@ -130,12 +137,13 @@ int m4sensorhub_load_firmware(struct m4sensorhub_data *m4sensorhub,
 	}
 	if (ret < 0) {
 		KDEBUG(M4SH_ERROR, "%s: request_firmware failed for %s\n",
-			__func__,  m4sensorhub->filename);
+			__func__, m4sensorhub->filename);
 		KDEBUG(M4SH_ERROR, "Trying to run firmware already on hw.\n");
 		ret = 0;
 		goto done;
 	}
-	KDEBUG(M4SH_INFO, "%s:Firmware = %s\n", m4sensorhub->filename, __func__);
+	KDEBUG(M4SH_INFO, "%s: Firmware = %s\n",
+		__func__, m4sensorhub->filename);
 
 	if (firmware->size > MAX_FILE_SIZE) {
 		KDEBUG(M4SH_ERROR, "%s: firmware file size is too big.\n",
@@ -408,7 +416,7 @@ static int m4sensorhub_jump_to_user(struct m4sensorhub_data *m4sensorhub)
 				__func__, __LINE__);
 			return ret;
 		}
-		KDEBUG(M4SH_NOTICE, "Executing M4 code \n");
+		KDEBUG(M4SH_NOTICE, "Executing M4 code\n");
 		msleep(5000); /* 5 secs delay */
 		ret = 0;
 	} else {
diff --git a/drivers/mfd/m4sensorhub-stm32_401-fw.c b/drivers/mfd/m4sensorhub-stm32_401-fw.c
index 01a4161be9c..73ae3a31519 100644
--- a/drivers/mfd/m4sensorhub-stm32_401-fw.c
+++ b/drivers/mfd/m4sensorhub-stm32_401-fw.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2013 Motorola Mobility LLC.
+ * Copyright (C) 2013-2014 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
@@ -134,10 +134,17 @@ int m4sensorhub_401_load_firmware(struct m4sensorhub_data *m4sensorhub,
 	u16 fw_version_file, fw_version_device;
 	u32 barker_read_from_device;
 
+	if (m4sensorhub == NULL) {
+		KDEBUG(M4SH_ERROR, "%s: M4 data is NULL\n", __func__);
+		ret = -ENODATA;
+		goto done;
+	}
+
 	m4sensorhub_hw_reset(m4sensorhub);
 
 	buf = kzalloc(MAX_TRANSFER_SIZE, GFP_KERNEL);
 	if (!buf) {
+		KDEBUG(M4SH_ERROR, "%s: Failed to allocate buf\n", __func__);
 		ret = -ENOMEM;
 		goto done;
 	}
@@ -153,8 +160,8 @@ int m4sensorhub_401_load_firmware(struct m4sensorhub_data *m4sensorhub,
 		ret = 0;
 		goto done;
 	}
-	 KDEBUG(M4SH_INFO, "%s: Filename = %s",
-		__func__, m4sensorhub->filename);
+
+	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",
@@ -196,7 +203,7 @@ int m4sensorhub_401_load_firmware(struct m4sensorhub_data *m4sensorhub,
 
 			if (fw_version_file == fw_version_device) {
 				KDEBUG(M4SH_NOTICE,
-					"Version of firmware on device is 0x%4x\n",
+					"Version of firmware on device is 0x%04x\n",
 					fw_version_device);
 				KDEBUG(M4SH_NOTICE,
 					"Firmware on device same as file, not loading firmware.\n");
@@ -205,23 +212,23 @@ int m4sensorhub_401_load_firmware(struct m4sensorhub_data *m4sensorhub,
 			/* 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",
+				"Version of firmware on device is 0x%04x\n",
 				fw_version_device);
 			KDEBUG(M4SH_ERROR,
-				"Version of firmware on file is 0x%4x\n",
+				"Version of firmware on file is 0x%04x\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",
+		KDEBUG(M4SH_NOTICE, "Version of firmware on file is 0x%04x\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");
+		pr_err("%s: erase failed\n", __func__);
 		/* TODO : Not sure if this is critical error
 		goto done; */
 	}
@@ -241,8 +248,8 @@ int m4sensorhub_401_load_firmware(struct m4sensorhub_data *m4sensorhub,
 
 		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__);
+			KDEBUG(M4SH_ERROR, "%s : %d : error writing %d\n",
+				__func__, __LINE__, address_to_write);
 			ret = -EINVAL;
 			goto done;
 		}
@@ -678,6 +685,8 @@ static int m4sensorhub_jump_to_user(struct m4sensorhub_data *m4sensorhub)
 	    (u8 *)&barker_read_from_device, 4) != 0) {
 		KDEBUG(M4SH_ERROR, "%s : %d : error reading from device\n",
 		       __func__, __LINE__);
+		KDEBUG(M4SH_ERROR, "*** Not executing M4 code ***\n");
+		return -EIO;
 	}
 
 	if (barker_read_from_device == BARKER_NUMBER) {
diff --git a/include/linux/m4sensorhub.h b/include/linux/m4sensorhub.h
index 4b0cdd62eb8..0559eb980f0 100644
--- a/include/linux/m4sensorhub.h
+++ b/include/linux/m4sensorhub.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012, Motorola, Inc. All Rights Reserved.
+ * Copyright (c) 2012-2014, Motorola, Inc. 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 version 2 as
@@ -37,10 +37,6 @@ extern char m4sensorhub_debug;
 			printk(KERN_CRIT format, ##s);	\
 	} while (0)
 
-#define CHECK_REG_ACCESS_RETVAL(m4sensorhub, retval, reg) \
-		((retval == m4sensorhub_reg_getsize(m4sensorhub, reg)) \
-		 ? 0 : -EFAULT);
-
 enum m4sensorhub_debug_level {
 	M4SH_NODEBUG = 0x0,
 	M4SH_CRITICAL,
@@ -68,11 +64,15 @@ enum m4sensorhub_bootmode {
 /* This enum is used to register M4 panic callback
  * The sequence of this enum is also the sequence of calling
  *   i.e. it will be called follow this enum 0, 1, 2 ... max
-*/
+ */
 enum m4sensorhub_panichdl_index {
 	PANICHDL_DISPLAY_RESTORE,
-	/* Please add enum before PANICHDL_IRQ_RESTORE
-	   to make sure IRQ resotre will be called at last
+	/*
+	 * Please add enum before PANICHDL_IRQ_RESTORE
+	 * to make sure IRQ restore will be called last.
+	 *
+	 * Also, add your debug string name to
+	 * m4sensorhub-panic.c.
 	 */
 	PANICHDL_IRQ_RESTORE, /* Keep it as the last one */
 	PANICHDL_MAX = PANICHDL_IRQ_RESTORE+1
@@ -226,4 +226,3 @@ int m4sensorhub_irq_disable_all(struct m4sensorhub_data *m4sensorhub);
 
 #endif /* __KERNEL__ */
 #endif  /* __M4SENSORHUB_H__ */
-