soc: rockchip: opp_select: Add support to adjust power scale

Change-Id: I2358d75c2fdada7cfe385e85d2106370f9aa5ea3
Signed-off-by: Finley Xiao <finley.xiao@rock-chips.com>

2 files changed, 30 insertions, 8 deletions

diff --git a/drivers/clk/rockchip/clk-pll.c b/drivers/clk/rockchip/clk-pll.c
index da7221078b33..dea6ce39cb04 100644
--- a/drivers/clk/rockchip/clk-pll.c
+++ b/drivers/clk/rockchip/clk-pll.c
@@ -114,12 +114,12 @@ int rockchip_pll_clk_adaptive_scaling(struct clk *clk, int sel)
 	return 0;
 }
 
-int rockchip_pll_clk_adaptive_rate(struct clk *clk, unsigned long rate)
+int rockchip_pll_clk_rate_to_scale(struct clk *clk, unsigned long rate)
 {
 	const struct rockchip_pll_rate_table *rate_table;
 	struct clk *parent = clk_get_parent(clk);
 	struct rockchip_clk_pll *pll;
-	int i;
+	unsigned int i;
 
 	if (IS_ERR_OR_NULL(parent))
 		return -EINVAL;
@@ -130,13 +130,34 @@ int rockchip_pll_clk_adaptive_rate(struct clk *clk, unsigned long rate)
 
 	rate_table = pll->rate_table;
 	for (i = 0; i < pll->rate_count; i++) {
-		if (rate >= rate_table[i].rate) {
-			pll->sel = i;
-			break;
-		}
+		if (rate >= rate_table[i].rate)
+			return i;
 	}
 
-	return 0;
+	return -EINVAL;
+}
+
+int rockchip_pll_clk_scale_to_rate(struct clk *clk, unsigned int scale)
+{
+	const struct rockchip_pll_rate_table *rate_table;
+	struct clk *parent = clk_get_parent(clk);
+	struct rockchip_clk_pll *pll;
+	unsigned int i;
+
+	if (IS_ERR_OR_NULL(parent))
+		return -EINVAL;
+
+	pll = to_rockchip_clk_pll(__clk_get_hw(parent));
+	if (!pll)
+		return -EINVAL;
+
+	rate_table = pll->rate_table;
+	for (i = 0; i < pll->rate_count; i++) {
+		if (i == scale)
+			return rate_table[i].rate;
+	}
+
+	return -EINVAL;
 }
 
 static struct rockchip_pll_rate_table *rk_pll_rate_table_get(void)
diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h
index 779497c5512b..e4bcd2a5abb1 100644
--- a/drivers/clk/rockchip/clk.h
+++ b/drivers/clk/rockchip/clk.h
@@ -781,7 +781,8 @@ void rockchip_clk_register_armclk(struct rockchip_clk_provider *ctx,
 			int nrates);
 void rockchip_clk_protect_critical(const char *const clocks[], int nclocks);
 int rockchip_pll_clk_adaptive_scaling(struct clk *clk, int sel);
-int rockchip_pll_clk_adaptive_rate(struct clk *clk, unsigned long rate);
+int rockchip_pll_clk_rate_to_scale(struct clk *clk, unsigned long rate);
+int rockchip_pll_clk_scale_to_rate(struct clk *clk, unsigned int scale);
 void rockchip_register_restart_notifier(struct rockchip_clk_provider *ctx,
 					unsigned int reg, void (*cb)(void));