From 93689e871eb70d20006095f8a927654503a03579 Mon Sep 17 00:00:00 2001
From: Cristian Ciocaltea <cristian.ciocaltea@collabora.com>
Date: Fri, 3 Nov 2023 19:58:02 +0200
Subject: [PATCH 430/463] drm/rockchip: vop2: Improve display modes handling on
 rk3588

The initial vop2 support for rk3588 in mainline is not able to handle
all display modes supported by connected displays, e.g.
2560x1440-75.00Hz, 2048x1152-60.00Hz, 1024x768-60.00Hz.

Additionally, it doesn't cope with non-integer refresh rates like 59.94,
29.97, 23.98, etc.

Improve HDMI0 clocking in order to support the additional display modes.

Fixes: 5a028e8f062f ("drm/rockchip: vop2: Add support for rk3588")
Signed-off-by: Cristian Ciocaltea <cristian.ciocaltea@collabora.com>
---
 drivers/gpu/drm/rockchip/rockchip_drm_vop2.c | 553 ++++++++++++++++++-
 1 file changed, 552 insertions(+), 1 deletion(-)

diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c b/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c
index 9873172e3fd33..629f60184f66d 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c
@@ -5,6 +5,8 @@
  */
 #include <linux/bitfield.h>
 #include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
 #include <linux/component.h>
 #include <linux/delay.h>
 #include <linux/iopoll.h>
@@ -212,6 +214,10 @@ struct vop2 {
 	struct clk *hclk;
 	struct clk *aclk;
 	struct clk *pclk;
+	// [CC:] hack to support additional display modes
+	struct clk *hdmi0_phy_pll;
+	/* list_head of internal clk */
+	struct list_head clk_list_head;
 
 	/* optional internal rgb encoder */
 	struct rockchip_rgb *rgb;
@@ -220,6 +226,19 @@ struct vop2 {
 	struct vop2_win win[];
 };
 
+struct vop2_clk {
+	struct vop2 *vop2;
+	struct list_head list;
+	unsigned long rate;
+	struct clk_hw hw;
+	struct clk_divider div;
+	int div_val;
+	u8 parent_index;
+};
+
+#define to_vop2_clk(_hw) container_of(_hw, struct vop2_clk, hw)
+#define VOP2_MAX_DCLK_RATE	600000 /* kHz */
+
 #define vop2_output_if_is_hdmi(x)	((x) == ROCKCHIP_VOP2_EP_HDMI0 || \
 					 (x) == ROCKCHIP_VOP2_EP_HDMI1)
 
@@ -1476,9 +1495,30 @@ static bool vop2_crtc_mode_fixup(struct drm_crtc *crtc,
 				 const struct drm_display_mode *mode,
 				 struct drm_display_mode *adj_mode)
 {
+	struct vop2_video_port *vp = to_vop2_video_port(crtc);
+	struct drm_connector *connector;
+	struct drm_connector_list_iter conn_iter;
+	struct drm_crtc_state *new_crtc_state = container_of(mode, struct drm_crtc_state, mode);
 	drm_mode_set_crtcinfo(adj_mode, CRTC_INTERLACE_HALVE_V |
 					CRTC_STEREO_DOUBLE);
 
+	if (mode->flags & DRM_MODE_FLAG_DBLCLK)
+		adj_mode->crtc_clock *= 2;
+
+	drm_connector_list_iter_begin(crtc->dev, &conn_iter);
+	drm_for_each_connector_iter(connector, &conn_iter) {
+		if ((new_crtc_state->connector_mask & drm_connector_mask(connector)) &&
+		    ((connector->connector_type == DRM_MODE_CONNECTOR_DisplayPort) ||
+		     (connector->connector_type == DRM_MODE_CONNECTOR_HDMIA))) {
+			drm_connector_list_iter_end(&conn_iter);
+			return true;
+		}
+	}
+	drm_connector_list_iter_end(&conn_iter);
+
+	if (adj_mode->crtc_clock <= VOP2_MAX_DCLK_RATE)
+		adj_mode->crtc_clock = DIV_ROUND_UP(clk_round_rate(vp->dclk,
+						    adj_mode->crtc_clock * 1000), 1000);
 	return true;
 }
 
@@ -1663,6 +1703,31 @@ static unsigned long rk3588_calc_dclk(unsigned long child_clk, unsigned long max
 		return 0;
 }
 
+static struct vop2_clk *vop2_clk_get(struct vop2 *vop2, const char *name);
+
+static int vop2_cru_set_rate(struct vop2_clk *if_pixclk, struct vop2_clk *if_dclk)
+{
+	int ret = 0;
+
+	if (if_pixclk) {
+		ret =  clk_set_rate(if_pixclk->hw.clk, if_pixclk->rate);
+		if (ret < 0) {
+			DRM_DEV_ERROR(if_pixclk->vop2->dev, "set %s to %ld failed: %d\n",
+				      clk_hw_get_name(&if_pixclk->hw), if_pixclk->rate, ret);
+			return ret;
+		}
+	}
+
+	if (if_dclk) {
+		ret = clk_set_rate(if_dclk->hw.clk, if_dclk->rate);
+		if (ret < 0)
+			DRM_DEV_ERROR(if_dclk->vop2->dev, "set %s to %ld failed %d\n",
+				      clk_hw_get_name(&if_dclk->hw), if_dclk->rate, ret);
+	}
+
+	return ret;
+}
+
 /*
  * 4 pixclk/cycle on rk3588
  * RGB/eDP/HDMI: if_pixclk >= dclk_core
@@ -1686,6 +1751,72 @@ static unsigned long rk3588_calc_cru_cfg(struct vop2_video_port *vp, int id,
 	int K = 1;
 
 	if (vop2_output_if_is_hdmi(id)) {
+		if (vop2->data->soc_id == 3588 && id == ROCKCHIP_VOP2_EP_HDMI0 &&
+		    vop2->hdmi0_phy_pll) {
+			const char *clk_src_name = "hdmi_edp0_clk_src";
+			const char *clk_parent_name = "dclk";
+			const char *pixclk_name = "hdmi_edp0_pixclk";
+			const char *dclk_name = "hdmi_edp0_dclk";
+			struct vop2_clk *if_clk_src, *if_clk_parent, *if_pixclk, *if_dclk, *dclk, *dclk_core, *dclk_out;
+			char clk_name[32];
+			int ret;
+
+			if_clk_src = vop2_clk_get(vop2, clk_src_name);
+			snprintf(clk_name, sizeof(clk_name), "%s%d", clk_parent_name, vp->id);
+			if_clk_parent = vop2_clk_get(vop2, clk_name);
+			if_pixclk = vop2_clk_get(vop2, pixclk_name);
+			if_dclk = vop2_clk_get(vop2, dclk_name);
+			if (!if_pixclk || !if_clk_parent) {
+				DRM_DEV_ERROR(vop2->dev, "failed to get connector interface clk\n");
+				return -ENODEV;
+			}
+
+			ret = clk_set_parent(if_clk_src->hw.clk, if_clk_parent->hw.clk);
+			if (ret < 0) {
+				DRM_DEV_ERROR(vop2->dev, "failed to set parent(%s) for %s: %d\n",
+					      __clk_get_name(if_clk_parent->hw.clk),
+					      __clk_get_name(if_clk_src->hw.clk), ret);
+				return ret;
+			}
+
+			if (output_mode == ROCKCHIP_OUT_MODE_YUV420)
+				K = 2;
+
+			if_pixclk->rate = (dclk_core_rate << 1) / K;
+			if_dclk->rate = dclk_core_rate / K;
+
+			snprintf(clk_name, sizeof(clk_name), "dclk_core%d", vp->id);
+			dclk_core = vop2_clk_get(vop2, clk_name);
+
+			snprintf(clk_name, sizeof(clk_name), "dclk_out%d", vp->id);
+			dclk_out = vop2_clk_get(vop2, clk_name);
+
+			snprintf(clk_name, sizeof(clk_name), "dclk%d", vp->id);
+			dclk = vop2_clk_get(vop2, clk_name);
+			if (v_pixclk <= (VOP2_MAX_DCLK_RATE * 1000)) {
+				if (output_mode == ROCKCHIP_OUT_MODE_YUV420)
+					v_pixclk = v_pixclk >> 1;
+			} else {
+				v_pixclk = v_pixclk >> 2;
+			}
+			clk_set_rate(dclk->hw.clk, v_pixclk);
+
+			if (dclk_core_rate > if_pixclk->rate) {
+				clk_set_rate(dclk_core->hw.clk, dclk_core_rate);
+				ret = vop2_cru_set_rate(if_pixclk, if_dclk);
+			} else {
+				ret = vop2_cru_set_rate(if_pixclk, if_dclk);
+				clk_set_rate(dclk_core->hw.clk, dclk_core_rate);
+			}
+
+			*dclk_core_div = dclk_core->div_val;
+			*dclk_out_div = dclk_out->div_val;
+			*if_pixclk_div = if_pixclk->div_val;
+			*if_dclk_div = if_dclk->div_val;
+
+			return dclk->rate;
+		}
+
 		/*
 		 * K = 2: dclk_core = if_pixclk_rate > if_dclk_rate
 		 * K = 1: dclk_core = hdmie_edp_dclk > if_pixclk_rate
@@ -1917,6 +2048,22 @@ static int us_to_vertical_line(struct drm_display_mode *mode, int us)
 	return us * mode->clock / mode->htotal / 1000;
 }
 
+// [CC:] rework virtual clock
+static struct vop2_clk *vop2_clk_get(struct vop2 *vop2, const char *name)
+{
+	struct vop2_clk *clk, *n;
+
+	if (!name)
+		return NULL;
+
+	list_for_each_entry_safe(clk, n, &vop2->clk_list_head, list) {
+		if (!strcmp(clk_hw_get_name(&clk->hw), name))
+			return clk;
+	}
+
+	return NULL;
+}
+
 static void vop2_crtc_atomic_enable(struct drm_crtc *crtc,
 				    struct drm_atomic_state *state)
 {
@@ -1944,6 +2091,8 @@ static void vop2_crtc_atomic_enable(struct drm_crtc *crtc,
 	u32 val, polflags;
 	int ret;
 	struct drm_encoder *encoder;
+	char clk_name[32];
+	struct vop2_clk *dclk;
 
 	drm_dbg(vop2->drm, "Update mode to %dx%d%s%d, type: %d for vp%d\n",
 		hdisplay, vdisplay, mode->flags & DRM_MODE_FLAG_INTERLACE ? "i" : "p",
@@ -2044,11 +2193,38 @@ static void vop2_crtc_atomic_enable(struct drm_crtc *crtc,
 
 	if (mode->flags & DRM_MODE_FLAG_DBLCLK) {
 		dsp_ctrl |= RK3568_VP_DSP_CTRL__CORE_DCLK_DIV;
-		clock *= 2;
+		// [CC:] done via mode_fixup
+		// clock *= 2;
 	}
 
 	vop2_vp_write(vp, RK3568_VP_MIPI_CTRL, 0);
 
+	snprintf(clk_name, sizeof(clk_name), "dclk%d", vp->id);
+	dclk = vop2_clk_get(vop2, clk_name);
+	if (dclk) {
+		/*
+		 * use HDMI_PHY_PLL as dclk source under 4K@60 if it is available,
+		 * otherwise use system cru as dclk source.
+		 */
+		drm_for_each_encoder_mask(encoder, crtc->dev, crtc_state->encoder_mask) {
+			struct rockchip_encoder *rkencoder = to_rockchip_encoder(encoder);
+
+			// [CC:] Using PHY PLL to handle all display modes
+			if (rkencoder->crtc_endpoint_id == ROCKCHIP_VOP2_EP_HDMI0) {
+				clk_get_rate(vop2->hdmi0_phy_pll);
+
+				if (mode->crtc_clock <= VOP2_MAX_DCLK_RATE) {
+					ret = clk_set_parent(vp->dclk, vop2->hdmi0_phy_pll);
+					if (ret < 0)
+						DRM_WARN("failed to set clock parent for %s\n",
+							 __clk_get_name(vp->dclk));
+				}
+
+				clock = dclk->rate;
+			}
+		}
+	}
+
 	clk_set_rate(vp->dclk, clock);
 
 	vop2_post_config(crtc);
@@ -2504,7 +2680,43 @@ static void vop2_crtc_atomic_flush(struct drm_crtc *crtc,
 	spin_unlock_irq(&crtc->dev->event_lock);
 }
 
+static enum drm_mode_status
+vop2_crtc_mode_valid(struct drm_crtc *crtc, const struct drm_display_mode *mode)
+{
+	struct rockchip_crtc_state *vcstate = to_rockchip_crtc_state(crtc->state);
+	struct vop2_video_port *vp = to_vop2_video_port(crtc);
+	struct vop2 *vop2 = vp->vop2;
+	const struct vop2_data *vop2_data = vop2->data;
+	const struct vop2_video_port_data *vp_data = &vop2_data->vp[vp->id];
+	int request_clock = mode->clock;
+	int clock;
+
+	if (mode->hdisplay > vp_data->max_output.width)
+		return MODE_BAD_HVALUE;
+
+	if (mode->flags & DRM_MODE_FLAG_DBLCLK)
+		request_clock *= 2;
+
+	if (request_clock <= VOP2_MAX_DCLK_RATE) {
+		clock = request_clock;
+	} else {
+		request_clock = request_clock >> 2;
+		clock = clk_round_rate(vp->dclk, request_clock * 1000) / 1000;
+	}
+
+	/*
+	 * Hdmi or DisplayPort request a Accurate clock.
+	 */
+	if (vcstate->output_type == DRM_MODE_CONNECTOR_HDMIA ||
+	    vcstate->output_type == DRM_MODE_CONNECTOR_DisplayPort)
+		if (clock != request_clock)
+			return MODE_CLOCK_RANGE;
+
+	return MODE_OK;
+}
+
 static const struct drm_crtc_helper_funcs vop2_crtc_helper_funcs = {
+	.mode_valid = vop2_crtc_mode_valid,
 	.mode_fixup = vop2_crtc_mode_fixup,
 	.atomic_check = vop2_crtc_atomic_check,
 	.atomic_begin = vop2_crtc_atomic_begin,
@@ -3074,6 +3286,336 @@ static const struct regmap_config vop2_regmap_config = {
 	.cache_type	= REGCACHE_MAPLE,
 };
 
+/*
+ * BEGIN virtual clock
+ */
+#define PLL_RATE_MIN	30000000
+
+#define cru_dbg(format, ...) do {				\
+		if (cru_debug)					\
+			pr_info("%s: " format, __func__, ## __VA_ARGS__); \
+	} while (0)
+
+#define PNAME(x) static const char *const x[]
+
+enum vop_clk_branch_type {
+	branch_mux,
+	branch_divider,
+	branch_factor,
+	branch_virtual,
+};
+
+#define VIR(cname)						\
+	{							\
+		.branch_type	= branch_virtual,		\
+		.name		= cname,			\
+	}
+
+
+#define MUX(cname, pnames, f)			\
+	{							\
+		.branch_type	= branch_mux,			\
+		.name		= cname,			\
+		.parent_names	= pnames,			\
+		.num_parents	= ARRAY_SIZE(pnames),		\
+		.flags		= f,				\
+	}
+
+#define FACTOR(cname, pname,  f)			\
+	{							\
+		.branch_type	= branch_factor,		\
+		.name		= cname,			\
+		.parent_names	= (const char *[]){ pname },	\
+		.num_parents	= 1,				\
+		.flags		= f,				\
+	}
+
+#define DIV(cname, pname, f, w)			\
+	{							\
+		.branch_type	= branch_divider,		\
+		.name		= cname,			\
+		.parent_names	= (const char *[]){ pname },	\
+		.num_parents	= 1,				\
+		.flags		= f,				\
+		.div_width	= w,				\
+	}
+
+struct vop2_clk_branch {
+	enum vop_clk_branch_type	branch_type;
+	const char			*name;
+	const char			*const *parent_names;
+	u8				num_parents;
+	unsigned long			flags;
+	u8				div_shift;
+	u8				div_width;
+	u8				div_flags;
+};
+
+PNAME(mux_port0_dclk_src_p)		= { "dclk0", "dclk1" };
+PNAME(mux_port2_dclk_src_p)		= { "dclk2", "dclk1" };
+PNAME(mux_dp_pixclk_p)			= { "dclk_out0", "dclk_out1", "dclk_out2" };
+PNAME(mux_hdmi_edp_clk_src_p)		= { "dclk0", "dclk1", "dclk2" };
+PNAME(mux_mipi_clk_src_p)		= { "dclk_out1", "dclk_out2", "dclk_out3" };
+PNAME(mux_dsc_8k_clk_src_p)		= { "dclk0", "dclk1", "dclk2", "dclk3" };
+PNAME(mux_dsc_4k_clk_src_p)		= { "dclk0", "dclk1", "dclk2", "dclk3" };
+
+/*
+ * We only use this clk driver calculate the div
+ * of dclk_core/dclk_out/if_pixclk/if_dclk and
+ * the rate of the dclk from the soc.
+ *
+ * We don't touch the cru in the vop here, as
+ * these registers has special read andy write
+ * limits.
+ */
+static struct vop2_clk_branch rk3588_vop_clk_branches[] = {
+	VIR("dclk0"),
+	VIR("dclk1"),
+	VIR("dclk2"),
+	VIR("dclk3"),
+
+	MUX("port0_dclk_src", mux_port0_dclk_src_p, CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+	DIV("dclk_core0", "port0_dclk_src", CLK_SET_RATE_PARENT, 2),
+	DIV("dclk_out0", "port0_dclk_src", CLK_SET_RATE_PARENT, 2),
+
+	FACTOR("port1_dclk_src", "dclk1", CLK_SET_RATE_PARENT),
+	DIV("dclk_core1", "port1_dclk_src", CLK_SET_RATE_PARENT, 2),
+	DIV("dclk_out1", "port1_dclk_src", CLK_SET_RATE_PARENT, 2),
+
+	MUX("port2_dclk_src", mux_port2_dclk_src_p, CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+	DIV("dclk_core2", "port2_dclk_src", CLK_SET_RATE_PARENT, 2),
+	DIV("dclk_out2", "port2_dclk_src", CLK_SET_RATE_PARENT, 2),
+
+	FACTOR("port3_dclk_src", "dclk3", CLK_SET_RATE_PARENT),
+	DIV("dclk_core3", "port3_dclk_src", CLK_SET_RATE_PARENT, 2),
+	DIV("dclk_out3", "port3_dclk_src", CLK_SET_RATE_PARENT, 2),
+
+	MUX("dp0_pixclk", mux_dp_pixclk_p, CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+	MUX("dp1_pixclk", mux_dp_pixclk_p, CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+
+	MUX("hdmi_edp0_clk_src", mux_hdmi_edp_clk_src_p,
+	    CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+	DIV("hdmi_edp0_dclk", "hdmi_edp0_clk_src", 0, 2),
+	DIV("hdmi_edp0_pixclk", "hdmi_edp0_clk_src", CLK_SET_RATE_PARENT, 1),
+
+	MUX("hdmi_edp1_clk_src", mux_hdmi_edp_clk_src_p,
+	    CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+	DIV("hdmi_edp1_dclk", "hdmi_edp1_clk_src", 0, 2),
+	DIV("hdmi_edp1_pixclk", "hdmi_edp1_clk_src", CLK_SET_RATE_PARENT, 1),
+
+	MUX("mipi0_clk_src", mux_mipi_clk_src_p, CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+	DIV("mipi0_pixclk", "mipi0_clk_src", CLK_SET_RATE_PARENT, 2),
+
+	MUX("mipi1_clk_src", mux_mipi_clk_src_p, CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+	DIV("mipi1_pixclk", "mipi1_clk_src", CLK_SET_RATE_PARENT, 2),
+
+	FACTOR("rgb_pixclk", "port3_dclk_src", CLK_SET_RATE_PARENT),
+
+	MUX("dsc_8k_txp_clk_src", mux_dsc_8k_clk_src_p, CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+	DIV("dsc_8k_txp_clk", "dsc_8k_txp_clk_src", 0, 2),
+	DIV("dsc_8k_pxl_clk", "dsc_8k_txp_clk_src", 0, 2),
+	DIV("dsc_8k_cds_clk", "dsc_8k_txp_clk_src", 0, 2),
+
+	MUX("dsc_4k_txp_clk_src", mux_dsc_4k_clk_src_p, CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT),
+	DIV("dsc_4k_txp_clk", "dsc_4k_txp_clk_src", 0, 2),
+	DIV("dsc_4k_pxl_clk", "dsc_4k_txp_clk_src", 0, 2),
+	DIV("dsc_4k_cds_clk", "dsc_4k_txp_clk_src", 0, 2),
+};
+
+static unsigned long clk_virtual_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct vop2_clk *vop2_clk = to_vop2_clk(hw);
+
+	return (unsigned long)vop2_clk->rate;
+}
+
+static long clk_virtual_round_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long *prate)
+{
+	struct vop2_clk *vop2_clk = to_vop2_clk(hw);
+
+	vop2_clk->rate = rate;
+
+	return rate;
+}
+
+static int clk_virtual_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	return 0;
+}
+
+const struct clk_ops clk_virtual_ops = {
+	.round_rate = clk_virtual_round_rate,
+	.set_rate = clk_virtual_set_rate,
+	.recalc_rate = clk_virtual_recalc_rate,
+};
+
+static u8 vop2_mux_get_parent(struct clk_hw *hw)
+{
+	struct vop2_clk *vop2_clk = to_vop2_clk(hw);
+
+	// cru_dbg("%s index: %d\n", clk_hw_get_name(hw), vop2_clk->parent_index);
+	return vop2_clk->parent_index;
+}
+
+static int vop2_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct vop2_clk *vop2_clk = to_vop2_clk(hw);
+
+	vop2_clk->parent_index = index;
+
+	// cru_dbg("%s index: %d\n", clk_hw_get_name(hw), index);
+	return 0;
+}
+
+static int vop2_clk_mux_determine_rate(struct clk_hw *hw,
+			     struct clk_rate_request *req)
+{
+	// cru_dbg("%s  %ld(min: %ld max: %ld)\n",
+	//        clk_hw_get_name(hw), req->rate, req->min_rate, req->max_rate);
+	return __clk_mux_determine_rate(hw, req);
+}
+
+static const struct clk_ops vop2_mux_clk_ops = {
+	.get_parent = vop2_mux_get_parent,
+	.set_parent = vop2_mux_set_parent,
+	.determine_rate = vop2_clk_mux_determine_rate,
+};
+
+#define div_mask(width)	((1 << (width)) - 1)
+
+static int vop2_div_get_val(unsigned long rate, unsigned long parent_rate)
+{
+	unsigned int div, value;
+
+	div = DIV_ROUND_UP_ULL((u64)parent_rate, rate);
+
+	value = ilog2(div);
+
+	return value;
+}
+
+static unsigned long vop2_clk_div_recalc_rate(struct clk_hw *hw,
+					  unsigned long parent_rate)
+{
+	struct vop2_clk *vop2_clk = to_vop2_clk(hw);
+	unsigned long rate;
+	unsigned int div;
+
+	div =  1 << vop2_clk->div_val;
+	rate = parent_rate / div;
+
+	// cru_dbg("%s rate: %ld(prate: %ld)\n", clk_hw_get_name(hw), rate, parent_rate);
+	return rate;
+}
+
+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);
+	return rate;
+}
+
+static int vop2_clk_div_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate)
+{
+	struct vop2_clk *vop2_clk = to_vop2_clk(hw);
+	int div_val;
+
+	div_val = vop2_div_get_val(rate, parent_rate);
+	vop2_clk->div_val = div_val;
+
+	// cru_dbg("%s prate: %ld rate: %ld div_val: %d\n",
+	//        clk_hw_get_name(hw), parent_rate, rate, div_val);
+	return 0;
+}
+
+static const struct clk_ops vop2_div_clk_ops = {
+	.recalc_rate = vop2_clk_div_recalc_rate,
+	.round_rate = vop2_clk_div_round_rate,
+	.set_rate = vop2_clk_div_set_rate,
+};
+
+static struct clk *vop2_clk_register(struct vop2 *vop2, struct vop2_clk_branch *branch)
+{
+	struct clk_init_data init = {};
+	struct vop2_clk *vop2_clk;
+	struct clk *clk;
+
+	vop2_clk = devm_kzalloc(vop2->dev, sizeof(*vop2_clk), GFP_KERNEL);
+	if (!vop2_clk)
+		return ERR_PTR(-ENOMEM);
+
+	vop2_clk->vop2 = vop2;
+	vop2_clk->hw.init = &init;
+	vop2_clk->div.shift = branch->div_shift;
+	vop2_clk->div.width = branch->div_width;
+
+	init.name = branch->name;
+	init.flags = branch->flags;
+	init.num_parents = branch->num_parents;
+	init.parent_names = branch->parent_names;
+	if (branch->branch_type == branch_divider) {
+		init.ops = &vop2_div_clk_ops;
+	} else if (branch->branch_type == branch_virtual) {
+		init.ops = &clk_virtual_ops;
+		init.num_parents = 0;
+		init.parent_names = NULL;
+	} else {
+		init.ops = &vop2_mux_clk_ops;
+	}
+
+	clk = devm_clk_register(vop2->dev, &vop2_clk->hw);
+	if (!IS_ERR(clk))
+		list_add_tail(&vop2_clk->list, &vop2->clk_list_head);
+	else
+		DRM_DEV_ERROR(vop2->dev, "Register %s failed\n", branch->name);
+
+	return clk;
+}
+
+static int vop2_clk_init(struct vop2 *vop2)
+{
+	struct vop2_clk_branch *branch = rk3588_vop_clk_branches;
+	unsigned int nr_clk = ARRAY_SIZE(rk3588_vop_clk_branches);
+	unsigned int idx;
+	struct vop2_clk *clk, *n;
+
+	INIT_LIST_HEAD(&vop2->clk_list_head);
+
+	if (vop2->data->soc_id < 3588 || vop2->hdmi0_phy_pll == NULL)
+		return 0;
+
+	list_for_each_entry_safe(clk, n, &vop2->clk_list_head, list) {
+		list_del(&clk->list);
+	}
+
+	for (idx = 0; idx < nr_clk; idx++, branch++)
+		vop2_clk_register(vop2, branch);
+
+	return 0;
+}
+/*
+ * END virtual clock
+ */
+
 static int vop2_bind(struct device *dev, struct device *master, void *data)
 {
 	struct platform_device *pdev = to_platform_device(dev);
@@ -3167,6 +3709,12 @@ static int vop2_bind(struct device *dev, struct device *master, void *data)
 		return PTR_ERR(vop2->pclk);
 	}
 
+	vop2->hdmi0_phy_pll = devm_clk_get_optional(vop2->drm->dev, "hdmi0_phy_pll");
+	if (IS_ERR(vop2->hdmi0_phy_pll)) {
+		DRM_DEV_ERROR(vop2->dev, "failed to get hdmi0_phy_pll source\n");
+		return PTR_ERR(vop2->hdmi0_phy_pll);
+	}
+
 	vop2->irq = platform_get_irq(pdev, 0);
 	if (vop2->irq < 0) {
 		drm_err(vop2->drm, "cannot find irq for vop2\n");
@@ -3183,6 +3731,9 @@ static int vop2_bind(struct device *dev, struct device *master, void *data)
 	if (ret)
 		return ret;
 
+	// [CC:] rework virtual clock
+	vop2_clk_init(vop2);
+
 	ret = vop2_find_rgb_encoder(vop2);
 	if (ret >= 0) {
 		vop2->rgb = rockchip_rgb_init(dev, &vop2->vps[ret].crtc,
-- 
2.46.0

