From 56faf4c5a3957012f3a3a8467b3dfde760093784 Mon Sep 17 00:00:00 2001
From: "Jason M. Bills" <jason.m.bills@linux.intel.com>
Date: Fri, 3 May 2019 16:12:39 -0700
Subject: [PATCH] Enable pass-through on GPIOE1 and GPIOE3 free

This change adds a gpio_disable_free() implementation that checks
if the GPIO being freed is GPIOE1 (33) or GPIOE3 (35) and will
re-enable the pass-through mux.

Tested:
Requested GPIOs 33 and 35 and used devmem to check that pass-through
was disabled. Then freed them and checked that pass-through was
enabled again.

Signed-off-by: Jason M. Bills <jason.m.bills@linux.intel.com>
---
 drivers/pinctrl/aspeed/pinctrl-aspeed-g5.c |  1 +
 drivers/pinctrl/aspeed/pinctrl-aspeed.c    | 60 ++++++++++++++++++++++++++++++
 drivers/pinctrl/aspeed/pinctrl-aspeed.h    |  3 ++
 3 files changed, 64 insertions(+)

diff --git a/drivers/pinctrl/aspeed/pinctrl-aspeed-g5.c b/drivers/pinctrl/aspeed/pinctrl-aspeed-g5.c
index e20e2a259141..fbc9a68bd88c 100644
--- a/drivers/pinctrl/aspeed/pinctrl-aspeed-g5.c
+++ b/drivers/pinctrl/aspeed/pinctrl-aspeed-g5.c
@@ -2605,6 +2605,7 @@ static const struct pinmux_ops aspeed_g5_pinmux_ops = {
 	.get_function_groups = aspeed_pinmux_get_fn_groups,
 	.set_mux = aspeed_pinmux_set_mux,
 	.gpio_request_enable = aspeed_gpio_request_enable,
+	.gpio_disable_free = aspeed_gpio_disable_free,
 	.strict = true,
 };
 
diff --git a/drivers/pinctrl/aspeed/pinctrl-aspeed.c b/drivers/pinctrl/aspeed/pinctrl-aspeed.c
index 18f46177b4af..6feea140b438 100644
--- a/drivers/pinctrl/aspeed/pinctrl-aspeed.c
+++ b/drivers/pinctrl/aspeed/pinctrl-aspeed.c
@@ -356,6 +356,66 @@ int aspeed_gpio_request_enable(struct pinctrl_dev *pctldev,
 	return aspeed_sig_expr_enable(&pdata->pinmux, expr);
 }
 
+void aspeed_gpio_disable_free(struct pinctrl_dev *pctldev,
+			      struct pinctrl_gpio_range *range,
+			      unsigned int offset)
+{
+	const struct aspeed_pinctrl_data *pdata = pinctrl_dev_get_drvdata(pctldev);
+	const struct aspeed_pin_desc *pdesc = pdata->pins[offset].drv_data;
+	const struct aspeed_sig_expr ***prios, **funcs, *expr;
+	int ret;
+
+	/*
+	 * If we're freeing GPIOE1 (33) or GPIOE3 (35) then re-enable the
+	 * pass-through mux setting; otherwise, do nothing.
+	 */
+	if (offset != 33 && offset != 35)
+		return;
+
+	dev_dbg(pctldev->dev,
+		"Freeing pass-through pin %s (%d). Re-enabling pass-through.\n",
+		pdesc->name, offset);
+
+	if (!pdesc)
+		return;
+
+	prios = pdesc->prios;
+
+	if (!prios)
+		return;
+
+	/* Disable any functions of higher priority than GPIO just in case */
+	while ((funcs = *prios)) {
+		if (aspeed_gpio_in_exprs(funcs))
+			break;
+
+		ret = aspeed_disable_sig(&pdata->pinmux, funcs);
+		if (ret)
+			return;
+
+		prios++;
+	}
+
+	if (!funcs) {
+		char *signals = get_defined_signals(pdesc);
+
+		pr_warn("No GPIO signal type found on pin %s (%d). Found: %s\n",
+			pdesc->name, offset, signals);
+		kfree(signals);
+
+		return;
+	}
+
+	/*
+	 * Pass-through should be one priority higher than the GPIO function,
+	 * so decrement our prios and enable that function
+	 */
+	prios--;
+	funcs = *prios;
+	expr = *funcs;
+	aspeed_sig_expr_enable(&pdata->pinmux, expr);
+}
+
 int aspeed_pinctrl_probe(struct platform_device *pdev,
 			 struct pinctrl_desc *pdesc,
 			 struct aspeed_pinctrl_data *pdata)
diff --git a/drivers/pinctrl/aspeed/pinctrl-aspeed.h b/drivers/pinctrl/aspeed/pinctrl-aspeed.h
index 11cc0eb6666b..735162ebb855 100644
--- a/drivers/pinctrl/aspeed/pinctrl-aspeed.h
+++ b/drivers/pinctrl/aspeed/pinctrl-aspeed.h
@@ -64,6 +64,9 @@ int aspeed_pinmux_set_mux(struct pinctrl_dev *pctldev, unsigned int function,
 int aspeed_gpio_request_enable(struct pinctrl_dev *pctldev,
 		struct pinctrl_gpio_range *range,
 		unsigned int offset);
+void aspeed_gpio_disable_free(struct pinctrl_dev *pctldev,
+		struct pinctrl_gpio_range *range,
+		unsigned int offset);
 int aspeed_pinctrl_probe(struct platform_device *pdev,
 		struct pinctrl_desc *pdesc,
 		struct aspeed_pinctrl_data *pdata);
-- 
2.7.4