[U-Boot] [PATCH v2 2/5] usb: host: add error path and remove callback in ehci-generic

[patrice.chotard at st.com]

From: Patrice Chotard <patrice.chotard at st.com>

Add error path to disable enabled clocks and to assert
deasserted resets
Populate the remove callback

Signed-off-by: Patrice Chotard <patrice.chotard at st.com>
---
v2:	_ split previous path 1, add error path and .remove callback

 drivers/usb/host/ehci-generic.c | 81 +++++++++++++++++++++++++++++++++++------
 1 file changed, 70 insertions(+), 11 deletions(-)

diff --git a/drivers/usb/host/ehci-generic.c b/drivers/usb/host/ehci-generic.c
index 2190adb..0c29f63 100644
--- a/drivers/usb/host/ehci-generic.c
+++ b/drivers/usb/host/ehci-generic.c
@@ -11,6 +11,9 @@
 #include <dm.h>
 #include "ehci.h"
 
+#define EHCI_MAX_CLOCKS 3
+#define EHCI_MAX_RESETS 3
+
 /*
  * Even though here we don't explicitly use "struct ehci_ctrl"
  * ehci_register() expects it to be the first thing that resides in
@@ -18,35 +21,74 @@
  */
 struct generic_ehci {
 	struct ehci_ctrl ctrl;
+	struct clk clks[EHCI_MAX_CLOCKS];
+	struct reset_ctl resets[EHCI_MAX_RESETS];
 };
 
+static void ehci_assert_resets(struct udevice *dev) {
+	struct generic_ehci *priv = dev_get_priv(dev);
+	struct reset_ctl reset;
+	int i;
+
+	for (i = EHCI_MAX_RESETS; i >= 0; --i) {
+		reset = priv->resets[i];
+
+		if (reset.dev) {
+                        reset_request(&reset);
+                        reset_assert(&reset);
+                        reset_free(&reset);
+		}
+	}
+}
+
+static void ehci_disable_clocks(struct udevice *dev) {
+	struct generic_ehci *priv = dev_get_priv(dev);
+	struct clk clk;
+	int i;
+
+	for (i = EHCI_MAX_CLOCKS; i >= 0; --i) {
+		clk = priv->clks[i];
+
+		if (clk.dev) {
+			clk_request(clk.dev, &clk);
+			clk_disable(&clk);
+			clk_free(&clk);
+		}
+	}
+}
+
 static int ehci_usb_probe(struct udevice *dev)
 {
+	struct generic_ehci *priv = dev_get_priv(dev);
 	struct ehci_hccr *hccr;
 	struct ehci_hcor *hcor;
-	int i;
+	int i, ret;
 
-	for (i = 0; ; i++) {
-		struct clk clk;
-		int ret;
+	for (i = 0; i < EHCI_MAX_CLOCKS; i++) {
+		struct clk clk = priv->clks[i];
 
 		ret = clk_get_by_index(dev, i, &clk);
 		if (ret < 0)
 			break;
-		if (clk_enable(&clk))
+		if (clk_enable(&clk)) {
 			printf("failed to enable clock %d\n", i);
+			clk_free(&clk);
+			goto clk_err;
+		}
 		clk_free(&clk);
 	}
 
-	for (i = 0; ; i++) {
-		struct reset_ctl reset;
-		int ret;
+	for (i = 0; i < EHCI_MAX_RESETS ; i++) {
+		struct reset_ctl reset = priv->resets[i];
 
 		ret = reset_get_by_index(dev, i, &reset);
 		if (ret < 0)
 			break;
-		if (reset_deassert(&reset))
+		if (reset_deassert(&reset)) {
 			printf("failed to deassert reset %d\n", i);
+			reset_free(&reset);
+			goto reset_err;
+		}
 		reset_free(&reset);
 	}
 
@@ -54,7 +96,24 @@ static int ehci_usb_probe(struct udevice *dev)
 	hcor = (struct ehci_hcor *)((uintptr_t)hccr +
 				    HC_LENGTH(ehci_readl(&hccr->cr_capbase)));
 
-	return ehci_register(dev, hccr, hcor, NULL, 0, USB_INIT_HOST);
+	ret = ehci_register(dev, hccr, hcor, NULL, 0, USB_INIT_HOST);
+	if (!ret)
+		return ret;
+
+reset_err:
+	ehci_assert_resets(dev);
+clk_err:
+	ehci_disable_clocks(dev);
+
+	return ret;
+}
+
+static int ehci_usb_remove(struct udevice *dev) {
+
+	ehci_assert_resets(dev);
+	ehci_disable_clocks(dev);
+
+	return ehci_deregister(dev);
 }
 
 static const struct udevice_id ehci_usb_ids[] = {
@@ -67,7 +126,7 @@ U_BOOT_DRIVER(ehci_generic) = {
 	.id	= UCLASS_USB,
 	.of_match = ehci_usb_ids,
 	.probe = ehci_usb_probe,
-	.remove = ehci_deregister,
+	.remove = ehci_usb_remove,
 	.ops	= &ehci_usb_ops,
 	.priv_auto_alloc_size = sizeof(struct generic_ehci),
 	.flags	= DM_FLAG_ALLOC_PRIV_DMA,
-- 
1.9.1