From: Patrice Chotard patrice.chotard@st.com
use list to save reference to enabled clocks and deasserted resets in order to respectively disabled and asserted them in case of error during probe() or during driver removal.
Signed-off-by: Patrice Chotard patrice.chotard@st.com ---
v3: _ keep enabled clocks and deasserted resets reference in list in order to disable clock or assert resets in error path or in .remove callback _ use struct generic_ehci * instead of struct udevice * as parameter for ehci_release_resets() and ehci_release_clocks()
drivers/usb/host/ehci-generic.c | 162 ++++++++++++++++++++++++++++++++++++---- 1 file changed, 149 insertions(+), 13 deletions(-)
diff --git a/drivers/usb/host/ehci-generic.c b/drivers/usb/host/ehci-generic.c index 6058e9a..d281218 100644 --- a/drivers/usb/host/ehci-generic.c +++ b/drivers/usb/host/ehci-generic.c @@ -11,6 +11,16 @@ #include <dm.h> #include "ehci.h"
+struct ehci_clock { + struct clk *clk; + struct list_head list; +}; + +struct ehci_reset { + struct reset_ctl *reset; + struct list_head list; +}; + /* * Even though here we don't explicitly use "struct ehci_ctrl" * ehci_register() expects it to be the first thing that resides in @@ -18,43 +28,169 @@ */ struct generic_ehci { struct ehci_ctrl ctrl; + struct list_head clks; + struct list_head resets; };
+static int ehci_release_resets(struct generic_ehci *priv) +{ + struct ehci_reset *ehci_reset, *tmp; + struct reset_ctl *reset; + int ret; + + list_for_each_entry_safe(ehci_reset, tmp, &priv->resets, list) { + reset = ehci_reset->reset; + + ret = reset_request(reset); + if (ret) + return ret; + + ret = reset_assert(reset); + if (ret) + return ret; + + ret = reset_free(reset); + if (ret) + return ret; + + list_del(&ehci_reset->list); + } + return 0; +} + +static int ehci_release_clocks(struct generic_ehci *priv) +{ + struct ehci_clock *ehci_clock, *tmp; + struct clk *clk; + int ret; + + list_for_each_entry_safe(ehci_clock, tmp, &priv->clks, list) { + clk = ehci_clock->clk; + + clk_request(clk->dev, clk); + if (ret) + return ret; + + clk_disable(clk); + + ret = clk_free(clk); + if (ret) + return ret; + + list_del(&ehci_clock->list); + } + return 0; +} + 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; + + INIT_LIST_HEAD(&priv->clks); + INIT_LIST_HEAD(&priv->resets);
for (i = 0; ; i++) { - struct clk clk; - int ret; + struct ehci_clock *ehci_clock; + struct clk *clk;
- ret = clk_get_by_index(dev, i, &clk); + clk = devm_kmalloc(dev, sizeof(*clk), GFP_KERNEL); + if (!clk) { + error("Can't allocate resource\n"); + goto clk_err; + } + + ret = clk_get_by_index(dev, i, clk); if (ret < 0) break; - if (clk_enable(&clk)) + + if (clk_enable(clk)) { error("failed to enable clock %d\n", i); - clk_free(&clk); + clk_free(clk); + goto clk_err; + } + clk_free(clk); + + /* + * add enabled clocks into clks list in order to be disabled + * later on ehci_usb_remove() call or in error path if needed + */ + ehci_clock = devm_kmalloc(dev, sizeof(*ehci_clock), GFP_KERNEL); + if (!ehci_clock) { + error("Can't allocate resource\n"); + goto clk_err; + } + ehci_clock->clk = clk; + list_add(&ehci_clock->list, &priv->clks); }
for (i = 0; ; i++) { - struct reset_ctl reset; - int ret; + struct ehci_reset *ehci_reset; + struct reset_ctl *reset; + + reset = devm_kmalloc(dev, sizeof(*reset), GFP_KERNEL); + if (!reset) { + error("Can't allocate resource\n"); + goto clk_err; + }
- ret = reset_get_by_index(dev, i, &reset); + ret = reset_get_by_index(dev, i, reset); if (ret < 0) break; - if (reset_deassert(&reset)) + + if (reset_deassert(reset)) { error("failed to deassert reset %d\n", i); - reset_free(&reset); + reset_free(reset); + goto reset_err; + } + reset_free(reset); + + /* + * add deasserted resets into resets list in order to be + * asserted later on ehci_usb_remove() call or in error + * path if needed + */ + ehci_reset = devm_kmalloc(dev, sizeof(*ehci_reset), GFP_KERNEL); + if (!ehci_reset) { + error("Can't allocate resource\n"); + goto reset_err; + } + ehci_reset->reset = reset; + list_add(&ehci_reset->list, &priv->resets); }
hccr = map_physmem(dev_get_addr(dev), 0x100, MAP_NOCACHE); 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: + ret = ehci_release_resets(priv); + if (ret) + return ret; +clk_err: + return ehci_release_clocks(priv); +} + +static int ehci_usb_remove(struct udevice *dev) +{ + struct generic_ehci *priv = dev_get_priv(dev); + int ret; + + ret = ehci_deregister(dev); + if (ret) + return ret; + + ret = ehci_release_resets(priv); + if (ret) + return ret; + + return ehci_release_clocks(priv); }
static const struct udevice_id ehci_usb_ids[] = { @@ -67,7 +203,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,