IOT2000_BuildRoot/board/PSG/iot2050/files/0020-fix-PLL4_DCO-freq-over-range-cause-DP-not-display.patch

From 1c9d8953f936f6e1beee71af59aa5e2533be0ef1 Mon Sep 17 00:00:00 2001
From: chao zeng <chao.zeng@siemens.com>
Date: Fri, 11 Dec 2020 17:20:12 +0800
Subject: [PATCH 20/26] fix:PLL4_DCO freq over range cause DP not display

  1.some DP may be can not display.
  2.reason: TI sysfw can not correct set PLL4 some frequency
            division parameter(M,N,M2),It will cause DCO over range,
            calculate DCO frequency not between HS1 and HS2 mode,
            So,CLKOUT is bypass mode,only out 25Mhz,Then DSS_PLL_CLKOUT
            and DPI_1_IN_CLK is always 25MHz,and It don't match the
            frequency of the monitor,so can not display.

            so,through dynamic acquisition of monitor frequency(Rate),
            calculation (M,N,M2) value and set correlation register.

Signed-off-by: Sheng Long Wang <shenglong.wang.ext@siemens.com>
---
 drivers/gpu/drm/tidss/tidss_dispc7.c | 196 +++++++++++++++++++++++++++
 1 file changed, 196 insertions(+)

diff --git a/drivers/gpu/drm/tidss/tidss_dispc7.c b/drivers/gpu/drm/tidss/tidss_dispc7.c
index 32f7535a0f83..b43cd213b6d9 100644
--- a/drivers/gpu/drm/tidss/tidss_dispc7.c
+++ b/drivers/gpu/drm/tidss/tidss_dispc7.c
@@ -33,6 +33,19 @@
 #include "tidss_scale_coefs.h"
 #include "tidss_dispc7.h"
 
+#define DSS_PLL_BASE_ADDR	0x00684000
+#define PLL_KICK0_OFFSET	0x0010
+#define PLL_KICK1_OFFSET	0x0014
+#define PLL_FREQ_CTRL0_OFFSET	0x0020
+#define PLL_FREQ_CTRL1_OFFSET	0x0024
+#define PLL_CLKDIV_OFFSET	0x0028
+#define PLL_PROG_OFFSET		0x002C
+#define PLL_CTRL_OFFSET		0x0030
+#define PLL_STAT_OFFSET		0x0034
+#define KICK0_UNLOCK		0x68EF3490
+#define KICK1_UNLOCK		0xD172BC5A
+#define KICK_LOCK		0x00000000
+
 static const char *dispc7_plane_name(struct dispc_device *dispc, u32 hw_plane);
 
 static const struct dispc7_features dispc7_am6_feats = {
@@ -1122,11 +1135,139 @@ static unsigned int dispc7_pclk_diff(unsigned long rate,
 	return (unsigned int)(abs(((rr - r) * 100) / r));
 }
 
+static void dss_pll4_reg_write(u32 reg, u32 val)
+{
+	void __iomem *regval;
+
+	regval = ioremap(DSS_PLL_BASE_ADDR + reg ,SZ_4);
+	writel(val, regval);
+	mdelay(2);
+	iounmap(regval);
+}
+
+static u32 dss_pll4_reg_read(u32 reg)
+{
+	void __iomem *regval;
+	u32 data;
+
+	regval = ioremap(DSS_PLL_BASE_ADDR + reg,SZ_4);
+	data = readl(regval);
+	iounmap(regval);
+
+	return data;
+}
+
+static int get_dp_clock_parameter(int Rate, u16 *M_INT, u8 *N_DIV, u8 *M2, u8 *choose_flag)
+{
+	u16 m2_div[127] = {0};
+	u16 result[127][3] = {0};
+	u16 hs2_dco_freq   = 0;
+	u16 hs1_dco_freq   = 0;
+	u16 start,ck,N,M;
+	u16 first = 0, second = 0, mid = 0;
+
+	u8  hs2_data_exist = 0;
+	u8  hs1_data_exist = 0;
+	u8  count = 0, ret = 0, i = 0;
+
+	if (Rate < 6 || Rate > 2500) {
+		return 0;
+	}
+
+	for (start = 750 / Rate + (750%Rate!=0); start <= 2500 / Rate; ++start) {
+		if (start >= 1 && start <= 127) {
+			m2_div[count] = start;
+			count++;
+		}
+	}
+
+	for (i = 0; i < count; ++i) {
+		ck = Rate * m2_div[i];
+		for ( N = 0; N <= 255; ++N) {
+			M = (N + 1) * ck / 25;
+			if ((M * 25 == (N + 1) * ck) && (M >= 2) && (M <= 4095)) {
+			printk(KERN_DEBUG "M2=%d, N=%d, M=%d\n", m2_div[i], N, M);
+			result[i][0] = m2_div[i];
+			result[i][1] = M;
+			result[i][2] = N;
+			ret = 1;
+			}
+		}
+	}
+
+	if (!ret) {
+		return 0;
+	}
+
+	for (i = 0; i < count; ++i) {
+		if (Rate * m2_div[i] <= 1500) first++;
+		if (Rate * m2_div[i] >= 1250)
+			second++;
+		else
+			mid++;
+	}
+
+	if (first == 0) {
+		hs2_data_exist = 1;
+	}
+
+	if (second == 0) {
+		hs1_data_exist = 1;
+	}
+
+	if((hs1_data_exist == 0) && (hs2_data_exist == 0)) {
+		hs2_dco_freq = 25 * result[first / 2][1] / (result[first / 2][2] + 1);
+		hs1_dco_freq = 25 * result[second / 2 + mid][1]  / ( result[second / 2 + mid][2] +1);
+
+		if ((abs(hs2_dco_freq - 1125) > abs(hs1_dco_freq - 1875))) {
+			printk(KERN_DEBUG "(hs1 mode) [1250-2500] : M2=%d, M=%d, N=%d\n",
+				result[second / 2 + mid][0], result[second / 2 + mid][1], result[second / 2 + mid][2]);
+			*M_INT = (u16)(result[second / 2 + mid][1]);
+			*N_DIV = (u8)(result[second  / 2 + mid][2]);
+			*M2    = (u8)(result[second  / 2 + mid][0]);
+			*choose_flag = 2;
+		} else {
+			printk(KERN_DEBUG "(hs2 mode)[750-1500] : M2=%d, M=%d, N=%d\n",
+				result[first / 2][0], result[first / 2][1], result[first / 2][2]);
+			*M_INT = (u16)(result[first / 2][1]);
+			*N_DIV = (u8)(result[first  / 2][2]);
+			*M2    = (u8)(result[first  / 2][0]);
+			*choose_flag = 1;
+		}
+	} else {
+		if ((hs2_data_exist == 0) && (hs1_data_exist == 1)) {
+			printk(KERN_DEBUG "(hs2 mode)[750-1500]   : M2=%d, M=%d, N=%d\n",
+				result[first / 2][0], result[first / 2][1], result[first / 2][2]);
+			*M_INT = (u16)(result[first / 2][1]);
+			*N_DIV = (u8)(result[first  / 2][2]);
+			*M2    = (u8)(result[first  / 2][0]);
+			*choose_flag = 1;
+		} else if ((hs2_data_exist == 1) && (hs1_data_exist == 0)) {
+				printk(KERN_DEBUG "(hs1 mode)[1250--2500] : M2=%d, M=%d, N=%d\n",
+					result[second / 2 + mid][0], result[second / 2 + mid][1], result[second / 2 + mid][2]);
+				*M_INT = (u16)(result[second / 2 + mid][1]);
+				*N_DIV = (u8)(result[second  / 2 + mid][2]);
+				*M2    = (u8)(result[second  / 2 + mid][0]);
+				*choose_flag = 2;
+		} else {
+			return 0;
+		}
+	}
+
+	return 1;
+}
+
+
 static int dispc7_vp_set_clk_rate(struct dispc_device *dispc, u32 hw_videoport,
 				  unsigned long rate)
 {
 	int r;
 	unsigned long new_rate;
+	u32 data;
+	u16 M_INT;
+	u8  N_DIV;
+	u8  M2;
+	u8  choose_flag = 0;
 
 	r = clk_set_rate(dispc->vp_clk[hw_videoport], rate);
 	if (r) {
@@ -1145,6 +1286,61 @@ static int dispc7_vp_set_clk_rate(struct dispc_device *dispc, u32 hw_videoport,
 	dev_dbg(dispc->dev, "vp%d: new rate %lu Hz (requested %lu Hz)\n",
 		hw_videoport, clk_get_rate(dispc->vp_clk[hw_videoport]), rate);
 
+	/**dynamic Get (M,N,M2) value**/
+	r = get_dp_clock_parameter(rate/1000000, &M_INT, &N_DIV, &M2, &choose_flag);
+	if(!r){
+		dev_err(dispc->dev,"get DP PLL parameter erro \n");
+		return 0;
+	}
+	mdelay(2);
+
+	/**Unlock PLL registers**/
+	dss_pll4_reg_write(PLL_KICK0_OFFSET,KICK0_UNLOCK);
+	dss_pll4_reg_write(PLL_KICK1_OFFSET,KICK1_UNLOCK);
+
+	/**Switch PLL outputs to bypass freq**/
+	dss_pll4_reg_write(PLL_CTRL_OFFSET,0x2100099);
+
+	/**Prepare PLL for programming**/
+	dss_pll4_reg_write(PLL_PROG_OFFSET,0);
+
+	/** DSS Clock Set **/
+	data = dss_pll4_reg_read(PLL_FREQ_CTRL0_OFFSET);
+	data &= 0xFFF00000;  //clear bit [0:19]
+	data |= (u32)N_DIV;
+	data |= (u32)(M_INT << 8);
+	dss_pll4_reg_write(PLL_FREQ_CTRL0_OFFSET,data);  //set M,N
+	if(choose_flag == 1)
+		dss_pll4_reg_write(PLL_FREQ_CTRL1_OFFSET,0x02000000);  //set M.f HS2
+	else if (choose_flag == 2)
+		dss_pll4_reg_write(PLL_FREQ_CTRL1_OFFSET,0x04000000);  //set M.f HS1
+
+
+	/**Set  M2 **/
+	data = dss_pll4_reg_read(PLL_CLKDIV_OFFSET);
+	data &= ~((0x7F << 8));  //clear bit [8:14]
+	data |= (u32)(M2 << 8);
+	dss_pll4_reg_write(PLL_CLKDIV_OFFSET,data);
+
+	/**Trigger PLL update to new values**/
+	dss_pll4_reg_write(PLL_PROG_OFFSET,0x2);
+
+	/**Trigger PLL update to new values**/
+	dss_pll4_reg_write(PLL_PROG_OFFSET,0x102);
+
+	/** Trigger PLL lock to new values**/
+	dss_pll4_reg_write(PLL_PROG_OFFSET,0x103);
+
+	/** Send PLL to idle**/
+	dss_pll4_reg_write(PLL_PROG_OFFSET,0x1);
+
+	/** Switch PLL outputs to locked freq**/
+	dss_pll4_reg_write(PLL_CTRL_OFFSET,0x2100019);
+
+	/**Lock PLL registers **/
+	dss_pll4_reg_write(PLL_KICK0_OFFSET,KICK_LOCK);
+	dss_pll4_reg_write(PLL_KICK1_OFFSET,KICK_LOCK);
+
 	return 0;
 }
 
-- 
2.31.1