clk: rockchip: Add divider for backup pll when boost

Cpu clock rate should be less than or equal to low rate when
change pll rate in boost module.

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

3 files changed, 41 insertions, 3 deletions

diff --git a/drivers/clk/rockchip/clk-cpu.c b/drivers/clk/rockchip/clk-cpu.c
index 0e986bb4f28e..8166cfd45e09 100644
--- a/drivers/clk/rockchip/clk-cpu.c
+++ b/drivers/clk/rockchip/clk-cpu.c
@@ -166,6 +166,7 @@ static int rockchip_cpuclk_pre_rate_change(struct rockchip_cpuclk *cpuclk,
 				     reg_data->div_core_shift),
 		       cpuclk->reg_base + reg_data->core_reg);
 	}
+	rockchip_boost_add_core_div(cpuclk->pll_hw, alt_prate);
 
 	/* select alternate parent */
 	writel(HIWORD_UPDATE(reg_data->mux_core_alt,
diff --git a/drivers/clk/rockchip/clk-pll.c b/drivers/clk/rockchip/clk-pll.c
index 2426582ad23a..0a9f31f2dd27 100644
--- a/drivers/clk/rockchip/clk-pll.c
+++ b/drivers/clk/rockchip/clk-pll.c
@@ -57,6 +57,8 @@ struct rockchip_clk_pll {
 	struct rockchip_clk_provider *ctx;
 
 	bool			boost_enabled;
+	u32			boost_backup_pll_usage;
+	unsigned long		boost_backup_pll_rate;
 	unsigned long		boost_low_rate;
 	unsigned long		boost_high_rate;
 	struct regmap		*boost;
@@ -1581,10 +1583,12 @@ void rockchip_boost_init(struct clk_hw *hw)
 					   BOOST_BACKUP_PLL_SHIFT));
 	}
 	if (!of_property_read_u32(np, "rockchip,boost-backup-pll-usage",
-				  &value)) {
-		pr_debug("boost-backup-pll-usage=0x%x\n", value);
+				  &pll->boost_backup_pll_usage)) {
+		pr_debug("boost-backup-pll-usage=0x%x\n",
+			 pll->boost_backup_pll_usage);
 		regmap_write(pll->boost, BOOST_CLK_CON,
-			     HIWORD_UPDATE(value, BOOST_BACKUP_PLL_USAGE_MASK,
+			     HIWORD_UPDATE(pll->boost_backup_pll_usage,
+					   BOOST_BACKUP_PLL_USAGE_MASK,
 					   BOOST_BACKUP_PLL_USAGE_SHIFT));
 	}
 	if (!of_property_read_u32(np, "rockchip,boost-switch-threshold",
@@ -1674,6 +1678,33 @@ void rockchip_boost_disable_recovery_sw(struct clk_hw *hw)
 				   BOOST_SW_CTRL_SHIFT));
 }
 
+void rockchip_boost_add_core_div(struct clk_hw *hw, unsigned long prate)
+{
+	struct rockchip_clk_pll *pll;
+	unsigned int div;
+
+	if (!hw)
+		return;
+	pll = to_rockchip_clk_pll(hw);
+	if (!pll->boost_enabled || pll->boost_backup_pll_rate == prate)
+		return;
+
+	/* todo */
+	if (pll->boost_backup_pll_usage == BOOST_BACKUP_PLL_USAGE_TARGET)
+		return;
+	/*
+	 * cpu clock rate should be less than or equal to
+	 * low rate when change pll rate in boost module
+	 */
+	if (pll->boost_low_rate && prate > pll->boost_low_rate) {
+		div =  DIV_ROUND_UP(prate, pll->boost_low_rate) - 1;
+		regmap_write(pll->boost, BOOST_CLK_CON,
+			     HIWORD_UPDATE(div, BOOST_CORE_DIV_MASK,
+					   BOOST_CORE_DIV_SHIFT));
+		pll->boost_backup_pll_rate = prate;
+	}
+}
+
 #ifdef CONFIG_DEBUG_FS
 #include <linux/debugfs.h>
 
diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h
index 6db1e7a66f19..3056082c37ba 100644
--- a/drivers/clk/rockchip/clk.h
+++ b/drivers/clk/rockchip/clk.h
@@ -46,10 +46,14 @@ struct clk;
 #define BOOST_FSM_STATUS		0x0028
 #define BOOST_PLL_L_CON(x)		((x) * 0x4 + 0x2c)
 #define BOOST_PLL_CON_MASK		0xffff
+#define BOOST_CORE_DIV_MASK		0x1f
+#define BOOST_CORE_DIV_SHIFT		0
 #define BOOST_BACKUP_PLL_MASK		0x3
 #define BOOST_BACKUP_PLL_SHIFT		8
 #define BOOST_BACKUP_PLL_USAGE_MASK	0x1
 #define BOOST_BACKUP_PLL_USAGE_SHIFT	12
+#define BOOST_BACKUP_PLL_USAGE_BORROW	0
+#define BOOST_BACKUP_PLL_USAGE_TARGET	1
 #define BOOST_ENABLE_MASK		0x1
 #define BOOST_ENABLE_SHIFT		0
 #define BOOST_RECOVERY_MASK		0x1
@@ -355,6 +359,8 @@ void rockchip_boost_enable_recovery_sw_low(struct clk_hw *hw);
 
 void rockchip_boost_disable_recovery_sw(struct clk_hw *hw);
 
+void rockchip_boost_add_core_div(struct clk_hw *hw, unsigned long prate);
+
 struct rockchip_cpuclk_clksel {
 	int reg;
 	u32 val;