gpio: mxc: Fix clean up on probing

- Fix order of after-error clean procedures upon probing new device.

- Refactor handling of wakeup PAD setup.

Signed-off-by: Oleksandr Suvorov <oleksandr.suvorov@toradex.com>

1 files changed, 75 insertions, 44 deletions

diff --git a/drivers/gpio/gpio-mxc.c b/drivers/gpio/gpio-mxc.c
index d9eae362e0a5..27e1c51fa881 100644
--- a/drivers/gpio/gpio-mxc.c
+++ b/drivers/gpio/gpio-mxc.c
@@ -552,6 +552,58 @@ static void mxc_gpio_free(struct gpio_chip *chip, unsigned offset)
 	pm_runtime_put(chip->parent);
 }
 
+#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
+static int init_sc_ipc_handle(struct mxc_gpio_port *port)
+{
+	uint32_t mu_id;
+	sc_err_t sciErr;
+
+	if (!gpio_ipc_handle) {
+		sciErr = sc_ipc_getMuID(&mu_id);
+		if (sciErr != SC_ERR_NONE) {
+			dev_err(port->dev,
+				"can not obtain mu id: %d\n", sciErr);
+			return -EINVAL;
+		}
+
+		sciErr = sc_ipc_open(&gpio_ipc_handle, mu_id);
+		if (sciErr != SC_ERR_NONE) {
+			dev_err(port->dev,
+				"can not open mu channel to scu: %d\n", sciErr);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+/*
+ * parse pad wakeup info from dtb, each pad has to provide
+ * <pin_id, type, line>, these info should be put in each
+ * gpio node and with a "pad-wakeup-num" to indicate the
+ * total lines are with pad wakeup enabled.
+ */
+static int setup_pad_wakeup(struct mxc_gpio_port *port,
+			    struct device_node *np)
+{
+	int i;
+	int err = 0;
+
+	for (i = 0; i < port->pad_wakeup_num; i++) {
+		if ((err = of_property_read_u32_index(np, "pad-wakeup",
+			i * 3 + 0, &port->pad_wakeup[i].pin_id)))
+			break;
+		if ((err = of_property_read_u32_index(np, "pad-wakeup",
+			i * 3 + 1, &port->pad_wakeup[i].type)))
+			break;
+		if ((err = of_property_read_u32_index(np, "pad-wakeup",
+			i * 3 + 2, &port->pad_wakeup[i].line)))
+			break;
+	}
+
+	return err;
+}
+#endif
+
 static int mxc_gpio_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
@@ -559,11 +611,6 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 	struct resource *iores;
 	int irq_base = 0;
 	int err;
-#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
-	int i;
-	uint32_t mu_id;
-	sc_err_t sciErr;
-#endif
 
 	mxc_gpio_get_hw(pdev);
 
@@ -598,36 +645,18 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 	}
 
 #ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
-	/*
-	 * parse pad wakeup info from dtb, each pad has to provide
-	 * <pin_id, type, line>, these info should be put in each
-	 * gpio node and with a "pad-wakeup-num" to indicate the
-	 * total lines are with pad wakeup enabled.
-	 */
-	if (!of_property_read_u32(np, "pad-wakeup-num", &port->pad_wakeup_num)) {
-		if (port->pad_wakeup_num != 0) {
-			if (!gpio_ipc_handle) {
-				sciErr = sc_ipc_getMuID(&mu_id);
-				if (sciErr != SC_ERR_NONE) {
-					dev_err(&pdev->dev,
-						"can not obtain mu id: %d\n", sciErr);
-					return sciErr;
-				}
-				sciErr = sc_ipc_open(&gpio_ipc_handle, mu_id);
-				if (sciErr != SC_ERR_NONE) {
-					dev_err(&pdev->dev,
-						"can not open mu channel to scu: %d\n", sciErr);
-					return sciErr;
-				}
-			}
-			for (i = 0; i < port->pad_wakeup_num; i++) {
-				of_property_read_u32_index(np, "pad-wakeup",
-					i * 3 + 0, &port->pad_wakeup[i].pin_id);
-				of_property_read_u32_index(np, "pad-wakeup",
-					i * 3 + 1, &port->pad_wakeup[i].type);
-				of_property_read_u32_index(np, "pad-wakeup",
-					i * 3 + 2, &port->pad_wakeup[i].line);
-			}
+	if (!of_property_read_u32(np, "pad-wakeup-num", &port->pad_wakeup_num)
+	    && port->pad_wakeup_num != 0) {
+		err = init_sc_ipc_handle(port);
+		if (err)
+			goto out_clk_disable;
+
+		err = setup_pad_wakeup(port, np);
+		if (err) {
+			dev_err(&pdev->dev,
+				"Unable to set up wakeup PAD parameters: %d\n",
+				err);
+			goto out_clk_disable;
 		}
 	}
 #endif
@@ -636,7 +665,7 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 	pm_runtime_enable(&pdev->dev);
 	err = pm_runtime_get_sync(&pdev->dev);
 	if (err < 0)
-		goto out_pm_dis;
+		goto out_pm_disable;
 
 	/* disable the interrupt and clear the status */
 	if (!noclearirq) {
@@ -668,7 +697,7 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 			 port->base + GPIO_GDIR, NULL,
 			 BGPIOF_READ_OUTPUT_REG_SET);
 	if (err)
-		goto out_bgio;
+		goto out_pm_put;
 
 	if (of_property_read_bool(np, "gpio_ranges"))
 		port->gpio_ranges = true;
@@ -684,19 +713,19 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 
 	err = devm_gpiochip_add_data(&pdev->dev, &port->gc, port);
 	if (err)
-		goto out_bgio;
+		goto out_pm_put;
 
 	irq_base = devm_irq_alloc_descs(&pdev->dev, -1, 0, 32, numa_node_id());
 	if (irq_base < 0) {
 		err = irq_base;
-		goto out_bgio;
+		goto out_pm_put;
 	}
 
 	port->domain = irq_domain_add_legacy(np, 32, irq_base, 0,
 					     &irq_domain_simple_ops, NULL);
 	if (!port->domain) {
 		err = -ENODEV;
-		goto out_bgio;
+		goto out_pm_put;
 	}
 
 	/* gpio-mxc can be a generic irq chip */
@@ -711,12 +740,14 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 
 	return 0;
 
-out_pm_dis:
-	pm_runtime_disable(&pdev->dev);
-	clk_disable_unprepare(port->clk);
 out_irqdomain_remove:
 	irq_domain_remove(port->domain);
-out_bgio:
+out_pm_put:
+	pm_runtime_put(&pdev->dev);
+out_pm_disable:
+	pm_runtime_disable(&pdev->dev);
+out_clk_disable:
+	clk_disable_unprepare(port->clk);
 	dev_info(&pdev->dev, "%s failed with errno %d\n", __func__, err);
 	return err;
 }