From: Patrice Chotard <patrice.chot...@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.chot...@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, -- 1.9.1 _______________________________________________ U-Boot mailing list U-Boot@lists.denx.de https://lists.denx.de/listinfo/u-boot