clk: rockchip: add safe frequency support

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

Author: finley1226

drivers/clk/rockchip/clk-pll.c

@@ -31,6 +31,9 @@
 #define PLL_MODE_NORM		0x1
 #define PLL_MODE_DEEP		0x2
 
+unsigned long apll_safefreq = ULONG_MAX;
+unsigned long apll_maxfreq;
+
 struct rockchip_clk_pll {
 	struct clk_hw		hw;
 
@@ -49,6 +52,7 @@ struct rockchip_clk_pll {
 	spinlock_t		*lock;
 
 	struct rockchip_clk_provider *ctx;
+	u8			id;
 };
 
 #define to_rockchip_clk_pll(_hw) container_of(_hw, struct rockchip_clk_pll, hw)
@@ -617,6 +621,9 @@ static unsigned long rockchip_rk3066_pll_recalc_rate(struct clk_hw *hw,
 		return prate;
 	}
 
+	if (pll->id == 1 && apll_maxfreq)
+		return apll_maxfreq;
+
 	rockchip_rk3066_pll_get_params(pll, &cur);
 
 	rate64 *= cur.nf;
@@ -691,14 +698,19 @@ static int rockchip_rk3066_pll_set_rate(struct clk_hw *hw, unsigned long drate,
 	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
 	const struct rockchip_pll_rate_table *rate;
 	unsigned long old_rate = rockchip_rk3066_pll_recalc_rate(hw, prate);
+	 unsigned long temp_rate = drate;
 	struct regmap *grf = rockchip_clk_get_grf(pll->ctx);
+	int ret;
 
 	if (IS_ERR(grf)) {
 		pr_debug("%s: grf regmap not available, aborting rate change\n",
 			 __func__);
 		return PTR_ERR(grf);
 	}
 
+	if (pll->id == 1 && drate > apll_safefreq)
+		drate = apll_safefreq;
+
 	pr_debug("%s: changing %s from %lu to %lu with a parent rate of %lu\n",
 		 __func__, clk_hw_get_name(hw), old_rate, drate, prate);
 
@@ -710,7 +722,13 @@ static int rockchip_rk3066_pll_set_rate(struct clk_hw *hw, unsigned long drate,
 		return -EINVAL;
 	}
 
-	return rockchip_rk3066_pll_set_params(pll, rate);
+	ret = rockchip_rk3066_pll_set_params(pll, rate);
+	if (!ret && pll->id == 1 && drate == apll_safefreq)
+		apll_maxfreq = temp_rate;
+	else
+		apll_maxfreq = 0;
+
+	return ret;
 }
 
 static int rockchip_rk3066_pll_enable(struct clk_hw *hw)
@@ -1224,7 +1242,7 @@ struct clk *rockchip_clk_register_pll(struct rockchip_clk_provider *ctx,
 		u8 num_parents, int con_offset, int grf_lock_offset,
 		int lock_shift, int mode_offset, int mode_shift,
 		struct rockchip_pll_rate_table *rate_table,
-		unsigned long flags, u8 clk_pll_flags)
+		unsigned long flags, u8 clk_pll_flags, u8 id)
 {
 	const char *pll_parents[3];
 	struct clk_init_data init;
@@ -1340,6 +1358,7 @@ struct clk *rockchip_clk_register_pll(struct rockchip_clk_provider *ctx,
 	pll->flags = clk_pll_flags;
 	pll->lock = &ctx->lock;
 	pll->ctx = ctx;
+	pll->id = id;
 
 	pll_clk = clk_register(NULL, &pll->hw);
 	if (IS_ERR(pll_clk)) {

drivers/clk/rockchip/clk.c

@@ -391,7 +391,7 @@ void __init rockchip_clk_register_plls(struct rockchip_clk_provider *ctx,
 				list->con_offset, grf_lock_offset,
 				list->lock_shift, list->mode_offset,
 				list->mode_shift, list->rate_table,
-				list->flags, list->pll_flags);
+				list->flags, list->pll_flags, list->id);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n", __func__,
 				list->name);

drivers/clk/rockchip/clk.h

@@ -242,7 +242,7 @@ struct clk *rockchip_clk_register_pll(struct rockchip_clk_provider *ctx,
 		u8 num_parents, int con_offset, int grf_lock_offset,
 		int lock_shift, int mode_offset, int mode_shift,
 		struct rockchip_pll_rate_table *rate_table,
-		unsigned long flags, u8 clk_pll_flags);
+		unsigned long flags, u8 clk_pll_flags, u8 id);
 
 struct rockchip_cpuclk_clksel {
 	int reg;