disp: pll: fix sequence as per hardware recommendations

Update the PLL and PHY power on and clock set sequence as per
the hardware recommendations. Move the post link clock phy enable
part to the catalog so that it can be programmed after enabling
link clock.

Change-Id: I9b3b49e5a9ac93bebcb1cb7da63b715a8d5ed85c
Signed-off-by: Ajay Singh Parmar <[email protected]>

msm/dp/dp_catalog.c

@@ -6,6 +6,7 @@
 #define pr_fmt(fmt)	"[drm-dp] %s: " fmt, __func__
 
 #include <linux/delay.h>
+#include <linux/iopoll.h>
 #include <drm/drm_dp_helper.h>
 
 #include "dp_catalog.h"
@@ -14,6 +15,8 @@
 #define DP_GET_MSB(x)	(x >> 8)
 #define DP_GET_LSB(x)	(x & 0xff)
 
+#define DP_PHY_READY BIT(1)
+
 #define dp_catalog_get_priv(x) ({ \
 	struct dp_catalog *dp_catalog; \
 	dp_catalog = container_of(x, struct dp_catalog, x); \
@@ -354,7 +357,111 @@ static void dp_catalog_aux_get_irq(struct dp_catalog_aux *aux, bool cmd_busy)
 	dp_write(catalog->exe_mode, io_data, DP_INTR_STATUS, ack);
 }
 
+static bool dp_catalog_ctrl_wait_for_phy_ready(
+		struct dp_catalog_private *catalog)
+{
+	u32 reg = DP_PHY_STATUS, state;
+	void __iomem *base = catalog->io.dp_phy->io.base;
+	bool success = true;
+	u32 const poll_sleep_us = 500;
+	u32 const pll_timeout_us = 10000;
+
+	if (readl_poll_timeout_atomic((base + reg), state,
+			((state & DP_PHY_READY) > 0),
+			poll_sleep_us, pll_timeout_us)) {
+		pr_err("PHY status failed, status=%x\n", state);
+
+		success = false;
+	}
+
+	return success;
+}
+
 /* controller related catalog functions */
+static int dp_catalog_ctrl_late_phy_init(struct dp_catalog_ctrl *ctrl,
+					u8 lane_cnt, bool flipped)
+{
+	int rc = 0;
+	u32 bias0_en, drvr0_en, bias1_en, drvr1_en;
+	struct dp_catalog_private *catalog;
+	struct dp_io_data *io_data;
+
+	if (!ctrl) {
+		pr_err("invalid input\n");
+		return -EINVAL;
+	}
+
+	catalog = dp_catalog_get_priv(ctrl);
+
+	switch (lane_cnt) {
+	case 1:
+		drvr0_en = flipped ? 0x13 : 0x10;
+		bias0_en = flipped ? 0x3E : 0x15;
+		drvr1_en = flipped ? 0x10 : 0x13;
+		bias1_en = flipped ? 0x15 : 0x3E;
+		break;
+	case 2:
+		drvr0_en = flipped ? 0x10 : 0x10;
+		bias0_en = flipped ? 0x3F : 0x15;
+		drvr1_en = flipped ? 0x10 : 0x10;
+		bias1_en = flipped ? 0x15 : 0x3F;
+		break;
+	case 4:
+	default:
+		drvr0_en = 0x10;
+		bias0_en = 0x3F;
+		drvr1_en = 0x10;
+		bias1_en = 0x3F;
+		break;
+	}
+
+	io_data = catalog->io.dp_ln_tx0;
+	dp_write(catalog->exe_mode, io_data, TXn_HIGHZ_DRVR_EN_V420, drvr0_en);
+	dp_write(catalog->exe_mode, io_data,
+			TXn_TRANSCEIVER_BIAS_EN_V420, bias0_en);
+
+	io_data = catalog->io.dp_ln_tx1;
+	dp_write(catalog->exe_mode, io_data, TXn_HIGHZ_DRVR_EN_V420, drvr1_en);
+	dp_write(catalog->exe_mode, io_data,
+			TXn_TRANSCEIVER_BIAS_EN_V420, bias1_en);
+
+	io_data = catalog->io.dp_phy;
+	dp_write(catalog->exe_mode, io_data, DP_PHY_CFG, 0x18);
+	/* add hardware recommended delay */
+	udelay(2000);
+	dp_write(catalog->exe_mode, io_data, DP_PHY_CFG, 0x19);
+
+	/*
+	 * Make sure all the register writes are completed before
+	 * doing any other operation
+	 */
+	wmb();
+
+	if (!dp_catalog_ctrl_wait_for_phy_ready(catalog)) {
+		rc = -EINVAL;
+		goto lock_err;
+	}
+
+	io_data = catalog->io.dp_ln_tx0;
+	dp_write(catalog->exe_mode, io_data, TXn_TX_POL_INV_V420, 0x0a);
+	io_data = catalog->io.dp_ln_tx1;
+	dp_write(catalog->exe_mode, io_data, TXn_TX_POL_INV_V420, 0x0a);
+
+	io_data = catalog->io.dp_ln_tx0;
+	dp_write(catalog->exe_mode, io_data, TXn_TX_DRV_LVL_V420, 0x27);
+	io_data = catalog->io.dp_ln_tx1;
+	dp_write(catalog->exe_mode, io_data, TXn_TX_DRV_LVL_V420, 0x27);
+
+	io_data = catalog->io.dp_ln_tx0;
+	dp_write(catalog->exe_mode, io_data, TXn_TX_EMP_POST1_LVL, 0x20);
+	io_data = catalog->io.dp_ln_tx1;
+	dp_write(catalog->exe_mode, io_data, TXn_TX_EMP_POST1_LVL, 0x20);
+	/* Make sure the PHY register writes are done */
+	wmb();
+lock_err:
+	return rc;
+}
+
 static u32 dp_catalog_ctrl_read_hdcp_status(struct dp_catalog_ctrl *ctrl)
 {
 	struct dp_catalog_private *catalog;
@@ -2609,6 +2716,7 @@ struct dp_catalog *dp_catalog_get(struct device *dev, struct dp_parser *parser)
 		.channel_dealloc = dp_catalog_ctrl_channel_dealloc,
 		.fec_config = dp_catalog_ctrl_fec_config,
 		.mainlink_levels = dp_catalog_ctrl_mainlink_levels,
+		.late_phy_init = dp_catalog_ctrl_late_phy_init,
 	};
 	struct dp_catalog_hpd hpd = {
 		.config_hpd	= dp_catalog_hpd_config_hpd,

msm/dp/dp_catalog.h

@@ -121,6 +121,9 @@ struct dp_catalog_ctrl {
 			u32 ch, u32 ch_start_timeslot, u32 tot_ch_cnt);
 	void (*fec_config)(struct dp_catalog_ctrl *ctrl, bool enable);
 	void (*mainlink_levels)(struct dp_catalog_ctrl *ctrl, u8 lane_cnt);
+
+	int (*late_phy_init)(struct dp_catalog_ctrl *ctrl,
+					u8 lane_cnt, bool flipped);
 };
 
 struct dp_catalog_hpd {

msm/dp/dp_ctrl.c

@@ -663,6 +663,10 @@ static int dp_ctrl_link_setup(struct dp_ctrl_private *ctrl, bool shallow)
 		if (rc)
 			break;
 
+		ctrl->catalog->late_phy_init(ctrl->catalog,
+			ctrl->link->link_params.lane_count,
+			ctrl->orientation);
+
 		dp_ctrl_configure_source_link_params(ctrl, true);
 
 		rc = dp_ctrl_setup_main_link(ctrl);

msm/dp/dp_reg.h

@@ -374,11 +374,17 @@
 #define TXn_TX_DRV_LVL				(0x001C)
 #define TXn_TX_POL_INV				(0x0064)
 
+#define TXn_TRANSCEIVER_BIAS_EN			(0x005C)
+#define TXn_HIGHZ_DRVR_EN			(0x0060)
+
+#define DP_PHY_STATUS				(0x00DC)
 #define DP_PHY_AUX_INTERRUPT_MASK_V420		(0x0054)
 #define DP_PHY_AUX_INTERRUPT_CLEAR_V420		(0x0058)
 #define DP_PHY_AUX_INTERRUPT_STATUS_V420	(0x00D8)
 #define DP_PHY_SPARE0_V420			(0x00C8)
 #define TXn_TX_DRV_LVL_V420			(0x0014)
+#define TXn_TRANSCEIVER_BIAS_EN_V420		(0x0054)
+#define TXn_HIGHZ_DRVR_EN_V420			(0x0058)
 #define TXn_TX_POL_INV_V420			(0x005C)
 
 #define QSERDES_COM_BIAS_EN_CLKBUFLR_EN		(0x004)

pll/dp_pll_7nm_util.c

@@ -16,6 +16,7 @@
 #include "dp_pll_7nm.h"
 
 #define DP_PHY_CFG				0x0010
+#define DP_PHY_CFG_1				0x0014
 #define DP_PHY_PD_CTL				0x0018
 #define DP_PHY_MODE				0x001C
 
@@ -74,6 +75,7 @@
 #define QSERDES_COM_VCO_TUNE_CTRL		0x0108
 #define QSERDES_COM_VCO_TUNE_MAP		0x010C
 
+#define QSERDES_COM_CMN_STATUS			0x0140
 #define QSERDES_COM_CLK_SEL			0x0154
 #define QSERDES_COM_HSCLK_SEL			0x0158
 
@@ -99,6 +101,12 @@
 #define DP_VCO_RATE_9720MHZDIV1000		9720000UL
 #define DP_VCO_RATE_10800MHZDIV1000		10800000UL
 
+#define DP_7NM_C_READY		BIT(0)
+#define DP_7NM_FREQ_DONE	BIT(0)
+#define DP_7NM_PLL_LOCKED	BIT(1)
+#define DP_7NM_PHY_READY	BIT(1)
+#define DP_7NM_TSYNC_DONE	BIT(0)
+
 int dp_mux_set_parent_7nm(void *context, unsigned int reg, unsigned int val)
 {
 	struct mdss_pll_resources *dp_res = context;
@@ -191,7 +199,6 @@ static int dp_vco_pll_init_db_7nm(struct dp_pll_db_7nm *pdb,
 	pdb->integloop_gain1_mode0 = 0x00;
 	pdb->vco_tune_map = 0x00;
 	pdb->cmn_config = 0x02;
-	pdb->txn_tran_drv_emp_en = 0xf;
 
 	switch (rate) {
 	case DP_VCO_HSCLK_RATE_1620MHZDIV1000:
@@ -258,6 +265,8 @@ static int dp_config_vco_rate_7nm(struct dp_pll_vco_clk *vco,
 		return res;
 	}
 
+	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_CFG_1, 0x0F);
+
 	if (pdb->lane_cnt != 4) {
 		if (pdb->orientation == ORIENTATION_CC2)
 			MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_PD_CTL, 0x6d);
@@ -276,14 +285,20 @@ static int dp_config_vco_rate_7nm(struct dp_pll_vco_clk *vco,
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_CLK_ENABLE1, 0x0c);
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_SYSCLK_BUF_ENABLE, 0x06);
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_CLK_SEL, 0x30);
-	MDSS_PLL_REG_W(dp_res->pll_base,
-		QSERDES_COM_HSCLK_SEL, pdb->hsclk_sel);
+	/* Make sure the PHY register writes are done */
+	wmb();
+
+	/* PLL Optimization */
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_PLL_IVCO, 0x0f);
-	MDSS_PLL_REG_W(dp_res->pll_base,
-		QSERDES_COM_LOCK_CMP_EN, pdb->lock_cmp_en);
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_PLL_CCTRL_MODE0, 0x36);
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_PLL_RCTRL_MODE0, 0x16);
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_CP_CTRL_MODE0, 0x06);
+	/* Make sure the PHY register writes are done */
+	wmb();
+
+	/* link rate dependent params */
+	MDSS_PLL_REG_W(dp_res->pll_base,
+		QSERDES_COM_HSCLK_SEL, pdb->hsclk_sel);
 	MDSS_PLL_REG_W(dp_res->pll_base,
 		QSERDES_COM_DEC_START_MODE0, pdb->dec_start_mode0);
 	MDSS_PLL_REG_W(dp_res->pll_base,
@@ -292,21 +307,25 @@ static int dp_config_vco_rate_7nm(struct dp_pll_vco_clk *vco,
 		QSERDES_COM_DIV_FRAC_START2_MODE0, pdb->div_frac_start2_mode0);
 	MDSS_PLL_REG_W(dp_res->pll_base,
 		QSERDES_COM_DIV_FRAC_START3_MODE0, pdb->div_frac_start3_mode0);
-	MDSS_PLL_REG_W(dp_res->pll_base,
-		QSERDES_COM_CMN_CONFIG, pdb->cmn_config);
-	MDSS_PLL_REG_W(dp_res->pll_base,
-		QSERDES_COM_INTEGLOOP_GAIN0_MODE0, pdb->integloop_gain0_mode0);
-	MDSS_PLL_REG_W(dp_res->pll_base,
-		QSERDES_COM_INTEGLOOP_GAIN1_MODE0, pdb->integloop_gain1_mode0);
-	MDSS_PLL_REG_W(dp_res->pll_base,
-		QSERDES_COM_VCO_TUNE_MAP, pdb->vco_tune_map);
 	MDSS_PLL_REG_W(dp_res->pll_base,
 		QSERDES_COM_LOCK_CMP1_MODE0, pdb->lock_cmp1_mode0);
 	MDSS_PLL_REG_W(dp_res->pll_base,
 		QSERDES_COM_LOCK_CMP2_MODE0, pdb->lock_cmp2_mode0);
+	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_LOCK_CMP_EN,
+		pdb->lock_cmp_en);
+	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_VCO_DIV, pdb->phy_vco_div);
 	/* Make sure the PLL register writes are done */
 	wmb();
 
+	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_CMN_CONFIG, 0x02);
+	MDSS_PLL_REG_W(dp_res->pll_base,
+		QSERDES_COM_INTEGLOOP_GAIN0_MODE0, 0x3f);
+	MDSS_PLL_REG_W(dp_res->pll_base,
+		QSERDES_COM_INTEGLOOP_GAIN1_MODE0, 0x00);
+	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_VCO_TUNE_MAP, 0x00);
+	/* Make sure the PHY register writes are done */
+	wmb();
+
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_BG_TIMER, 0x0a);
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_CORECLK_DIV_MODE0, 0x0a);
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_VCO_TUNE_CTRL, 0x00);
@@ -319,85 +338,123 @@ static int dp_config_vco_rate_7nm(struct dp_pll_vco_clk *vco,
 		MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_MODE, 0x4c);
 	else
 		MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_MODE, 0x5c);
+
+	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_AUX_CFG1, 0x13);
+	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_AUX_CFG2, 0xA4);
 	/* Make sure the PLL register writes are done */
 	wmb();
 
-	/* TX Lane configuration */
-	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_TX0_TX1_LANE_CTL, 0x05);
-	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_TX2_TX3_LANE_CTL, 0x05);
-
 	/* TX-0 register configuration */
-	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_TRANSCEIVER_BIAS_EN, 0x1a);
+	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_TX0_TX1_LANE_CTL, 0x05);
 	MDSS_PLL_REG_W(dp_res->ln_tx0_vmode_base, DP_VMODE_CTRL1, 0x40);
 	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_PRE_STALL_LDO_BOOST_EN, 0x30);
 	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_INTERFACE_SELECT, 0x3b);
 	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_CLKBUF_ENABLE, 0x0f);
 	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_RESET_TSYNC_EN, 0x03);
-	MDSS_PLL_REG_W(dp_res->ln_tx0_tran_base, DP_TRAN_DRVR_EMP_EN,
-		pdb->txn_tran_drv_emp_en);
+	MDSS_PLL_REG_W(dp_res->ln_tx0_tran_base, DP_TRAN_DRVR_EMP_EN, 0xf);
 	MDSS_PLL_REG_W(dp_res->ln_tx0_base,
 		TXn_PARRATE_REC_DETECT_IDLE_EN, 0x00);
 	MDSS_PLL_REG_W(dp_res->ln_tx0_tran_base, DP_TX_INTERFACE_MODE, 0x00);
-	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_TX_BAND, 0x4);
+	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_RES_CODE_LANE_OFFSET_TX, 0x11);
+	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_RES_CODE_LANE_OFFSET_RX, 0x11);
+	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_TX_BAND, 0x04);
 
 	/* TX-1 register configuration */
-	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_TRANSCEIVER_BIAS_EN, 0x1a);
+	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_TX2_TX3_LANE_CTL, 0x05);
 	MDSS_PLL_REG_W(dp_res->ln_tx1_vmode_base, DP_VMODE_CTRL1, 0x40);
 	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_PRE_STALL_LDO_BOOST_EN, 0x30);
 	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_INTERFACE_SELECT, 0x3b);
 	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_CLKBUF_ENABLE, 0x0f);
 	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_RESET_TSYNC_EN, 0x03);
-	MDSS_PLL_REG_W(dp_res->ln_tx1_tran_base, DP_TRAN_DRVR_EMP_EN,
-		pdb->txn_tran_drv_emp_en);
+	MDSS_PLL_REG_W(dp_res->ln_tx1_tran_base, DP_TRAN_DRVR_EMP_EN, 0xf);
 	MDSS_PLL_REG_W(dp_res->ln_tx1_base,
 		TXn_PARRATE_REC_DETECT_IDLE_EN, 0x00);
 	MDSS_PLL_REG_W(dp_res->ln_tx1_tran_base, DP_TX_INTERFACE_MODE, 0x00);
-	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_TX_BAND, 0x4);
+	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_RES_CODE_LANE_OFFSET_TX, 0x11);
+	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_RES_CODE_LANE_OFFSET_RX, 0x11);
+	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_TX_BAND, 0x04);
 	/* Make sure the PHY register writes are done */
 	wmb();
 
-	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_VCO_DIV, pdb->phy_vco_div);
-
 	return res;
 }
 
-static bool dp_7nm_pll_lock_status(struct mdss_pll_resources *dp_res)
-{
-	u32 status;
-	bool pll_locked;
+enum dp_7nm_pll_status {
+	C_READY,
+	FREQ_DONE,
+	PLL_LOCKED,
+	PHY_READY,
+	TSYNC_DONE,
+};
 
-	if (readl_poll_timeout_atomic((dp_res->pll_base +
-			QSERDES_COM_C_READY_STATUS),
-			status,
-			((status & BIT(0)) > 0),
-			DP_PHY_PLL_POLL_SLEEP_US,
-			DP_PHY_PLL_POLL_TIMEOUT_US)) {
-		pr_err("C_READY status is not high. Status=%x\n", status);
-		pll_locked = false;
-	} else {
-		pll_locked = true;
+char *dp_7nm_pll_get_status_name(enum dp_7nm_pll_status status)
+{
+	switch (status) {
+	case C_READY:
+		return "C_READY";
+	case FREQ_DONE:
+		return "FREQ_DONE";
+	case PLL_LOCKED:
+		return "PLL_LOCKED";
+	case PHY_READY:
+		return "PHY_READY";
+	case TSYNC_DONE:
+		return "TSYNC_DONE";
+	default:
+		return "unknown";
 	}
 
-	return pll_locked;
+
 }
 
-static bool dp_7nm_phy_rdy_status(struct mdss_pll_resources *dp_res)
+static bool dp_7nm_pll_get_status(struct mdss_pll_resources *dp_res,
+		enum dp_7nm_pll_status status)
 {
-	u32 status;
-	bool phy_ready = true;
-
-	/* poll for PHY ready status */
-	if (readl_poll_timeout_atomic((dp_res->phy_base +
-			DP_PHY_STATUS),
-			status,
-			((status & (BIT(1))) > 0),
+	u32 reg, state, bit;
+	void __iomem *base;
+	bool success = true;
+
+	switch (status) {
+	case C_READY:
+		base = dp_res->pll_base;
+		reg = QSERDES_COM_C_READY_STATUS;
+		bit = DP_7NM_C_READY;
+		break;
+	case FREQ_DONE:
+		base = dp_res->pll_base;
+		reg = QSERDES_COM_CMN_STATUS;
+		bit = DP_7NM_FREQ_DONE;
+		break;
+	case PLL_LOCKED:
+		base = dp_res->pll_base;
+		reg = QSERDES_COM_CMN_STATUS;
+		bit = DP_7NM_PLL_LOCKED;
+		break;
+	case PHY_READY:
+		base = dp_res->phy_base;
+		reg = DP_PHY_STATUS;
+		bit = DP_7NM_PHY_READY;
+		break;
+	case TSYNC_DONE:
+		base = dp_res->phy_base;
+		reg = DP_PHY_STATUS;
+		bit = DP_7NM_TSYNC_DONE;
+		break;
+	default:
+		return false;
+	}
+
+	if (readl_poll_timeout_atomic((base + reg), state,
+			((state & bit) > 0),
 			DP_PHY_PLL_POLL_SLEEP_US,
 			DP_PHY_PLL_POLL_TIMEOUT_US)) {
-		pr_err("Phy_ready is not high. Status=%x\n", status);
-		phy_ready = false;
+		pr_err("%s failed, status=%x\n",
+			dp_7nm_pll_get_status_name(status), state);
+
+		success = false;
 	}
 
-	return phy_ready;
+	return success;
 }
 
 static int dp_pll_enable_7nm(struct clk_hw *hw)
@@ -405,97 +462,44 @@ static int dp_pll_enable_7nm(struct clk_hw *hw)
 	int rc = 0;
 	struct dp_pll_vco_clk *vco = to_dp_vco_hw(hw);
 	struct mdss_pll_resources *dp_res = vco->priv;
-	struct dp_pll_db_7nm *pdb = (struct dp_pll_db_7nm *)dp_res->priv;
-	u32 bias_en, drvr_en;
 
-	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_AUX_CFG1, 0x13);
-	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_AUX_CFG2, 0xA4);
 	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_CFG, 0x01);
 	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_CFG, 0x05);
 	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_CFG, 0x01);
 	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_CFG, 0x09);
-	wmb(); /* Make sure the PHY register writes are done */
-
 	MDSS_PLL_REG_W(dp_res->pll_base, QSERDES_COM_RESETSM_CNTRL, 0x20);
 	wmb();	/* Make sure the PLL register writes are done */
 
-	if (!dp_7nm_pll_lock_status(dp_res)) {
+	if (!dp_7nm_pll_get_status(dp_res, C_READY)) {
 		rc = -EINVAL;
 		goto lock_err;
 	}
 
-	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_CFG, 0x19);
-	/* Make sure the PHY register writes are done */
-	wmb();
-	/* poll for PHY ready status */
-	if (!dp_7nm_phy_rdy_status(dp_res)) {
+	if (!dp_7nm_pll_get_status(dp_res, FREQ_DONE)) {
 		rc = -EINVAL;
 		goto lock_err;
 	}
 
-	pr_debug("PLL is locked\n");
-
-	if (pdb->lane_cnt == 1) {
-		bias_en = 0x3e;
-		drvr_en = 0x13;
-	} else {
-		bias_en = 0x3f;
-		drvr_en = 0x10;
-	}
-
-	if (pdb->lane_cnt != 4) {
-		if (pdb->orientation == ORIENTATION_CC1) {
-			MDSS_PLL_REG_W(dp_res->ln_tx1_base,
-				TXn_HIGHZ_DRVR_EN, drvr_en);
-			MDSS_PLL_REG_W(dp_res->ln_tx1_base,
-				TXn_TRANSCEIVER_BIAS_EN, bias_en);
-		} else {
-			MDSS_PLL_REG_W(dp_res->ln_tx0_base,
-				TXn_HIGHZ_DRVR_EN, drvr_en);
-			MDSS_PLL_REG_W(dp_res->ln_tx0_base,
-				TXn_TRANSCEIVER_BIAS_EN, bias_en);
-		}
-	} else {
-		MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_HIGHZ_DRVR_EN, drvr_en);
-		MDSS_PLL_REG_W(dp_res->ln_tx0_base,
-			TXn_TRANSCEIVER_BIAS_EN, bias_en);
-		MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_HIGHZ_DRVR_EN, drvr_en);
-		MDSS_PLL_REG_W(dp_res->ln_tx1_base,
-			TXn_TRANSCEIVER_BIAS_EN, bias_en);
+	if (!dp_7nm_pll_get_status(dp_res, PLL_LOCKED)) {
+		rc = -EINVAL;
+		goto lock_err;
 	}
 
-	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_TX_POL_INV, 0x0a);
-	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_TX_POL_INV, 0x0a);
-	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_CFG, 0x18);
-	udelay(2000);
-
 	MDSS_PLL_REG_W(dp_res->phy_base, DP_PHY_CFG, 0x19);
-
-	/*
-	 * Make sure all the register writes are completed before
-	 * doing any other operation
-	 */
+	/* Make sure the PHY register writes are done */
 	wmb();
 
-	/* poll for PHY ready status */
-	if (!dp_7nm_phy_rdy_status(dp_res)) {
+	if (!dp_7nm_pll_get_status(dp_res, TSYNC_DONE)) {
 		rc = -EINVAL;
 		goto lock_err;
 	}
 
-	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_TX_DRV_LVL, 0x3f);
-	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_TX_DRV_LVL, 0x3f);
-	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_TX_EMP_POST1_LVL, 0x23);
-	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_TX_EMP_POST1_LVL, 0x23);
-	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_RES_CODE_LANE_OFFSET_TX, 0x11);
-	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_RES_CODE_LANE_OFFSET_TX, 0x11);
-	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_RES_CODE_LANE_OFFSET_RX, 0x11);
-	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_RES_CODE_LANE_OFFSET_RX, 0x11);
-	MDSS_PLL_REG_W(dp_res->ln_tx0_base, TXn_INTERFACE_SELECT, 0x3b);
-	MDSS_PLL_REG_W(dp_res->ln_tx1_base, TXn_INTERFACE_SELECT, 0x3b);
-	/* Make sure the PHY register writes are done */
-	wmb();
+	if (!dp_7nm_pll_get_status(dp_res, PHY_READY)) {
+		rc = -EINVAL;
+		goto lock_err;
+	}
 
+	pr_debug("PLL is locked\n");
 lock_err:
 	return rc;
 }