clk: rockchip: Adjust the order of cpu boost

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

3 files changed, 62 insertions, 33 deletions

diff --git a/drivers/clk/rockchip/clk-cpu.c b/drivers/clk/rockchip/clk-cpu.c
index cd2b1d87f4a4..4d9f38de342b 100644
--- a/drivers/clk/rockchip/clk-cpu.c
+++ b/drivers/clk/rockchip/clk-cpu.c
@@ -121,7 +121,8 @@ static void rockchip_cpuclk_set_dividers(struct rockchip_cpuclk *cpuclk,
 }
 
 static int rockchip_cpuclk_pre_rate_change(struct rockchip_cpuclk *cpuclk,
-					   struct clk_notifier_data *ndata)
+					   struct clk_notifier_data *ndata,
+					   struct clk_hw *pphw)
 {
 	const struct rockchip_cpuclk_reg_data *reg_data = cpuclk->reg_data;
 	const struct rockchip_cpuclk_rate_table *rate;
@@ -136,6 +137,8 @@ static int rockchip_cpuclk_pre_rate_change(struct rockchip_cpuclk *cpuclk,
 		return -EINVAL;
 	}
 
+	rockchip_boost_enable_recovery_sw_low(pphw);
+
 	alt_prate = clk_get_rate(cpuclk->alt_parent);
 
 	spin_lock_irqsave(cpuclk->lock, flags);
@@ -184,7 +187,8 @@ static int rockchip_cpuclk_pre_rate_change(struct rockchip_cpuclk *cpuclk,
 }
 
 static int rockchip_cpuclk_post_rate_change(struct rockchip_cpuclk *cpuclk,
-					    struct clk_notifier_data *ndata)
+					    struct clk_notifier_data *ndata,
+					    struct clk_hw *pphw)
 {
 	const struct rockchip_cpuclk_reg_data *reg_data = cpuclk->reg_data;
 	const struct rockchip_cpuclk_rate_table *rate;
@@ -219,6 +223,8 @@ static int rockchip_cpuclk_post_rate_change(struct rockchip_cpuclk *cpuclk,
 	if (ndata->old_rate > ndata->new_rate)
 		rockchip_cpuclk_set_dividers(cpuclk, rate);
 
+	rockchip_boost_disable_recovery_sw(pphw);
+
 	spin_unlock_irqrestore(cpuclk->lock, flags);
 	return 0;
 }
@@ -234,14 +240,16 @@ static int rockchip_cpuclk_notifier_cb(struct notifier_block *nb,
 {
 	struct clk_notifier_data *ndata = data;
 	struct rockchip_cpuclk *cpuclk = to_rockchip_cpuclk_nb(nb);
+	struct clk_hw *phw = clk_hw_get_parent(__clk_get_hw(ndata->clk));
+	struct clk_hw *pphw = clk_hw_get_parent(phw);
 	int ret = 0;
 
 	pr_debug("%s: event %lu, old_rate %lu, new_rate: %lu\n",
 		 __func__, event, ndata->old_rate, ndata->new_rate);
 	if (event == PRE_RATE_CHANGE)
-		ret = rockchip_cpuclk_pre_rate_change(cpuclk, ndata);
+		ret = rockchip_cpuclk_pre_rate_change(cpuclk, ndata, pphw);
 	else if (event == POST_RATE_CHANGE)
-		ret = rockchip_cpuclk_post_rate_change(cpuclk, ndata);
+		ret = rockchip_cpuclk_post_rate_change(cpuclk, ndata, pphw);
 
 	return notifier_from_errno(ret);
 }
diff --git a/drivers/clk/rockchip/clk-pll.c b/drivers/clk/rockchip/clk-pll.c
index 2eb42294f8fa..80d1d4095f2e 100644
--- a/drivers/clk/rockchip/clk-pll.c
+++ b/drivers/clk/rockchip/clk-pll.c
@@ -330,6 +330,51 @@ static int rockchip_pll_wait_lock(struct rockchip_clk_pll *pll)
 	return -ETIMEDOUT;
 }
 
+void rockchip_boost_enable_recovery_sw_low(struct clk_hw *hw)
+{
+	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
+	int ret;
+
+	if (pll->type == pll_px30) {
+		writel_relaxed(HIWORD_UPDATE(1, PX30_BOOST_RECOVERY_MASK,
+					     PX30_BOOST_RECOVERY_SHIFT),
+			       pll->reg_base + PX30_BOOST_BOOST_CON);
+		do {
+			ret = readl_relaxed(pll->reg_base +
+					    PX30_BOOST_FSM_STATUS);
+		} while (!(ret & PX30_BOOST_BUSY_STATE));
+
+		writel_relaxed(HIWORD_UPDATE(1, PX30_BOOST_SW_CTRL_MASK,
+					     PX30_BOOST_SW_CTRL_SHIFT) |
+			       HIWORD_UPDATE(1, PX30_BOOST_LOW_FREQ_EN_MASK,
+					     PX30_BOOST_LOW_FREQ_EN_SHIFT),
+			       pll->reg_base + PX30_BOOST_BOOST_CON);
+	}
+}
+
+void rockchip_boost_disable_low(struct rockchip_clk_pll *pll)
+{
+	if (pll->type == pll_px30) {
+		writel_relaxed(HIWORD_UPDATE(0, PX30_BOOST_LOW_FREQ_EN_MASK,
+					     PX30_BOOST_LOW_FREQ_EN_SHIFT),
+			       pll->reg_base + PX30_BOOST_BOOST_CON);
+	}
+}
+
+void rockchip_boost_disable_recovery_sw(struct clk_hw *hw)
+{
+	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
+
+	if (pll->type == pll_px30) {
+		writel_relaxed(HIWORD_UPDATE(0, PX30_BOOST_RECOVERY_MASK,
+					     PX30_BOOST_RECOVERY_SHIFT),
+			       pll->reg_base + PX30_BOOST_BOOST_CON);
+		writel_relaxed(HIWORD_UPDATE(0, PX30_BOOST_SW_CTRL_MASK,
+					     PX30_BOOST_SW_CTRL_SHIFT),
+			       pll->reg_base + PX30_BOOST_BOOST_CON);
+	}
+}
+
 /**
  * PLL used in RK3036
  */
@@ -421,21 +466,6 @@ static int rockchip_rk3036_pll_set_params(struct rockchip_clk_pll *pll,
 	rockchip_rk3036_pll_get_params(pll, &cur);
 	cur.rate = 0;
 
-	if (pll->type == pll_px30) {
-		writel_relaxed(HIWORD_UPDATE(1, PX30_BOOST_RECOVERY_MASK,
-					     PX30_BOOST_RECOVERY_SHIFT),
-			       pll->reg_base + PX30_BOOST_BOOST_CON);
-		do {
-			ret = readl_relaxed(pll->reg_base +
-					    PX30_BOOST_FSM_STATUS);
-		} while (!(ret & PX30_BOOST_BUSY_STATE));
-		writel_relaxed(HIWORD_UPDATE(1, PX30_BOOST_SW_CTRL_MASK,
-					     PX30_BOOST_SW_CTRL_SHIFT) |
-			       HIWORD_UPDATE(1, PX30_BOOST_LOW_FREQ_EN_MASK,
-					     PX30_BOOST_LOW_FREQ_EN_SHIFT),
-			       pll->reg_base + PX30_BOOST_BOOST_CON);
-	}
-
 	cur_parent = pll_mux_ops->get_parent(&pll_mux->hw);
 	if (cur_parent == PLL_MODE_NORM) {
 		pll_mux_ops->set_parent(&pll_mux->hw, PLL_MODE_SLOW);
@@ -468,11 +498,7 @@ static int rockchip_rk3036_pll_set_params(struct rockchip_clk_pll *pll,
 	pllcon |= rate->frac << RK3036_PLLCON2_FRAC_SHIFT;
 	writel_relaxed(pllcon, pll->reg_base + RK3036_PLLCON(2));
 
-	if (pll->type == pll_px30) {
-		writel_relaxed(HIWORD_UPDATE(0, PX30_BOOST_LOW_FREQ_EN_MASK,
-					     PX30_BOOST_LOW_FREQ_EN_SHIFT),
-			       pll->reg_base + PX30_BOOST_BOOST_CON);
-	}
+	rockchip_boost_disable_low(pll);
 
 	/* set pll power up */
 	writel(HIWORD_UPDATE(0, RK3036_PLLCON1_PWRDOWN, 13),
@@ -489,15 +515,6 @@ static int rockchip_rk3036_pll_set_params(struct rockchip_clk_pll *pll,
 	if (rate_change_remuxed)
 		pll_mux_ops->set_parent(&pll_mux->hw, PLL_MODE_NORM);
 
-	if (pll->type == pll_px30) {
-		writel_relaxed(HIWORD_UPDATE(0, PX30_BOOST_RECOVERY_MASK,
-					     PX30_BOOST_RECOVERY_SHIFT),
-			       pll->reg_base + PX30_BOOST_BOOST_CON);
-		writel_relaxed(HIWORD_UPDATE(0, PX30_BOOST_SW_CTRL_MASK,
-					     PX30_BOOST_SW_CTRL_SHIFT),
-			       pll->reg_base + PX30_BOOST_BOOST_CON);
-	}
-
 	return ret;
 }
 
diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h
index e958deef3dfb..4a90b3371f0c 100644
--- a/drivers/clk/rockchip/clk.h
+++ b/drivers/clk/rockchip/clk.h
@@ -303,6 +303,10 @@ struct clk *rockchip_clk_register_pll(struct rockchip_clk_provider *ctx,
 		struct rockchip_pll_rate_table *rate_table,
 		unsigned long flags, u8 clk_pll_flags);
 
+void rockchip_boost_enable_recovery_sw_low(struct clk_hw *hw);
+
+void rockchip_boost_disable_recovery_sw(struct clk_hw *hw);
+
 struct rockchip_cpuclk_clksel {
 	int reg;
 	u32 val;