From 4f5250b100398f632f8083bc69a505a36caf250a Mon Sep 17 00:00:00 2001
From: Andy Yan <andy.yan@rock-chips.com>
Date: Tue, 16 Nov 2021 22:07:25 +0800
Subject: [PATCH] drm/rockchip: vop2: Check internal cru div width

Make sure the div we set is not out of max div.

Signed-off-by: Andy Yan <andy.yan@rock-chips.com>
Change-Id: Ifa9a94e519894da340a91bbbd339c9d01b14104c
---
 drivers/gpu/drm/rockchip/rockchip_drm_vop2.c |  7 +++++--
 drivers/gpu/drm/rockchip/rockchip_vop2_clk.c | 13 +++++++++++++
 2 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c b/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c
index 9906d38e7b10..dac9803baba7 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c
@@ -5835,10 +5835,13 @@ static void vop2_crtc_atomic_enable(struct drm_crtc *crtc, struct drm_crtc_state
 
 	snprintf(clk_name, sizeof(clk_name), "dclk%d", vp->id);
 	dclk = vop2_clk_get(vop2, clk_name);
-	if (dclk)
+	if (dclk) {
 		clk_set_rate(vp->dclk, dclk->rate);
-	else
+		DRM_DEV_INFO(vop2->dev, "set %s to %ld, get %ld\n",
+			      __clk_get_name(vp->dclk), dclk->rate, clk_get_rate(vp->dclk));
+	} else {
 		clk_set_rate(vp->dclk, adjusted_mode->crtc_clock * 1000);
+	}
 
 	if (vp_data->feature & VOP_FEATURE_OVERSCAN)
 		vop2_post_config(crtc);
diff --git a/drivers/gpu/drm/rockchip/rockchip_vop2_clk.c b/drivers/gpu/drm/rockchip/rockchip_vop2_clk.c
index 7a4beaa7be18..fd5dfaab358e 100644
--- a/drivers/gpu/drm/rockchip/rockchip_vop2_clk.c
+++ b/drivers/gpu/drm/rockchip/rockchip_vop2_clk.c
@@ -9,6 +9,8 @@
 
 static int cru_debug;
 
+#define PLL_RATE_MIN	30000000
+
 #define cru_dbg(format, ...) do {				\
 		if (cru_debug)					\
 			pr_info("%s: " format, __func__, ## __VA_ARGS__); \
@@ -233,9 +235,20 @@ static unsigned long vop2_clk_div_recalc_rate(struct clk_hw *hw,
 static long vop2_clk_div_round_rate(struct clk_hw *hw, unsigned long rate,
 				unsigned long *prate)
 {
+	struct vop2_clk *vop2_clk = to_vop2_clk(hw);
+
 	if (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) {
 		if (*prate < rate)
 			*prate = rate;
+		if ((*prate >> vop2_clk->div.width) > rate)
+			*prate = rate;
+
+		if ((*prate % rate))
+			*prate = rate;
+
+		/* SOC PLL can't output a too low pll freq */
+		if (*prate < PLL_RATE_MIN)
+			*prate = rate << vop2_clk->div.width;
 	}
 
 	cru_dbg("%s rate: %ld(prate: %ld)\n", clk_hw_get_name(hw), rate, *prate);