IKXCLOCK-1762: Fix m4 panic handlers

Change-Id: I53820435227388c323d2beec6e678a94b674fff9

11 files changed, 143 insertions, 20 deletions

diff --git a/drivers/mfd/m4sensorhub-panic.c b/drivers/mfd/m4sensorhub-panic.c
index 725c2ff80c0..0f8693e71eb 100644
--- a/drivers/mfd/m4sensorhub-panic.c
+++ b/drivers/mfd/m4sensorhub-panic.c
@@ -34,6 +34,11 @@
 static const char *callback_name[PANICHDL_MAX] = {
 	[PANICHDL_DISPLAY_RESTORE] = "display_restore",
 	[PANICHDL_IRQ_RESTORE] = "irq_restore",
+	[PANICHDL_HEARTRATE_RESTORE] = "heartrate_restore",
+	[PANICHDL_PASSIVE_RESTORE] = "passive_restore",
+	[PANICHDL_ALS_RESTORE] = "als_restore",
+	[PANICHDL_FUSION_RESTORE] = "fusion_restore",
+	[PANICHDL_MPU9150_RESTORE] = "mpu9150_restore",
 };
 
 struct m4sensorhub_panic_callback {
@@ -288,19 +293,16 @@ void m4sensorhub_panic_process(struct m4sensorhub_data *m4sensorhub)
 	}
 
 	KDEBUG(M4SH_ERROR, "%s: Detected M4 panic, reset M4!\n", __func__);
-	m4sensorhub->pdev->hw_reset(m4sensorhub);
 	msleep(100);
 	if (m4sensorhub->i2c_client->addr == 0x39)
 		ret = m4sensorhub_401_load_firmware(m4sensorhub, 0, NULL);
 	else
 		ret = m4sensorhub_load_firmware(m4sensorhub, 0, NULL);
-
 	if (ret < 0) {
 		KDEBUG(M4SH_ERROR, "%s: Failed to restart M4, ret = %d\n",
 			__func__, ret);
 		BUG();
 	}
-
 	m4sensorhub_reg_access_unlock();
 
 	for (i = 0; i < PANICHDL_MAX; i++) {
diff --git a/drivers/mfd/m4sensorhub-stm32-fw.c b/drivers/mfd/m4sensorhub-stm32-fw.c
index 3b3a366828c..ce29cf40d1b 100644
--- a/drivers/mfd/m4sensorhub-stm32-fw.c
+++ b/drivers/mfd/m4sensorhub-stm32-fw.c
@@ -125,6 +125,8 @@ int m4sensorhub_load_firmware(struct m4sensorhub_data *m4sensorhub,
 		goto done;
 	}
 
+	m4sensorhub_hw_reset(m4sensorhub);
+
 	buf = kzalloc(MAX_TRANSFER_SIZE+8, GFP_KERNEL);
 	if (!buf) {
 		KDEBUG(M4SH_ERROR, "%s: Failed to allocate buf\n", __func__);
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 2f037745c9b..8bc71936bb7 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -58,8 +58,6 @@ obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_mpu9150.o
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_pedometer.o
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_gesture.o
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_stillmode.o
-obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_download.o
-obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_display.o
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_passive.o
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_als.o
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_heartrate.o
diff --git a/drivers/misc/m4sensorhub_als.c b/drivers/misc/m4sensorhub_als.c
index fbb7c110672..781e74fc07e 100644
--- a/drivers/misc/m4sensorhub_als.c
+++ b/drivers/misc/m4sensorhub_als.c
@@ -268,6 +268,29 @@ m4als_create_m4eventdev_fail:
 	return err;
 }
 
+static void m4als_panic_restore(struct m4sensorhub_data *m4sensorhub,
+				void *data)
+{
+	int size, err;
+	struct m4als_driver_data *dd = (struct m4als_driver_data *)data;
+	
+	if (dd == NULL) {
+		m4als_err("%s: Driver data is null, unable to restore\n");
+		return;
+	}
+
+	size = m4sensorhub_reg_getsize(dd->m4,
+				       M4SH_REG_LIGHTSENSOR_SAMPLERATE);
+	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_LIGHTSENSOR_SAMPLERATE,
+				   (char *)&dd->samplerate, m4sh_no_mask);
+	if (err < 0) {
+		m4als_err("%s: Failed to set sample rate.\n", __func__);
+	} else if (err != size) {
+		m4als_err("%s: Wrote %d bytes instead of %d.\n",
+			  __func__, err, size);
+	}
+}
+
 static int m4als_driver_init(struct init_calldata *p_arg)
 {
 	struct m4als_driver_data *dd = p_arg->p_data;
@@ -294,6 +317,10 @@ static int m4als_driver_init(struct init_calldata *p_arg)
 		goto m4als_driver_init_irq_fail;
 	}
 
+	err = m4sensorhub_panic_register(dd->m4, PANICHDL_ALS_RESTORE,
+					 m4als_panic_restore, dd);
+	if (err < 0)
+		KDEBUG(M4SH_ERROR, "Als panic callback register failed\n");
 	goto m4als_driver_init_exit;
 
 m4als_driver_init_irq_fail:
diff --git a/drivers/misc/m4sensorhub_display.c b/drivers/misc/m4sensorhub_display.c
index 0c8cf733337..bca7829c97a 100644
--- a/drivers/misc/m4sensorhub_display.c
+++ b/drivers/misc/m4sensorhub_display.c
@@ -475,7 +475,6 @@ static int __exit display_client_remove(struct platform_device *pdev)
 	struct display_client *display_data =
 						platform_get_drvdata(pdev);
 
-	display_data->m4sensorhub->pdev->set_display_control = NULL;
 	m4sensorhub_unregister_initcall(display_driver_init);
 	misc_deregister(&display_client_miscdrv);
 	global_display_data = NULL;
diff --git a/drivers/misc/m4sensorhub_fusion.c b/drivers/misc/m4sensorhub_fusion.c
index e426efe9021..99c80df1d33 100644
--- a/drivers/misc/m4sensorhub_fusion.c
+++ b/drivers/misc/m4sensorhub_fusion.c
@@ -308,6 +308,27 @@ m4fus_create_iiodev_exit:
 	return err;
 }
 
+static void m4fus_panic_restore(struct m4sensorhub_data *m4sensorhub,
+				void *data)
+{
+	int size, err;
+	struct m4fus_driver_data *dd = (struct m4fus_driver_data *)data;
+
+	if (dd == NULL) {
+		m4als_err("%s: Driver data is null, unable to restore\n");
+		return;
+	}
+	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_FUSION_SAMPLERATE);
+	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_FUSION_SAMPLERATE,
+				   (char *)&dd->samplerate, m4sh_no_mask);
+	if (err < 0) {
+		m4fus_err("%s: Failed to set sample rate.\n", __func__);
+	} else if (err != size) {
+		m4fus_err("%s:  Wrote %d bytes instead of %d.\n",
+			  __func__, err, size);
+	}
+}
+
 static int m4fus_driver_init(struct init_calldata *p_arg)
 {
 	struct iio_dev *iio = p_arg->p_data;
@@ -330,6 +351,11 @@ static int m4fus_driver_init(struct init_calldata *p_arg)
 		goto m4fus_driver_init_fail;
 	}
 
+	err = m4sensorhub_panic_register(dd->m4, PANICHDL_FUSION_RESTORE,
+					 m4fus_panic_restore, dd);
+	if (err < 0)
+		KDEBUG(M4SH_ERROR, "Fusion panic callback register failed\n");
+
 	goto m4fus_driver_init_exit;
 
 m4fus_driver_init_fail:
diff --git a/drivers/misc/m4sensorhub_heartrate.c b/drivers/misc/m4sensorhub_heartrate.c
index 839c6f939a8..67e5d9c92e0 100644
--- a/drivers/misc/m4sensorhub_heartrate.c
+++ b/drivers/misc/m4sensorhub_heartrate.c
@@ -480,6 +480,29 @@ m4hrt_create_iiodev_exit:
 	return err;
 }
 
+static void m4hrt_panic_restore(struct m4sensorhub_data *m4sensorhub,
+				void *data)
+{
+	int size, err;
+	struct m4hrt_driver_data *dd = (struct m4hrt_driver_data *)data;
+
+	if (dd == NULL) {
+		m4als_err("%s: Driver data is null, unable to restore\n");
+		return;
+	}
+	size = m4sensorhub_reg_getsize(dd->m4,
+				      M4SH_REG_HEARTRATE_APSAMPLERATE);
+
+	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_HEARTRATE_APSAMPLERATE,
+				   (char *)&dd->samplerate, m4sh_no_mask);
+	if (err < 0) {
+		m4hrt_err("%s: Failed to set sample rate.\n", __func__);
+	} else if (err != size) {
+		m4hrt_err("%s:  Wrote %d bytes instead of %d.\n",
+			  __func__, err, size);
+	}
+}
+
 static int m4hrt_driver_init(struct init_calldata *p_arg)
 {
 	struct iio_dev *iio = p_arg->p_data;
@@ -502,6 +525,11 @@ static int m4hrt_driver_init(struct init_calldata *p_arg)
 		goto m4hrt_driver_init_fail;
 	}
 
+	err = m4sensorhub_panic_register(dd->m4, PANICHDL_HEARTRATE_RESTORE,
+					m4hrt_panic_restore, dd);
+	if (err < 0)
+		KDEBUG(M4SH_ERROR, "HR panic callback register failed\n");
+
 	goto m4hrt_driver_init_exit;
 
 m4hrt_driver_init_fail:
diff --git a/drivers/misc/m4sensorhub_mpu9150.c b/drivers/misc/m4sensorhub_mpu9150.c
index 805842076cd..abd01b5c7f7 100644
--- a/drivers/misc/m4sensorhub_mpu9150.c
+++ b/drivers/misc/m4sensorhub_mpu9150.c
@@ -646,13 +646,38 @@ static int mpu9150_irq_enable_disable(struct mpu9150_client *mpu9150_client_data
 	return ret;
 }
 
+static void mpu9150_panic_restore(struct m4sensorhub_data *m4sensorhub,
+				void *data)
+{
+	struct mpu9150_client *dd = (struct mpu9150_client *)data;
+
+	if (dd == NULL) {
+		m4als_err("%s: Driver data is null, unable to restore\n");
+		return;
+	}
+	KDEBUG(M4SH_INFO, "Executing mpu9150 panic restore\n");
+	m4_set_mpu9150_delay(dd, dd->samplerate[TYPE_ACCEL], TYPE_ACCEL);
+	m4_set_mpu9150_delay(dd, dd->samplerate[TYPE_GYRO], TYPE_GYRO);
+	m4_set_mpu9150_delay(dd, dd->samplerate[TYPE_COMPASS], TYPE_COMPASS);
+}
+
 static int mpu9150_driver_init(struct init_calldata *p_arg)
 {
 	int ret;
 	ret = mpu9150_irq_init(misc_mpu9150_data);
+	if (ret < 0) {
+		KDEBUG(M4SH_ERROR, "mpu9150 irq init failed\n");
+		goto driver_init_exit;
+	}
 
+	ret = m4sensorhub_panic_register(misc_mpu9150_data->m4sensorhub,
+					 PANICHDL_MPU9150_RESTORE,
+					 mpu9150_panic_restore,
+					 misc_mpu9150_data);
 	if (ret < 0)
-		KDEBUG(M4SH_ERROR, "mpu9150 irq init failed\n");
+		KDEBUG(M4SH_ERROR, "HR panic callback register failed\n");
+
+driver_init_exit:
 	return ret;
 }
 
diff --git a/drivers/misc/m4sensorhub_passive.c b/drivers/misc/m4sensorhub_passive.c
index 797258a2169..39fb51db6cb 100644
--- a/drivers/misc/m4sensorhub_passive.c
+++ b/drivers/misc/m4sensorhub_passive.c
@@ -405,6 +405,26 @@ m4pas_create_iiodev_exit:
 	return err;
 }
 
+static void m4pas_panic_restore(struct m4sensorhub_data *m4sensorhub,
+				void *data)
+{
+	int err;
+	char en;
+	struct m4pas_driver_data *dd = (struct m4pas_driver_data *)data;
+
+	if (dd == NULL) {
+		m4als_err("%s: Driver data is null, unable to restore\n");
+		return;
+	}
+
+	en = (dd->samplerate >= 0) ? 1 : 0;
+	err = m4sensorhub_reg_write_1byte(dd->m4, M4SH_REG_PASSIVE_ENABLE,
+					  en, 0xFF);
+	if (err != 1)
+		m4pas_err("%s: Failed to enable with %d.\n",
+			  __func__, err);
+}
+
 static int m4pas_driver_init(struct init_calldata *p_arg)
 {
 	struct iio_dev *iio = p_arg->p_data;
@@ -427,6 +447,10 @@ static int m4pas_driver_init(struct init_calldata *p_arg)
 		goto m4pas_driver_init_fail;
 	}
 
+	err = m4sensorhub_panic_register(dd->m4, PANICHDL_PASSIVE_RESTORE,
+					 m4pas_panic_restore, dd);
+	if (err < 0)
+		KDEBUG(M4SH_ERROR, "Passive panic callback register failed\n");
 	goto m4pas_driver_init_exit;
 
 m4pas_driver_init_fail:
diff --git a/drivers/misc/m4sensorhub_stillmode.c b/drivers/misc/m4sensorhub_stillmode.c
index f0f95ae47c4..ec872d3ea0f 100644
--- a/drivers/misc/m4sensorhub_stillmode.c
+++ b/drivers/misc/m4sensorhub_stillmode.c
@@ -347,7 +347,6 @@ destroy_wakelock:
 	input_unregister_device(stillmode_client_data->input_dev);
 free_memory:
 	platform_set_drvdata(pdev, NULL);
-	m4sensorhub->pdev->stillmode_exit = NULL;
 	stillmode_client_data->m4sensorhub = NULL;
 	kfree(stillmode_client_data);
 	g_stillmode_data = NULL;
@@ -371,7 +370,6 @@ static int __exit stillmode_client_remove(struct platform_device *pdev)
 	m4sensorhub_unregister_initcall(stillmode_driver_init);
 	input_unregister_device(stillmode_client_data->input_dev);
 	platform_set_drvdata(pdev, NULL);
-	m4sensorhub->pdev->stillmode_exit = NULL;
 	stillmode_client_data->m4sensorhub = NULL;
 	kfree(stillmode_client_data);
 	g_stillmode_data = NULL;
diff --git a/include/linux/m4sensorhub.h b/include/linux/m4sensorhub.h
index 2e0de82aa1b..c78c0914770 100644
--- a/include/linux/m4sensorhub.h
+++ b/include/linux/m4sensorhub.h
@@ -67,6 +67,11 @@ enum m4sensorhub_bootmode {
  */
 enum m4sensorhub_panichdl_index {
 	PANICHDL_DISPLAY_RESTORE,
+	PANICHDL_HEARTRATE_RESTORE,
+	PANICHDL_PASSIVE_RESTORE,
+	PANICHDL_FUSION_RESTORE,
+	PANICHDL_ALS_RESTORE,
+	PANICHDL_MPU9150_RESTORE,
 	/*
 	 * Please add enum before PANICHDL_IRQ_RESTORE
 	 * to make sure IRQ restore will be called last.
@@ -89,16 +94,6 @@ struct init_calldata {
 	void *p_data;
 };
 
-struct m4sensorhub_platform_data {
-	int (*hw_init)(struct m4sensorhub_data *);
-	void (*hw_free)(struct m4sensorhub_data *);
-	void (*hw_reset)(struct m4sensorhub_data *);
-	int (*set_bootmode)(struct m4sensorhub_data *,
-			    enum m4sensorhub_bootmode);
-	int (*stillmode_exit)(void);
-	int (*set_display_control)(int m4_ctrl, int gpio_mipi_mux);
-};
-
 struct m4sensorhub_hwconfig {
 	int irq_gpio;
 	int reset_gpio;
@@ -113,7 +108,6 @@ struct m4sensorhub_data {
 	void *irqdata;
 	void *panicdata;
 	enum m4sensorhub_mode mode;
-	struct m4sensorhub_platform_data *pdev;
 	struct m4sensorhub_hwconfig hwconfig;
 	char *filename;
 	u16 fw_version;