soc: rockchip: power-domain: support qos save and restore

support qos save and restore when power domain on/off.

Signed-off-by: Elaine Zhang <zhangqing@rock-chips.com>
Signed-off-by: Heiko Stuebner <heiko@sntech.de>

diff --git a/drivers/soc/rockchip/pm_domains.c b/drivers/soc/rockchip/pm_domains.c
index ac729fe42cc91c4b23c4f59c866507d1512ce720..44842a205e4b1eea972af00f2f413dfa5c238c72 100644 (file)
--- a/drivers/soc/rockchip/pm_domains.c
+++ b/drivers/soc/rockchip/pm_domains.c
@@ -46,10 +46,20 @@ struct rockchip_pmu_info {
        const struct rockchip_domain_info *domain_info;
 };
 
+#define MAX_QOS_REGS_NUM       5
+#define QOS_PRIORITY           0x08
+#define QOS_MODE               0x0c
+#define QOS_BANDWIDTH          0x10
+#define QOS_SATURATION         0x14
+#define QOS_EXTCONTROL         0x18
+
 struct rockchip_pm_domain {
        struct generic_pm_domain genpd;
        const struct rockchip_domain_info *info;
        struct rockchip_pmu *pmu;
+       int num_qos;
+       struct regmap **qos_regmap;
+       u32 *qos_save_regs[MAX_QOS_REGS_NUM];
        int num_clks;
        struct clk *clks[];
 };
@@ -118,6 +128,55 @@ static int rockchip_pmu_set_idle_request(struct rockchip_pm_domain *pd,
        return 0;
 }
 
+static int rockchip_pmu_save_qos(struct rockchip_pm_domain *pd)
+{
+       int i;
+
+       for (i = 0; i < pd->num_qos; i++) {
+               regmap_read(pd->qos_regmap[i],
+                           QOS_PRIORITY,
+                           &pd->qos_save_regs[0][i]);
+               regmap_read(pd->qos_regmap[i],
+                           QOS_MODE,
+                           &pd->qos_save_regs[1][i]);
+               regmap_read(pd->qos_regmap[i],
+                           QOS_BANDWIDTH,
+                           &pd->qos_save_regs[2][i]);
+               regmap_read(pd->qos_regmap[i],
+                           QOS_SATURATION,
+                           &pd->qos_save_regs[3][i]);
+               regmap_read(pd->qos_regmap[i],
+                           QOS_EXTCONTROL,
+                           &pd->qos_save_regs[4][i]);
+       }
+       return 0;
+}
+
+static int rockchip_pmu_restore_qos(struct rockchip_pm_domain *pd)
+{
+       int i;
+
+       for (i = 0; i < pd->num_qos; i++) {
+               regmap_write(pd->qos_regmap[i],
+                            QOS_PRIORITY,
+                            pd->qos_save_regs[0][i]);
+               regmap_write(pd->qos_regmap[i],
+                            QOS_MODE,
+                            pd->qos_save_regs[1][i]);
+               regmap_write(pd->qos_regmap[i],
+                            QOS_BANDWIDTH,
+                            pd->qos_save_regs[2][i]);
+               regmap_write(pd->qos_regmap[i],
+                            QOS_SATURATION,
+                            pd->qos_save_regs[3][i]);
+               regmap_write(pd->qos_regmap[i],
+                            QOS_EXTCONTROL,
+                            pd->qos_save_regs[4][i]);
+       }
+
+       return 0;
+}
+
 static bool rockchip_pmu_domain_is_on(struct rockchip_pm_domain *pd)
 {
        struct rockchip_pmu *pmu = pd->pmu;
@@ -161,7 +220,7 @@ static int rockchip_pd_power(struct rockchip_pm_domain *pd, bool power_on)
                        clk_enable(pd->clks[i]);
 
                if (!power_on) {
-                       /* FIXME: add code to save AXI_QOS */
+                       rockchip_pmu_save_qos(pd);
 
                        /* if powering down, idle request to NIU first */
                        rockchip_pmu_set_idle_request(pd, true);
@@ -173,7 +232,7 @@ static int rockchip_pd_power(struct rockchip_pm_domain *pd, bool power_on)
                        /* if powering up, leave idle mode */
                        rockchip_pmu_set_idle_request(pd, false);
 
-                       /* FIXME: add code to restore AXI_QOS */
+                       rockchip_pmu_restore_qos(pd);
                }
 
                for (i = pd->num_clks - 1; i >= 0; i--)
@@ -241,9 +300,10 @@ static int rockchip_pm_add_one_domain(struct rockchip_pmu *pmu,
 {
        const struct rockchip_domain_info *pd_info;
        struct rockchip_pm_domain *pd;
+       struct device_node *qos_node;
        struct clk *clk;
        int clk_cnt;
-       int i;
+       int i, j;
        u32 id;
        int error;
 
@@ -303,6 +363,45 @@ static int rockchip_pm_add_one_domain(struct rockchip_pmu *pmu,
                        clk, node->name);
        }
 
+       pd->num_qos = of_count_phandle_with_args(node, "pm_qos",
+                                                NULL);
+
+       if (pd->num_qos > 0) {
+               pd->qos_regmap = devm_kcalloc(pmu->dev, pd->num_qos,
+                                             sizeof(*pd->qos_regmap),
+                                             GFP_KERNEL);
+               if (!pd->qos_regmap) {
+                       error = -ENOMEM;
+                       goto err_out;
+               }
+
+               for (j = 0; j < MAX_QOS_REGS_NUM; j++) {
+                       pd->qos_save_regs[j] = devm_kcalloc(pmu->dev,
+                                                           pd->num_qos,
+                                                           sizeof(u32),
+                                                           GFP_KERNEL);
+                       if (!pd->qos_save_regs[j]) {
+                               error = -ENOMEM;
+                               goto err_out;
+                       }
+               }
+
+               for (j = 0; j < pd->num_qos; j++) {
+                       qos_node = of_parse_phandle(node, "pm_qos", j);
+                       if (!qos_node) {
+                               error = -ENODEV;
+                               goto err_out;
+                       }
+                       pd->qos_regmap[j] = syscon_node_to_regmap(qos_node);
+                       if (IS_ERR(pd->qos_regmap[j])) {
+                               error = -ENODEV;
+                               of_node_put(qos_node);
+                               goto err_out;
+                       }
+                       of_node_put(qos_node);
+               }
+       }
+
        error = rockchip_pd_power(pd, true);
        if (error) {
                dev_err(pmu->dev,