1 files changed, 68 insertions, 15 deletions

diff --git a/drivers/soc/tegra/pmc.c b/drivers/soc/tegra/pmc.c
index 741afb141887..5648e5c09ef5 100644
--- a/drivers/soc/tegra/pmc.c
+++ b/drivers/soc/tegra/pmc.c
@@ -990,20 +990,21 @@ out:
 	return err;
 }
 
-static void tegra_powergate_add(struct tegra_pmc *pmc, struct device_node *np)
+static int tegra_powergate_add(struct tegra_pmc *pmc, struct device_node *np)
 {
 	struct device *dev = pmc->dev;
 	struct tegra_powergate *pg;
-	int id, err;
+	int id, err = 0;
 	bool off;
 
 	pg = kzalloc(sizeof(*pg), GFP_KERNEL);
 	if (!pg)
-		return;
+		return -ENOMEM;
 
 	id = tegra_powergate_lookup(pmc, np->name);
 	if (id < 0) {
 		dev_err(dev, "powergate lookup failed for %pOFn: %d\n", np, id);
+		err = -ENODEV;
 		goto free_mem;
 	}
 
@@ -1056,7 +1057,7 @@ static void tegra_powergate_add(struct tegra_pmc *pmc, struct device_node *np)
 
 	dev_dbg(dev, "added PM domain %s\n", pg->genpd.name);
 
-	return;
+	return 0;
 
 remove_genpd:
 	pm_genpd_remove(&pg->genpd);
@@ -1075,25 +1076,67 @@ set_available:
 
 free_mem:
 	kfree(pg);
+
+	return err;
 }
 
-static void tegra_powergate_init(struct tegra_pmc *pmc,
-				 struct device_node *parent)
+static int tegra_powergate_init(struct tegra_pmc *pmc,
+				struct device_node *parent)
 {
 	struct device_node *np, *child;
-	unsigned int i;
+	int err = 0;
+
+	np = of_get_child_by_name(parent, "powergates");
+	if (!np)
+		return 0;
 
-	/* Create a bitmap of the available and valid partitions */
-	for (i = 0; i < pmc->soc->num_powergates; i++)
-		if (pmc->soc->powergates[i])
-			set_bit(i, pmc->powergates_available);
+	for_each_child_of_node(np, child) {
+		err = tegra_powergate_add(pmc, child);
+		if (err < 0) {
+			of_node_put(child);
+			break;
+		}
+	}
+
+	of_node_put(np);
+
+	return err;
+}
+
+static void tegra_powergate_remove(struct generic_pm_domain *genpd)
+{
+	struct tegra_powergate *pg = to_powergate(genpd);
+
+	reset_control_put(pg->reset);
+
+	while (pg->num_clks--)
+		clk_put(pg->clks[pg->num_clks]);
+
+	kfree(pg->clks);
+
+	set_bit(pg->id, pmc->powergates_available);
+
+	kfree(pg);
+}
+
+static void tegra_powergate_remove_all(struct device_node *parent)
+{
+	struct generic_pm_domain *genpd;
+	struct device_node *np, *child;
 
 	np = of_get_child_by_name(parent, "powergates");
 	if (!np)
 		return;
 
-	for_each_child_of_node(np, child)
-		tegra_powergate_add(pmc, child);
+	for_each_child_of_node(np, child) {
+		of_genpd_del_provider(child);
+
+		genpd = of_genpd_remove_last(child);
+		if (IS_ERR(genpd))
+			continue;
+
+		tegra_powergate_remove(genpd);
+	}
 
 	of_node_put(np);
 }
@@ -2054,9 +2097,13 @@ static int tegra_pmc_probe(struct platform_device *pdev)
 	if (err)
 		goto cleanup_restart_handler;
 
+	err = tegra_powergate_init(pmc, pdev->dev.of_node);
+	if (err < 0)
+		goto cleanup_powergates;
+
 	err = tegra_pmc_irq_init(pmc);
 	if (err < 0)
-		goto cleanup_restart_handler;
+		goto cleanup_powergates;
 
 	mutex_lock(&pmc->powergates_lock);
 	iounmap(pmc->base);
@@ -2067,6 +2114,8 @@ static int tegra_pmc_probe(struct platform_device *pdev)
 
 	return 0;
 
+cleanup_powergates:
+	tegra_powergate_remove_all(pdev->dev.of_node);
 cleanup_restart_handler:
 	unregister_restart_handler(&tegra_pmc_restart_handler);
 cleanup_debugfs:
@@ -2763,6 +2812,7 @@ static int __init tegra_pmc_early_init(void)
 	const struct of_device_id *match;
 	struct device_node *np;
 	struct resource regs;
+	unsigned int i;
 	bool invert;
 
 	mutex_init(&pmc->powergates_lock);
@@ -2819,7 +2869,10 @@ static int __init tegra_pmc_early_init(void)
 		if (pmc->soc->maybe_tz_only)
 			pmc->tz_only = tegra_pmc_detect_tz_only(pmc);
 
-		tegra_powergate_init(pmc, np);
+		/* Create a bitmap of the available and valid partitions */
+		for (i = 0; i < pmc->soc->num_powergates; i++)
+			if (pmc->soc->powergates[i])
+				set_bit(i, pmc->powergates_available);
 
 		/*
 		 * Invert the interrupt polarity if a PMC device tree node