diff options
| -rw-r--r-- | drivers/soc/mediatek/mtk-scpsys.c | 45 |
1 files changed, 28 insertions, 17 deletions
diff --git a/drivers/soc/mediatek/mtk-scpsys.c b/drivers/soc/mediatek/mtk-scpsys.c index aed540d2686c..73e4a1aace2f 100644 --- a/drivers/soc/mediatek/mtk-scpsys.c +++ b/drivers/soc/mediatek/mtk-scpsys.c @@ -207,6 +207,29 @@ static int scpsys_regulator_disable(struct scp_domain *scpd) return regulator_disable(scpd->supply); } +static void scpsys_clk_disable(struct clk *clk[], int max_num) +{ + int i; + + for (i = max_num - 1; i >= 0; i--) + clk_disable_unprepare(clk[i]); +} + +static int scpsys_clk_enable(struct clk *clk[], int max_num) +{ + int i, ret = 0; + + for (i = 0; i < max_num && clk[i]; i++) { + ret = clk_prepare_enable(clk[i]); + if (ret) { + scpsys_clk_disable(clk, i); + break; + } + } + + return ret; +} + static int scpsys_power_on(struct generic_pm_domain *genpd) { struct scp_domain *scpd = container_of(genpd, struct scp_domain, genpd); @@ -215,21 +238,14 @@ static int scpsys_power_on(struct generic_pm_domain *genpd) u32 pdn_ack = scpd->data->sram_pdn_ack_bits; u32 val; int ret, tmp; - int i; ret = scpsys_regulator_enable(scpd); if (ret < 0) return ret; - for (i = 0; i < MAX_CLKS && scpd->clk[i]; i++) { - ret = clk_prepare_enable(scpd->clk[i]); - if (ret) { - for (--i; i >= 0; i--) - clk_disable_unprepare(scpd->clk[i]); - - goto err_clk; - } - } + ret = scpsys_clk_enable(scpd->clk, MAX_CLKS); + if (ret) + goto err_clk; val = readl(ctl_addr); val |= PWR_ON_BIT; @@ -282,10 +298,7 @@ static int scpsys_power_on(struct generic_pm_domain *genpd) return 0; err_pwr_ack: - for (i = MAX_CLKS - 1; i >= 0; i--) { - if (scpd->clk[i]) - clk_disable_unprepare(scpd->clk[i]); - } + scpsys_clk_disable(scpd->clk, MAX_CLKS); err_clk: scpsys_regulator_disable(scpd); @@ -302,7 +315,6 @@ static int scpsys_power_off(struct generic_pm_domain *genpd) u32 pdn_ack = scpd->data->sram_pdn_ack_bits; u32 val; int ret, tmp; - int i; if (scpd->data->bus_prot_mask) { ret = mtk_infracfg_set_bus_protection(scp->infracfg, @@ -343,8 +355,7 @@ static int scpsys_power_off(struct generic_pm_domain *genpd) if (ret < 0) goto out; - for (i = 0; i < MAX_CLKS && scpd->clk[i]; i++) - clk_disable_unprepare(scpd->clk[i]); + scpsys_clk_disable(scpd->clk, MAX_CLKS); ret = scpsys_regulator_disable(scpd); if (ret < 0) |