From 930440dc319d1e37c2ab7e76c5880b0d9f1b3c61 Mon Sep 17 00:00:00 2001
From: Shantanu Jain <shjain@codeaurora.org>
Date: Thu, 17 Mar 2016 15:25:59 +0530
Subject: [PATCH 1/7] input: synaptics_dsx_2.6: add secure touch support for
 Synaptics V2.6

8953 QRD supports Synaptics touch controller. For synaptics_dsx_2.6 driver
to work with the TrustZone secure touch, all the touch interrupts must
be forwarded between the Linux Kernel and the TrustZone input driver.
Add APIs and configuration to support this.

Change-Id: I47520f9ba91b7645dff2c11b4c58a0b7aeed2765
Signed-off-by: Shantanu Jain <shjain@codeaurora.org>
---
 drivers/input/touchscreen/Kconfig             | 10 +++
 .../synaptics_dsx_2.6/synaptics_dsx_core.c    | 86 ++++++++++++++++++-
 .../synaptics_dsx_2.6/synaptics_dsx_core.h    | 12 +++
 3 files changed, 107 insertions(+), 1 deletion(-)

diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 69028bd45fdd..61a13ddabc5e 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -1177,4 +1177,14 @@ config TOUCHSCREEN_FT5X06
          To compile this driver as a module, choose M here: the
          module will be called ft5x06_ts.
 
+config SECURE_TOUCH_SYNAPTICS_DSX_V26
+	bool "Secure Touch support for Synaptics V2.6 Touchscreen"
+	depends on TOUCHSCREEN_SYNAPTICS_DSX_I2C_v26
+	help
+	  Say Y here
+	  -Synaptics DSX V2.6 touch driver is connected
+	  -To enable secure touch for Synaptics DSX V2.6 touch driver
+
+	  If unsure, say N.
+
 endif
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
index 07d2c479aa39..4b15935b2d4c 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
@@ -627,6 +627,67 @@ static struct kobj_attribute virtual_key_map_attr = {
 	.show = synaptics_rmi4_virtual_key_map_show,
 };
 
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+static void synaptics_secure_touch_init(struct synaptics_rmi4_data *data)
+{
+	init_completion(&data->st_powerdown);
+	init_completion(&data->st_irq_processed);
+}
+
+static void synaptics_secure_touch_notify(struct synaptics_rmi4_data *rmi4_data)
+{
+	sysfs_notify(&rmi4_data->input_dev->dev.kobj, NULL, "secure_touch");
+}
+
+static irqreturn_t synaptics_filter_interrupt(
+	struct synaptics_rmi4_data *rmi4_data)
+{
+	if (atomic_read(&rmi4_data->st_enabled)) {
+		if (atomic_cmpxchg(&rmi4_data->st_pending_irqs, 0, 1) == 0) {
+			reinit_completion(&rmi4_data->st_irq_processed);
+			synaptics_secure_touch_notify(rmi4_data);
+			wait_for_completion_interruptible(
+				&rmi4_data->st_irq_processed);
+		}
+		return IRQ_HANDLED;
+	}
+	return IRQ_NONE;
+}
+
+/*
+ * 'blocking' variable will have value 'true' when we want to prevent the driver
+ * from accessing the xPU/SMMU protected HW resources while the session is
+ * active.
+ */
+static void synaptics_secure_touch_stop(struct synaptics_rmi4_data *rmi4_data,
+					bool blocking)
+{
+	if (atomic_read(&rmi4_data->st_enabled)) {
+		atomic_set(&rmi4_data->st_pending_irqs, -1);
+		synaptics_secure_touch_notify(rmi4_data);
+		if (blocking)
+			wait_for_completion_interruptible(
+				&rmi4_data->st_powerdown);
+	}
+}
+
+#else
+static void synaptics_secure_touch_init(struct synaptics_rmi4_data *rmi4_data)
+{
+}
+
+static irqreturn_t synaptics_filter_interrupt(
+				struct synaptics_rmi4_data *rmi4_data)
+{
+	return IRQ_NONE;
+}
+
+static void synaptics_secure_touch_stop(struct synaptics_rmi4_data *rmi4_data,
+					bool blocking)
+{
+}
+#endif
+
 static ssize_t synaptics_rmi4_f01_reset_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
@@ -1512,6 +1573,9 @@ static irqreturn_t synaptics_rmi4_irq(int irq, void *data)
 	const struct synaptics_dsx_board_data *bdata =
 			rmi4_data->hw_if->board_data;
 
+	if (synaptics_filter_interrupt(data) == IRQ_HANDLED)
+		return IRQ_HANDLED;
+
 	if (gpio_get_value(bdata->irq_gpio) != bdata->irq_on_state)
 		goto exit;
 
@@ -3866,6 +3930,10 @@ static int synaptics_rmi4_probe(struct platform_device *pdev)
 	queue_work(rmi4_data->reset_workqueue, &rmi4_data->reset_work);
 #endif
 
+	/* Initialize secure touch */
+	synaptics_secure_touch_init(rmi4_data);
+	synaptics_secure_touch_stop(rmi4_data, true);
+
 	return retval;
 
 err_sysfs:
@@ -4106,7 +4174,9 @@ static int synaptics_rmi4_fb_notifier_cb(struct notifier_block *self,
 			fb_notifier);
 
 	if (evdata && evdata->data && rmi4_data) {
-		if (event == FB_EVENT_BLANK) {
+		if (event == FB_EARLY_EVENT_BLANK) {
+			synaptics_secure_touch_stop(rmi4_data, false);
+		} else if (event == FB_EVENT_BLANK) {
 			transition = evdata->data;
 			if (*transition == FB_BLANK_POWERDOWN) {
 				synaptics_rmi4_suspend(&rmi4_data->pdev->dev);
@@ -4133,6 +4203,14 @@ static void synaptics_rmi4_early_suspend(struct early_suspend *h)
 	if (rmi4_data->stay_awake)
 		return;
 
+	/*
+	 * During early suspend/late resume, the driver doesn't access xPU/SMMU
+	 * protected HW resources. So, there is no compelling need to block,
+	 * but notifying the userspace that a power event has occurred is
+	 * enough. Hence 'blocking' variable can be set to false.
+	 */
+	synaptics_secure_touch_stop(rmi4_data, false);
+
 	if (rmi4_data->enable_wakeup_gesture) {
 		synaptics_rmi4_wakeup_gesture(rmi4_data, true);
 		enable_irq_wake(rmi4_data->irq);
@@ -4170,6 +4248,8 @@ static void synaptics_rmi4_late_resume(struct early_suspend *h)
 	if (rmi4_data->stay_awake)
 		return;
 
+	synaptics_secure_touch_stop(rmi4_data, false);
+
 	if (rmi4_data->enable_wakeup_gesture) {
 		synaptics_rmi4_wakeup_gesture(rmi4_data, false);
 		disable_irq_wake(rmi4_data->irq);
@@ -4216,6 +4296,8 @@ static int synaptics_rmi4_suspend(struct device *dev)
 	if (rmi4_data->stay_awake)
 		return 0;
 
+	synaptics_secure_touch_stop(rmi4_data, true);
+
 	if (rmi4_data->enable_wakeup_gesture) {
 		synaptics_rmi4_wakeup_gesture(rmi4_data, true);
 		enable_irq_wake(rmi4_data->irq);
@@ -4253,6 +4335,8 @@ static int synaptics_rmi4_resume(struct device *dev)
 	if (rmi4_data->stay_awake)
 		return 0;
 
+	synaptics_secure_touch_stop(rmi4_data, true);
+
 	if (rmi4_data->enable_wakeup_gesture) {
 		synaptics_rmi4_wakeup_gesture(rmi4_data, false);
 		disable_irq_wake(rmi4_data->irq);
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
index 0de0e9905cb4..0bab0c233a12 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
@@ -48,6 +48,12 @@
 #include <linux/earlysuspend.h>
 #endif
 
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+#include <linux/completion.h>
+#include <linux/atomic.h>
+#include <linux/pm_runtime.h>
+#endif
+
 #if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 38))
 #define KERNEL_ABOVE_2_6_38
 #endif
@@ -374,6 +380,12 @@ struct synaptics_rmi4_data {
 			bool enable);
 	void (*report_touch)(struct synaptics_rmi4_data *rmi4_data,
 			struct synaptics_rmi4_fn *fhandler);
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+	atomic_t st_enabled;
+	atomic_t st_pending_irqs;
+	struct completion st_powerdown;
+	struct completion st_irq_processed;
+#endif
 };
 
 struct synaptics_dsx_bus_access {

From 52344fa3c6a2de9d51313ca07e4ad166f3b6bdcf Mon Sep 17 00:00:00 2001
From: Shantanu Jain <shjain@codeaurora.org>
Date: Thu, 17 Mar 2016 16:36:09 +0530
Subject: [PATCH 2/7] input: synaptics_dsx_2.6: creation of secure touch sysfs
 ifiles

This patch creates two sysfs files for secure touch - secure_touch
and secure_touch_enable, which will be accessed by secure UI app.
We also define the store and show function for these sysfs files.

Change-Id: Id43118120d4a1f0682904f48b3584d3ba62ef1dd
Signed-off-by: Shantanu Jain <shjain@codeaurora.org>
---
 .../synaptics_dsx_2.6/synaptics_dsx_core.c    | 136 ++++++++++++++++++
 .../synaptics_dsx_2.6/synaptics_dsx_core.h    |  15 ++
 .../synaptics_dsx_2.6/synaptics_dsx_i2c.c     |  27 ++++
 3 files changed, 178 insertions(+)

diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
index 4b15935b2d4c..3334d73f0fd7 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
@@ -172,6 +172,19 @@ static ssize_t synaptics_rmi4_wake_gesture_store(struct device *dev,
 static ssize_t synaptics_rmi4_virtual_key_map_show(struct kobject *kobj,
 		struct kobj_attribute *attr, char *buf);
 
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+static ssize_t synaptics_rmi4_secure_touch_enable_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t synaptics_rmi4_secure_touch_enable_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t synaptics_rmi4_secure_touch_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+#endif
+
+static irqreturn_t synaptics_rmi4_irq(int irq, void *data);
+
 struct synaptics_rmi4_f01_device_status {
 	union {
 		struct {
@@ -617,6 +630,14 @@ static struct device_attribute attrs[] = {
 	__ATTR(wake_gesture, (S_IRUGO | S_IWUSR | S_IWGRP),
 			synaptics_rmi4_wake_gesture_show,
 			synaptics_rmi4_wake_gesture_store),
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+	__ATTR(secure_touch_enable, (S_IRUGO | S_IWUSR | S_IWGRP),
+			synaptics_rmi4_secure_touch_enable_show,
+			synaptics_rmi4_secure_touch_enable_store),
+	__ATTR(secure_touch, S_IRUGO,
+			synaptics_rmi4_secure_touch_show,
+			NULL),
+#endif
 };
 
 static struct kobj_attribute virtual_key_map_attr = {
@@ -688,6 +709,121 @@ static void synaptics_secure_touch_stop(struct synaptics_rmi4_data *rmi4_data,
 }
 #endif
 
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+static ssize_t synaptics_rmi4_secure_touch_enable_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d",
+			atomic_read(&rmi4_data->st_enabled));
+}
+/*
+ * Accept only "0" and "1" valid values.
+ * "0" will reset the st_enabled flag, then wake up the reading process and
+ * the interrupt handler.
+ * The bus driver is notified via pm_runtime that it is not required to stay
+ * awake anymore.
+ * It will also make sure the queue of events is emptied in the controller,
+ * in case a touch happened in between the secure touch being disabled and
+ * the local ISR being ungated.
+ * "1" will set the st_enabled flag and clear the st_pending_irqs flag.
+ * The bus driver is requested via pm_runtime to stay awake.
+ */
+static ssize_t synaptics_rmi4_secure_touch_enable_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+	unsigned long value;
+	int err = 0;
+
+	if (count > 2)
+		return -EINVAL;
+
+	err = kstrtoul(buf, 10, &value);
+	if (err != 0)
+		return err;
+
+	err = count;
+
+	switch (value) {
+	case 0:
+		if (atomic_read(&rmi4_data->st_enabled) == 0)
+			break;
+
+		synaptics_rmi4_bus_put(rmi4_data);
+		atomic_set(&rmi4_data->st_enabled, 0);
+		synaptics_secure_touch_notify(rmi4_data);
+		complete(&rmi4_data->st_irq_processed);
+		synaptics_rmi4_irq(rmi4_data->irq, rmi4_data);
+		complete(&rmi4_data->st_powerdown);
+
+		break;
+	case 1:
+		if (atomic_read(&rmi4_data->st_enabled)) {
+			err = -EBUSY;
+			break;
+		}
+
+		synchronize_irq(rmi4_data->irq);
+
+		if (synaptics_rmi4_bus_get(rmi4_data) < 0) {
+			dev_err(
+				rmi4_data->pdev->dev.parent,
+				"synaptics_rmi4_bus_get failed\n");
+			err = -EIO;
+			break;
+		}
+		reinit_completion(&rmi4_data->st_powerdown);
+		reinit_completion(&rmi4_data->st_irq_processed);
+		atomic_set(&rmi4_data->st_enabled, 1);
+		atomic_set(&rmi4_data->st_pending_irqs,  0);
+		break;
+	default:
+		dev_err(
+			rmi4_data->pdev->dev.parent,
+			"unsupported value: %lu\n", value);
+		err = -EINVAL;
+		break;
+	}
+	return err;
+}
+
+/*
+ * This function returns whether there are pending interrupts, or
+ * other error conditions that need to be signaled to the userspace library,
+ * according tot he following logic:
+ * - st_enabled is 0 if secure touch is not enabled, returning -EBADF
+ * - st_pending_irqs is -1 to signal that secure touch is in being stopped,
+ *   returning -EINVAL
+ * - st_pending_irqs is 1 to signal that there is a pending irq, returning
+ *   the value "1" to the sysfs read operation
+ * - st_pending_irqs is 0 (only remaining case left) if the pending interrupt
+ *   has been processed, so the interrupt handler can be allowed to continue.
+ */
+static ssize_t synaptics_rmi4_secure_touch_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+	int val = 0;
+
+	if (atomic_read(&rmi4_data->st_enabled) == 0)
+		return -EBADF;
+
+	if (atomic_cmpxchg(&rmi4_data->st_pending_irqs, -1, 0) == -1)
+		return -EINVAL;
+
+	if (atomic_cmpxchg(&rmi4_data->st_pending_irqs, 1, 0) == 1)
+		val = 1;
+	else
+		complete(&rmi4_data->st_irq_processed);
+
+	return scnprintf(buf, PAGE_SIZE, "%u", val);
+
+}
+#endif
+
 static ssize_t synaptics_rmi4_f01_reset_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
index 0bab0c233a12..f0d8c124bac6 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
@@ -394,6 +394,10 @@ struct synaptics_dsx_bus_access {
 		unsigned char *data, unsigned short length);
 	int (*write)(struct synaptics_rmi4_data *rmi4_data, unsigned short addr,
 		unsigned char *data, unsigned short length);
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+	int (*get)(struct synaptics_rmi4_data *rmi4_data);
+	void (*put)(struct synaptics_rmi4_data *rmi4_data);
+#endif
 };
 
 struct synaptics_dsx_hw_interface {
@@ -444,6 +448,17 @@ static inline int synaptics_rmi4_reg_write(
 	return rmi4_data->hw_if->bus_access->write(rmi4_data, addr, data, len);
 }
 
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+static inline int synaptics_rmi4_bus_get(struct synaptics_rmi4_data *rmi4_data)
+{
+	return rmi4_data->hw_if->bus_access->get(rmi4_data);
+}
+static inline void synaptics_rmi4_bus_put(struct synaptics_rmi4_data *rmi4_data)
+{
+	rmi4_data->hw_if->bus_access->put(rmi4_data);
+}
+#endif
+
 static inline ssize_t synaptics_rmi4_show_error(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
index 784de607aa08..56c5d5e1b9f2 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
@@ -497,10 +497,37 @@ exit:
 	return retval;
 }
 
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+static int synaptics_rmi4_i2c_get(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+	retval = pm_runtime_get_sync(i2c->adapter->dev.parent);
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	return retval;
+}
+
+static void synaptics_rmi4_i2c_put(struct synaptics_rmi4_data *rmi4_data)
+{
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+	pm_runtime_put_sync(i2c->adapter->dev.parent);
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+}
+#endif
+
 static struct synaptics_dsx_bus_access bus_access = {
 	.type = BUS_I2C,
 	.read = synaptics_rmi4_i2c_read,
 	.write = synaptics_rmi4_i2c_write,
+#if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+	.get = synaptics_rmi4_i2c_get,
+	.put = synaptics_rmi4_i2c_put,
+#endif
 };
 
 static void synaptics_rmi4_i2c_dev_release(struct device *dev)

From 500142142449d0bbe9936df20212b9720c6e8d51 Mon Sep 17 00:00:00 2001
From: Mao Li <maol@codeaurora.org>
Date: Tue, 22 Mar 2016 17:48:04 +0800
Subject: [PATCH 3/7] input: synaptics_dsx_2.6: remove fake touch points

Update F12 touch report function with adding input_sync to
correct the pointer reporting status.

Change-Id: Iefe24525a9551736a6044697b447a25e2c5c65ca
Signed-off-by: Mao Li <maol@codeaurora.org>
---
 .../input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c  | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
index 3334d73f0fd7..f0bf062c8a3c 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
@@ -1370,7 +1370,6 @@ static int synaptics_rmi4_f12_abs_report(struct synaptics_rmi4_data *rmi4_data,
 #ifndef TYPE_B_PROTOCOL
 			input_mt_sync(rmi4_data->input_dev);
 #endif
-			input_sync(rmi4_data->input_dev);
 
 			dev_dbg(rmi4_data->pdev->dev.parent,
 					"%s: Finger %d: status = 0x%02x, x = %d, y = %d, wx = %d, wy = %d\n",
@@ -1442,7 +1441,6 @@ static int synaptics_rmi4_f12_abs_report(struct synaptics_rmi4_data *rmi4_data,
 #ifndef TYPE_B_PROTOCOL
 		input_mt_sync(rmi4_data->input_dev);
 #endif
-		input_sync(rmi4_data->input_dev);
 
 		if (rmi4_data->stylus_enable) {
 			stylus_presence = 0;
@@ -1458,6 +1456,8 @@ static int synaptics_rmi4_f12_abs_report(struct synaptics_rmi4_data *rmi4_data,
 		}
 	}
 
+	input_sync(rmi4_data->input_dev);
+
 	mutex_unlock(&(rmi4_data->rmi4_report_mutex));
 
 	return touch_count;

From a0732e6085b0b0f62444edf5b4d0e076de130d6b Mon Sep 17 00:00:00 2001
From: Shantanu Jain <shjain@codeaurora.org>
Date: Mon, 21 Mar 2016 14:09:27 +0530
Subject: [PATCH 4/7] input: synaptics_dsx_2.6: Add support for clocks for
 secure touch

Because of a change in the underlying bus driver, the secure touch
layer in the input drivers is now required to control directly the
clocks which are needed during the touch sessions. The clocks which
are associated with the underlying I2C bus are clk_gcc_blsp1_ahb_clk
and clk_gcc_blsp1_qup3_i2c_apps_clk. These clocks are turned on/off
when the secure touch session is started/ended.

CRs-Fixed: 990820
Change-Id: Ie2f6f5b736b27792e6e4dc2c39064e0ebdc03d7b
Signed-off-by: Shantanu Jain <shjain@codeaurora.org>
Signed-off-by: Abinaya P <abinayap@codeaurora.org>
---
 .../touchscreen/synaptics_dsxv26_i2c.txt      |  7 ++++
 drivers/input/touchscreen/Kconfig             | 10 ------
 .../touchscreen/synaptics_dsx_2.6/Kconfig     | 10 ++++++
 .../synaptics_dsx_2.6/synaptics_dsx_core.c    | 28 +++++++++++++++
 .../synaptics_dsx_2.6/synaptics_dsx_core.h    |  4 +++
 .../synaptics_dsx_2.6/synaptics_dsx_i2c.c     | 36 +++++++++++++++++++
 6 files changed, 85 insertions(+), 10 deletions(-)

diff --git a/Documentation/devicetree/bindings/input/touchscreen/synaptics_dsxv26_i2c.txt b/Documentation/devicetree/bindings/input/touchscreen/synaptics_dsxv26_i2c.txt
index 7dece8e06240..c10fd981df2b 100644
--- a/Documentation/devicetree/bindings/input/touchscreen/synaptics_dsxv26_i2c.txt
+++ b/Documentation/devicetree/bindings/input/touchscreen/synaptics_dsxv26_i2c.txt
@@ -25,6 +25,9 @@ Optional property:
  - synaptics,y-flip		       : modify orientation of the y axis.
  - synaptics,reset-delay-ms	   : reset delay for controller (ms), default 100.
  - synaptics,max-y-for-2d	   : maximal y value of the panel.
+ - clock-names			: Clock names used for secure touch. They are: "iface_clk", "core_clk"
+ - clocks			: Defined if 'clock-names' DT property is defined. These clocks
+				  are associated with the underlying I2C bus.
 
 Example:
 	i2c@78b7000 {
@@ -46,5 +49,9 @@ Example:
 			synaptics,max-y-for-2d = <1919>; /* remove if no virtual buttons */
 			synaptics,cap-button-codes = <139 172 158>;
 			synaptics,vir-button-codes = <139 180 2000 320 160 172 540 2000 320 160 158 900 2000 320 160>;
+			/* Underlying clocks used by secure touch */
+			clock-names = "iface_clk", "core_clk";
+			clocks = <&clock_gcc clk_gcc_blsp1_ahb_clk>,
+				<&clock_gcc clk_gcc_blsp1_qup3_i2c_apps_clk>;
 		};
 	};
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 61a13ddabc5e..69028bd45fdd 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -1177,14 +1177,4 @@ config TOUCHSCREEN_FT5X06
          To compile this driver as a module, choose M here: the
          module will be called ft5x06_ts.
 
-config SECURE_TOUCH_SYNAPTICS_DSX_V26
-	bool "Secure Touch support for Synaptics V2.6 Touchscreen"
-	depends on TOUCHSCREEN_SYNAPTICS_DSX_I2C_v26
-	help
-	  Say Y here
-	  -Synaptics DSX V2.6 touch driver is connected
-	  -To enable secure touch for Synaptics DSX V2.6 touch driver
-
-	  If unsure, say N.
-
 endif
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/Kconfig b/drivers/input/touchscreen/synaptics_dsx_2.6/Kconfig
index 78b995ec7c8a..53896288ba77 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/Kconfig
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/Kconfig
@@ -114,4 +114,14 @@ config TOUCHSCREEN_SYNAPTICS_DSX_VIDEO_v26
 	  To compile this driver as a module, choose M here: the
 	  module will be called synaptics_dsx_video.
 
+config SECURE_TOUCH_SYNAPTICS_DSX_V26
+	bool "Secure Touch support for Synaptics V2.6 Touchscreen"
+	depends on TOUCHSCREEN_SYNAPTICS_DSX_I2C_v26
+	help
+	  Say Y here
+	  -Synaptics DSX V2.6 touch driver is connected
+	  -To enable secure touch for Synaptics DSX V2.6 touch driver
+
+	  If unsure, say N.
+
 endif
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
index f0bf062c8a3c..f3e939c3686a 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
@@ -651,8 +651,33 @@ static struct kobj_attribute virtual_key_map_attr = {
 #if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
 static void synaptics_secure_touch_init(struct synaptics_rmi4_data *data)
 {
+	int ret = 0;
+
+	data->st_initialized = 0;
 	init_completion(&data->st_powerdown);
 	init_completion(&data->st_irq_processed);
+
+	/* Get clocks */
+	data->core_clk = devm_clk_get(data->pdev->dev.parent, "core_clk");
+	if (IS_ERR(data->core_clk)) {
+		ret = PTR_ERR(data->core_clk);
+		data->core_clk = NULL;
+		dev_warn(data->pdev->dev.parent,
+			"%s: error on clk_get(core_clk): %d\n", __func__, ret);
+		return;
+	}
+
+	data->iface_clk = devm_clk_get(data->pdev->dev.parent, "iface_clk");
+	if (IS_ERR(data->iface_clk)) {
+		ret = PTR_ERR(data->iface_clk);
+		data->iface_clk = NULL;
+		dev_warn(data->pdev->dev.parent,
+			"%s: error on clk_get(iface_clk): %d\n", __func__, ret);
+		return;
+	}
+
+	data->st_initialized = 1;
+	return;
 }
 
 static void synaptics_secure_touch_notify(struct synaptics_rmi4_data *rmi4_data)
@@ -745,6 +770,9 @@ static ssize_t synaptics_rmi4_secure_touch_enable_store(struct device *dev,
 	if (err != 0)
 		return err;
 
+	if (!rmi4_data->st_initialized)
+		return -EIO;
+
 	err = count;
 
 	switch (value) {
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
index f0d8c124bac6..25decbdf41d0 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
@@ -52,6 +52,7 @@
 #include <linux/completion.h>
 #include <linux/atomic.h>
 #include <linux/pm_runtime.h>
+#include <linux/clk.h>
 #endif
 
 #if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 38))
@@ -385,6 +386,9 @@ struct synaptics_rmi4_data {
 	atomic_t st_pending_irqs;
 	struct completion st_powerdown;
 	struct completion st_irq_processed;
+	bool st_initialized;
+	struct clk *core_clk;
+	struct clk *iface_clk;
 #endif
 };
 
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
index 56c5d5e1b9f2..fbf90f4ae62b 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
@@ -498,6 +498,34 @@ exit:
 }
 
 #if defined(CONFIG_SECURE_TOUCH_SYNAPTICS_DSX_V26)
+static int synaptics_rmi4_clk_prepare_enable(
+		struct synaptics_rmi4_data *rmi4_data)
+{
+	int ret;
+
+	ret = clk_prepare_enable(rmi4_data->iface_clk);
+	if (ret) {
+		dev_err(rmi4_data->pdev->dev.parent,
+			"error on clk_prepare_enable(iface_clk):%d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(rmi4_data->core_clk);
+	if (ret) {
+		clk_disable_unprepare(rmi4_data->iface_clk);
+		dev_err(rmi4_data->pdev->dev.parent,
+			"error clk_prepare_enable(core_clk):%d\n", ret);
+	}
+	return ret;
+}
+
+static void synaptics_rmi4_clk_disable_unprepare(
+		struct synaptics_rmi4_data *rmi4_data)
+{
+	clk_disable_unprepare(rmi4_data->core_clk);
+	clk_disable_unprepare(rmi4_data->iface_clk);
+}
+
 static int synaptics_rmi4_i2c_get(struct synaptics_rmi4_data *rmi4_data)
 {
 	int retval;
@@ -505,6 +533,12 @@ static int synaptics_rmi4_i2c_get(struct synaptics_rmi4_data *rmi4_data)
 
 	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
 	retval = pm_runtime_get_sync(i2c->adapter->dev.parent);
+	if (retval >= 0 && rmi4_data->core_clk != NULL &&
+				rmi4_data->iface_clk != NULL) {
+		retval = synaptics_rmi4_clk_prepare_enable(rmi4_data);
+		if (retval)
+			pm_runtime_put_sync(i2c->adapter->dev.parent);
+	}
 	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
 
 	return retval;
@@ -515,6 +549,8 @@ static void synaptics_rmi4_i2c_put(struct synaptics_rmi4_data *rmi4_data)
 	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
 
 	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+	if (rmi4_data->core_clk != NULL && rmi4_data->iface_clk != NULL)
+		synaptics_rmi4_clk_disable_unprepare(rmi4_data);
 	pm_runtime_put_sync(i2c->adapter->dev.parent);
 	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
 }

From b378a2f8ba08d3c529ccb7ed4b64d2b686543735 Mon Sep 17 00:00:00 2001
From: zhaoyuan <yzhao@codeaurora.org>
Date: Thu, 24 Mar 2016 10:36:00 +0800
Subject: [PATCH 5/7] input: synaptics: defer touch resume to workqueue for
 v2.6 driver

During device resume, the touch resume function is called after
display resume. Touch resume function will take about >200ms.
Defer the touch resume function to a workqueue to reduce the total
device resume time. An optional DT property is added to enable this
on targets that need this feature.

This change has been merged. Now, touch driver was updated to V2.6,
so, merge this change to V2.6 driver.

Change-Id: Ica477f35bd4fda59eb49c9b4f5e88b460366c761
Signed-off-by: zhaoyuan <yzhao@codeaurora.org>
---
 .../synaptics_dsx_2.6/synaptics_dsx_core.c    | 51 +++++++++++++++----
 .../synaptics_dsx_2.6/synaptics_dsx_core.h    |  3 +-
 .../synaptics_dsx_2.6/synaptics_dsx_i2c.c     |  5 +-
 include/linux/input/synaptics_dsx_v2_6.h      |  4 ++
 4 files changed, 51 insertions(+), 12 deletions(-)

diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
index f3e939c3686a..6e078002d087 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
@@ -122,6 +122,7 @@ static int synaptics_rmi4_reset_device(struct synaptics_rmi4_data *rmi4_data,
 		bool rebuild);
 
 #ifdef CONFIG_FB
+static void synaptics_rmi4_fb_notify_resume_work(struct work_struct *work);
 static int synaptics_rmi4_fb_notifier_cb(struct notifier_block *self,
 		unsigned long event, void *data);
 #endif
@@ -4012,6 +4013,8 @@ static int synaptics_rmi4_probe(struct platform_device *pdev)
 	}
 
 #ifdef CONFIG_FB
+	INIT_WORK(&rmi4_data->fb_notify_work,
+		  synaptics_rmi4_fb_notify_resume_work);
 	rmi4_data->fb_notifier.notifier_call = synaptics_rmi4_fb_notifier_cb;
 	retval = fb_register_client(&rmi4_data->fb_notifier);
 	if (retval < 0) {
@@ -4328,6 +4331,14 @@ static void synaptics_rmi4_wakeup_gesture(struct synaptics_rmi4_data *rmi4_data,
 }
 
 #ifdef CONFIG_FB
+static void synaptics_rmi4_fb_notify_resume_work(struct work_struct *work)
+{
+	struct synaptics_rmi4_data *rmi4_data =
+		container_of(work, struct synaptics_rmi4_data, fb_notify_work);
+	synaptics_rmi4_resume(&(rmi4_data->input_dev->dev));
+	rmi4_data->fb_ready = true;
+}
+
 static int synaptics_rmi4_fb_notifier_cb(struct notifier_block *self,
 		unsigned long event, void *data)
 {
@@ -4338,16 +4349,36 @@ static int synaptics_rmi4_fb_notifier_cb(struct notifier_block *self,
 			fb_notifier);
 
 	if (evdata && evdata->data && rmi4_data) {
-		if (event == FB_EARLY_EVENT_BLANK) {
-			synaptics_secure_touch_stop(rmi4_data, false);
-		} else if (event == FB_EVENT_BLANK) {
-			transition = evdata->data;
-			if (*transition == FB_BLANK_POWERDOWN) {
-				synaptics_rmi4_suspend(&rmi4_data->pdev->dev);
-				rmi4_data->fb_ready = false;
-			} else if (*transition == FB_BLANK_UNBLANK) {
-				synaptics_rmi4_resume(&rmi4_data->pdev->dev);
-				rmi4_data->fb_ready = true;
+		if (rmi4_data->hw_if->board_data->resume_in_workqueue) {
+			if (event == FB_EARLY_EVENT_BLANK) {
+				synaptics_secure_touch_stop(rmi4_data, false);
+			} else if (event == FB_EVENT_BLANK) {
+				transition = evdata->data;
+				if (*transition == FB_BLANK_POWERDOWN) {
+					flush_work(
+						&(rmi4_data->fb_notify_work));
+					synaptics_rmi4_suspend(
+						&rmi4_data->pdev->dev);
+					rmi4_data->fb_ready = false;
+				} else if (*transition == FB_BLANK_UNBLANK) {
+					schedule_work(
+						&(rmi4_data->fb_notify_work));
+				}
+			}
+		} else {
+			if (event == FB_EARLY_EVENT_BLANK) {
+				synaptics_secure_touch_stop(rmi4_data, false);
+			} else if (event == FB_EVENT_BLANK) {
+				transition = evdata->data;
+				if (*transition == FB_BLANK_POWERDOWN) {
+					synaptics_rmi4_suspend(
+						&rmi4_data->pdev->dev);
+					rmi4_data->fb_ready = false;
+				} else if (*transition == FB_BLANK_UNBLANK) {
+					synaptics_rmi4_resume(
+						&rmi4_data->pdev->dev);
+					rmi4_data->fb_ready = true;
+				}
 			}
 		}
 	}
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
index 25decbdf41d0..b860a553b334 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.h
@@ -5,7 +5,7 @@
  *
  * Copyright (C) 2012 Alexandra Chin <alexandra.chin@tw.synaptics.com>
  * Copyright (C) 2012 Scott Lin <scott.lin@tw.synaptics.com>
- *
+ * Copyright (C) 2016 The Linux Foundation. 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 as published by
  * the Free Software Foundation; either version 2 of the License, or
@@ -331,6 +331,7 @@ struct synaptics_rmi4_data {
 	struct delayed_work rb_work;
 	struct workqueue_struct *rb_workqueue;
 #ifdef CONFIG_FB
+	struct work_struct fb_notify_work;
 	struct notifier_block fb_notifier;
 	struct work_struct reset_work;
 	struct workqueue_struct *reset_workqueue;
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
index fbf90f4ae62b..f9230bfe8ea4 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
@@ -5,7 +5,7 @@
  *
  * Copyright (C) 2012 Alexandra Chin <alexandra.chin@tw.synaptics.com>
  * Copyright (C) 2012 Scott Lin <scott.lin@tw.synaptics.com>
- *
+ * Copyright (C) 2016, The Linux Foundation.  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 as published by
  * the Free Software Foundation; either version 2 of the License, or
@@ -78,6 +78,9 @@ static int parse_dt(struct device *dev, struct synaptics_dsx_board_data *bdata)
 	else
 		bdata->irq_on_state = value;
 
+	bdata->resume_in_workqueue = of_property_read_bool(np,
+			"synaptics,resume-in-workqueue");
+
 	retval = of_property_read_string(np, "synaptics,pwr-reg-name", &name);
 	if (retval < 0)
 		bdata->pwr_reg_name = NULL;
diff --git a/include/linux/input/synaptics_dsx_v2_6.h b/include/linux/input/synaptics_dsx_v2_6.h
index 2b91bc043f6f..5d4bbedb5f1a 100644
--- a/include/linux/input/synaptics_dsx_v2_6.h
+++ b/include/linux/input/synaptics_dsx_v2_6.h
@@ -5,6 +5,7 @@
  *
  * Copyright (C) 2012 Alexandra Chin <alexandra.chin@tw.synaptics.com>
  * Copyright (C) 2012 Scott Lin <scott.lin@tw.synaptics.com>
+ * Copyright (C) 2016, The Linux Foundation. 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 as published by
@@ -57,6 +58,7 @@ struct synaptics_dsx_button_map {
  * @x_flip: x flip flag
  * @y_flip: y flip flag
  * @swap_axes: swap axes flag
+ * @resume_in_workqueue: defer resume function to workqueue
  * @irq_gpio: attention interrupt GPIO
  * @irq_on_state: attention interrupt active state
  * @power_gpio: power switch GPIO
@@ -79,11 +81,13 @@ struct synaptics_dsx_button_map {
  * @bus_reg_name: pointer to name of regulator for bus pullup control
  * @cap_button_map: pointer to 0D button map
  * @vir_button_map: pointer to virtual button map
+ * @resume_in_workqueue: defer resume function to workqueue
  */
 struct synaptics_dsx_board_data {
 	bool x_flip;
 	bool y_flip;
 	bool swap_axes;
+	bool resume_in_workqueue;
 	int irq_gpio;
 	int irq_on_state;
 	int power_gpio;

From 11edf4f33c7a151ea9e4d5cbd21565792430ceba Mon Sep 17 00:00:00 2001
From: Mao Li <maol@codeaurora.org>
Date: Thu, 24 Mar 2016 14:20:21 +0800
Subject: [PATCH 6/7] input: synaptics_dsx_2.6: fix CTP power issue for QRD8953

Synaptics touch controller does not go to sleep in suspend because
L6 is still enabled. Disable L6 in suspend to ensure CTP go into
sleep mode.

Change-Id: I40f3f380c1a85c292029d4e22e49bebf25bd198c
Signed-off-by: Mao Li <maol@codeaurora.org>
---
 .../touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c   | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
index 6e078002d087..96b05075e9d6 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
@@ -4514,6 +4514,10 @@ exit:
 	}
 	mutex_unlock(&exp_data.mutex);
 
+	if (!rmi4_data->suspend) {
+		synaptics_rmi4_enable_reg(rmi4_data, false);
+		synaptics_rmi4_get_reg(rmi4_data, false);
+	}
 	rmi4_data->suspend = true;
 
 	return 0;
@@ -4540,6 +4544,11 @@ static int synaptics_rmi4_resume(struct device *dev)
 
 	rmi4_data->current_page = MASK_8BIT;
 
+	if (rmi4_data->suspend) {
+		synaptics_rmi4_get_reg(rmi4_data, true);
+		synaptics_rmi4_enable_reg(rmi4_data, true);
+	}
+
 	synaptics_rmi4_sleep_enable(rmi4_data, false);
 	synaptics_rmi4_irq_enable(rmi4_data, true, false);
 

From 29e0ce3c544d39b5f438a79f7949b55c59aeaece Mon Sep 17 00:00:00 2001
From: Shantanu Jain <shjain@codeaurora.org>
Date: Wed, 23 Mar 2016 16:28:50 +0530
Subject: [PATCH 7/7] input: synaptics_dsx_2.6: fix issues raised by static
 analyzer

Fix issues raised by static analyzer:
1. initialize "retval" before being returned from the driver function.
2. Check return value of the function create_singlethread_workqueue()
   and return -ENOMEM if it failed. If creation of the workqueue
   failed, then both queue_work() and queue_delayed_work() calls will
   later crash, as they expect the workqueue pointer to be a non-NULL
   value. Also add clean-up code for this.
3. check return value of the snprintf() call as array 'buf' of size
   16 may use index value(s) 16...20.

CRs-Fixed: 995687
Change-Id: I89d9f7cacbcf23de43a7e96556d1ac65911126d6
Signed-off-by: Shantanu Jain <shjain@codeaurora.org>
---
 .../synaptics_dsx_2.6/synaptics_dsx_core.c    | 33 ++++++++++++++++---
 .../synaptics_dsx_2.6/synaptics_dsx_i2c.c     |  2 +-
 2 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
index 96b05075e9d6..a0303ec3c26e 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_core.c
@@ -3140,7 +3140,9 @@ static int synaptics_rmi4_gpio_setup(int gpio, bool config, int dir, int state)
 	unsigned char buf[16];
 
 	if (config) {
-		snprintf(buf, PAGE_SIZE, "dsx_gpio_%u\n", gpio);
+		retval = snprintf(buf, ARRAY_SIZE(buf), "dsx_gpio_%u\n", gpio);
+		if (retval >= 16)
+			return -EINVAL;
 
 		retval = gpio_request(gpio, buf);
 		if (retval) {
@@ -4080,19 +4082,29 @@ static int synaptics_rmi4_probe(struct platform_device *pdev)
 
 	rmi4_data->rb_workqueue =
 			create_singlethread_workqueue("dsx_rebuild_workqueue");
+	if (!rmi4_data->rb_workqueue) {
+		retval = -ENOMEM;
+		goto err_rb_workqueue;
+	}
 	INIT_DELAYED_WORK(&rmi4_data->rb_work, synaptics_rmi4_rebuild_work);
 
 	exp_data.workqueue = create_singlethread_workqueue("dsx_exp_workqueue");
+	if (!exp_data.workqueue) {
+		retval = -ENOMEM;
+		goto err_exp_data_workqueue;
+	}
 	INIT_DELAYED_WORK(&exp_data.work, synaptics_rmi4_exp_fn_work);
 	exp_data.rmi4_data = rmi4_data;
 	exp_data.queue_work = true;
-	queue_delayed_work(exp_data.workqueue,
-			&exp_data.work,
-			0);
+	queue_delayed_work(exp_data.workqueue, &exp_data.work, 0);
 
 #ifdef FB_READY_RESET
 	rmi4_data->reset_workqueue =
 			create_singlethread_workqueue("dsx_reset_workqueue");
+	if (!rmi4_data->reset_workqueue) {
+		retval = -ENOMEM;
+		goto err_reset_workqueue;
+	}
 	INIT_WORK(&rmi4_data->reset_work, synaptics_rmi4_reset_work);
 	queue_work(rmi4_data->reset_workqueue, &rmi4_data->reset_work);
 #endif
@@ -4103,6 +4115,19 @@ static int synaptics_rmi4_probe(struct platform_device *pdev)
 
 	return retval;
 
+#ifdef FB_READY_RESET
+err_reset_workqueue:
+#endif
+	cancel_delayed_work_sync(&exp_data.work);
+	flush_workqueue(exp_data.workqueue);
+	destroy_workqueue(exp_data.workqueue);
+
+err_exp_data_workqueue:
+	cancel_delayed_work_sync(&rmi4_data->rb_work);
+	flush_workqueue(rmi4_data->rb_workqueue);
+	destroy_workqueue(rmi4_data->rb_workqueue);
+
+err_rb_workqueue:
 err_sysfs:
 	for (attr_count--; attr_count >= 0; attr_count--) {
 		sysfs_remove_file(&rmi4_data->input_dev->dev.kobj,
diff --git a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
index f9230bfe8ea4..f936f3c2ebb1 100644
--- a/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
+++ b/drivers/input/touchscreen/synaptics_dsx_2.6/synaptics_dsx_i2c.c
@@ -291,7 +291,7 @@ static void synaptics_rmi4_i2c_check_addr(struct synaptics_rmi4_data *rmi4_data,
 static int synaptics_rmi4_i2c_set_page(struct synaptics_rmi4_data *rmi4_data,
 		unsigned short addr)
 {
-	int retval;
+	int retval = 0;
 	unsigned char retry;
 	unsigned char buf[PAGE_SELECT_LEN];
 	unsigned char page;