From ad1d7cf016dc9292ab7fc4a877e78d3f51a04fa2 Mon Sep 17 00:00:00 2001 From: Finley Xiao Date: Tue, 7 Jun 2022 10:18:05 +0800 Subject: [PATCH] soc: rockchip: power-domain: init qos when pd is on Fixes: 4910bdcba8f9 ("soc: rockchip: power-domain: support qos init") Signed-off-by: Finley Xiao Change-Id: I282a3047b4c5c8b35bf5f8c2e6c3b5fca0d89ab7 --- drivers/soc/rockchip/pm_domains.c | 136 +++++++++++++++++------------- 1 file changed, 79 insertions(+), 57 deletions(-) diff --git a/drivers/soc/rockchip/pm_domains.c b/drivers/soc/rockchip/pm_domains.c index a4ce8bce6a63..e88fe9593a65 100644 --- a/drivers/soc/rockchip/pm_domains.c +++ b/drivers/soc/rockchip/pm_domains.c @@ -91,10 +91,12 @@ struct rockchip_pm_domain { int num_qos; struct regmap **qos_regmap; u32 *qos_save_regs[MAX_QOS_REGS_NUM]; + bool *qos_is_need_init[MAX_QOS_REGS_NUM]; int num_clks; struct clk_bulk_data *clks; bool is_ignore_pwr; bool is_qos_saved; + bool is_qos_need_init; struct regulator *supply; }; @@ -392,6 +394,45 @@ static int rockchip_pmu_restore_qos(struct rockchip_pm_domain *pd) return 0; } +static void rockchip_pmu_init_qos(struct rockchip_pm_domain *pd) +{ + int i; + + if (!pd->is_qos_need_init) + return; + + for (i = 0; i < pd->num_qos; i++) { + if (pd->qos_is_need_init[0][i]) + regmap_write(pd->qos_regmap[i], + QOS_PRIORITY, + pd->qos_save_regs[0][i]); + + if (pd->qos_is_need_init[1][i]) + regmap_write(pd->qos_regmap[i], + QOS_MODE, + pd->qos_save_regs[1][i]); + + if (pd->qos_is_need_init[2][i]) + regmap_write(pd->qos_regmap[i], + QOS_BANDWIDTH, + pd->qos_save_regs[2][i]); + + if (pd->qos_is_need_init[3][i]) + regmap_write(pd->qos_regmap[i], + QOS_SATURATION, + pd->qos_save_regs[3][i]); + + if (pd->qos_is_need_init[4][i]) + regmap_write(pd->qos_regmap[i], + QOS_EXTCONTROL, + pd->qos_save_regs[4][i]); + } + + kfree(pd->qos_is_need_init[0]); + pd->qos_is_need_init[0] = NULL; + pd->is_qos_need_init = false; +} + int rockchip_save_qos(struct device *dev) { struct generic_pm_domain *genpd; @@ -644,6 +685,8 @@ static int rockchip_pd_power(struct rockchip_pm_domain *pd, bool power_on) if (pd->is_qos_saved) rockchip_pmu_restore_qos(pd); + if (pd->is_qos_need_init) + rockchip_pmu_init_qos(pd); } out: @@ -774,44 +817,26 @@ static void rockchip_pd_detach_dev(struct generic_pm_domain *genpd, pm_clk_destroy(dev); } -static void rockchip_pd_qos_init(struct rockchip_pm_domain *pd, - bool **qos_is_need_init) +static void rockchip_pd_qos_init(struct rockchip_pm_domain *pd) { - int i, is_pd_on; + int is_pd_on, ret = 0; - is_pd_on = rockchip_pmu_domain_is_on(pd); - if (!is_pd_on) - rockchip_pd_power(pd, true); - - for (i = 0; i < pd->num_qos; i++) { - if (qos_is_need_init[0][i]) - regmap_write(pd->qos_regmap[i], - QOS_PRIORITY, - pd->qos_save_regs[0][i]); - - if (qos_is_need_init[1][i]) - regmap_write(pd->qos_regmap[i], - QOS_MODE, - pd->qos_save_regs[1][i]); - - if (qos_is_need_init[2][i]) - regmap_write(pd->qos_regmap[i], - QOS_BANDWIDTH, - pd->qos_save_regs[2][i]); - - if (qos_is_need_init[3][i]) - regmap_write(pd->qos_regmap[i], - QOS_SATURATION, - pd->qos_save_regs[3][i]); - - if (qos_is_need_init[4][i]) - regmap_write(pd->qos_regmap[i], - QOS_EXTCONTROL, - pd->qos_save_regs[4][i]); + if (!pd->is_qos_need_init) { + kfree(pd->qos_is_need_init[0]); + pd->qos_is_need_init[0] = NULL; + return; } - if (!is_pd_on) - rockchip_pd_power(pd, false); + is_pd_on = rockchip_pmu_domain_is_on(pd); + if (is_pd_on) { + ret = clk_bulk_enable(pd->num_clks, pd->clks); + if (ret < 0) { + dev_err(pd->pmu->dev, "failed to enable clocks\n"); + return; + } + rockchip_pmu_init_qos(pd); + clk_bulk_disable(pd->num_clks, pd->clks); + } } static int rockchip_pm_add_one_domain(struct rockchip_pmu *pmu, @@ -824,8 +849,6 @@ static int rockchip_pm_add_one_domain(struct rockchip_pmu *pmu, int i, j; u32 id, val; int error; - bool *qos_is_need_init[MAX_QOS_REGS_NUM] = { NULL }; - bool is_qos_need_init = false; error = of_property_read_u32(node, "reg", &id); if (error) { @@ -913,18 +936,19 @@ static int rockchip_pm_add_one_domain(struct rockchip_pmu *pmu, error = -ENOMEM; goto err_unprepare_clocks; } - qos_is_need_init[0] = kzalloc(sizeof(bool) * - MAX_QOS_REGS_NUM * - pd->num_qos, - GFP_KERNEL); - if (!qos_is_need_init[0]) { + pd->qos_is_need_init[0] = kzalloc(sizeof(bool) * + MAX_QOS_REGS_NUM * + pd->num_qos, + GFP_KERNEL); + if (!pd->qos_is_need_init[0]) { error = -ENOMEM; goto err_unprepare_clocks; } for (i = 1; i < MAX_QOS_REGS_NUM; i++) { pd->qos_save_regs[i] = pd->qos_save_regs[i - 1] + num_qos; - qos_is_need_init[i] = qos_is_need_init[i - 1] + num_qos; + pd->qos_is_need_init[i] = pd->qos_is_need_init[i - 1] + + num_qos; } for (j = 0; j < num_qos; j++) { @@ -945,40 +969,40 @@ static int rockchip_pm_add_one_domain(struct rockchip_pmu *pmu, "priority-init", &val)) { pd->qos_save_regs[0][j] = val; - qos_is_need_init[0][j] = true; - is_qos_need_init = true; + pd->qos_is_need_init[0][j] = true; + pd->is_qos_need_init = true; } if (!of_property_read_u32(qos_node, "mode-init", &val)) { pd->qos_save_regs[1][j] = val; - qos_is_need_init[1][j] = true; - is_qos_need_init = true; + pd->qos_is_need_init[1][j] = true; + pd->is_qos_need_init = true; } if (!of_property_read_u32(qos_node, "bandwidth-init", &val)) { pd->qos_save_regs[2][j] = val; - qos_is_need_init[2][j] = true; - is_qos_need_init = true; + pd->qos_is_need_init[2][j] = true; + pd->is_qos_need_init = true; } if (!of_property_read_u32(qos_node, "saturation-init", &val)) { pd->qos_save_regs[3][j] = val; - qos_is_need_init[3][j] = true; - is_qos_need_init = true; + pd->qos_is_need_init[3][j] = true; + pd->is_qos_need_init = true; } if (!of_property_read_u32(qos_node, "extcontrol-init", &val)) { pd->qos_save_regs[4][j] = val; - qos_is_need_init[4][j] = true; - is_qos_need_init = true; + pd->qos_is_need_init[4][j] = true; + pd->is_qos_need_init = true; } num_qos_reg++; @@ -1015,10 +1039,7 @@ static int rockchip_pm_add_one_domain(struct rockchip_pmu *pmu, } } #endif - if (is_qos_need_init) - rockchip_pd_qos_init(pd, &qos_is_need_init[0]); - - kfree(qos_is_need_init[0]); + rockchip_pd_qos_init(pd); pm_genpd_init(&pd->genpd, NULL, !rockchip_pmu_domain_is_on(pd)); @@ -1026,7 +1047,8 @@ static int rockchip_pm_add_one_domain(struct rockchip_pmu *pmu, return 0; err_unprepare_clocks: - kfree(qos_is_need_init[0]); + kfree(pd->qos_is_need_init[0]); + pd->qos_is_need_init[0] = NULL; clk_bulk_unprepare(pd->num_clks, pd->clks); err_put_clocks: clk_bulk_put(pd->num_clks, pd->clks);