From b2fd919bec3dd9fd7f0ac95363a733466be4252d Mon Sep 17 00:00:00 2001
From: Wenping Zhang <wenping.zhang@rock-chips.com>
Date: Sat, 19 Dec 2020 02:25:13 +0000
Subject: [PATCH] drivers: input: touch: fix bug system hang when cyttsp5 is
 not connected.

Signed-off-by: Wenping Zhang <wenping.zhang@rock-chips.com>
Change-Id: I7fd72a30b77ae3396c2220588794b03d78ed8642
---
 .../input/touchscreen/cyttsp5/cyttsp5_core.c  | 11 ----------
 .../input/touchscreen/cyttsp5/cyttsp5_i2c.c   | 22 ++++++++++++++++++-
 2 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/drivers/input/touchscreen/cyttsp5/cyttsp5_core.c b/drivers/input/touchscreen/cyttsp5/cyttsp5_core.c
index 25823fc74ce3..6b727bff8a21 100644
--- a/drivers/input/touchscreen/cyttsp5/cyttsp5_core.c
+++ b/drivers/input/touchscreen/cyttsp5/cyttsp5_core.c
@@ -6079,17 +6079,6 @@ int cyttsp5_probe(const struct cyttsp5_bus_ops *ops, struct device *dev,
 	 */
 	cyttsp5_bus_ops_save = ops;
 
-	/* Call platform init function */
-	if (cd->cpdata->init) {
-		parade_debug(cd->dev, DEBUG_LEVEL_1, "%s: Init HW\n", __func__);
-		rc = cd->cpdata->init(cd->cpdata, 1, cd->dev);
-	} else {
-		dev_info(cd->dev, "%s: No HW INIT function\n", __func__);
-		rc = 0;
-	}
-	if (rc < 0)
-		dev_err(cd->dev, "%s: HW Init fail r=%d\n", __func__, rc);
-
 	/* Call platform detect function */
 	if (cd->cpdata->detect) {
 		dev_info(cd->dev, "%s: Detect HW\n", __func__);
diff --git a/drivers/input/touchscreen/cyttsp5/cyttsp5_i2c.c b/drivers/input/touchscreen/cyttsp5/cyttsp5_i2c.c
index 9c34aab2d0c3..976db422b907 100644
--- a/drivers/input/touchscreen/cyttsp5/cyttsp5_i2c.c
+++ b/drivers/input/touchscreen/cyttsp5/cyttsp5_i2c.c
@@ -136,6 +136,7 @@ static int cyttsp5_i2c_probe(struct i2c_client *client,
 #ifdef CONFIG_TOUCHSCREEN_CYPRESS_CYTTSP5_DEVICETREE_SUPPORT
 	const struct of_device_id *match;
 #endif
+	struct cyttsp5_platform_data *pdata;
 	int rc;
 
 	printk("*****cyttsp5_i2c_probe start \n");
@@ -153,7 +154,26 @@ static int cyttsp5_i2c_probe(struct i2c_client *client,
 			return rc;
 	}
 #endif
-
+	pdata = dev_get_platdata(dev);
+	if (pdata && pdata->core_pdata) {
+		char buf[32];
+		/* Call platform init function */
+		if (pdata->core_pdata->init) {
+			dev_info(dev, "%s: Init HW\n", __func__);
+			rc = pdata->core_pdata->init(pdata->core_pdata, 1, dev);
+			if (rc < 0) {
+				dev_err(dev, "%s: HW Init fail %d\n", __func__, rc);
+				return -EIO;
+			}
+		} else {
+			dev_info(dev, "%s: No HW INIT function\n", __func__);
+		}
+		rc = cyttsp5_i2c_read_default(&client->dev, buf, 16);
+		if (rc < 0) {
+			dev_err(dev, "%s, read_default rc=%d\n", __func__, rc);
+			return -ENODEV;
+		}
+	}
 	rc = cyttsp5_probe(&cyttsp5_i2c_bus_ops, &client->dev, client->irq,
 			  CY_I2C_DATA_SIZE);