tegra124: add support for full DP link training

The original dp driver supports only fast link training and a
special drive setting is used for the link training sequence.
This might not be accepted by all panels. The better way is to
go through full link training sequence to negotiate for a best
drive setting.

With the change, dp driver will try fast link training first,
this is same as before. If it fails in fast link training, will
try full link training.

BUG=chrome-os-partner:32129
TEST=all panels on blaze/big devices work fine.

Original-Change-Id: I6f3402c4c5993a156c965c7f52b011d336a2946f
Original-Signed-off-by: Neil Chen <neilc@nvidia.com>
Original-Reviewed-on: https://chromium-review.googlesource.com/219543
Original-Reviewed-by: Jimmy Zhang <jimmzhang@nvidia.com>
Original-Reviewed-by: Julius Werner <jwerner@chromium.org>

(cherry picked from commit 24966517d41252384af3c2784def36aebad42434)
Signed-off-by: Aaron Durbin <adurbin@chromium.org>

Change-Id: I3e7e7e749e5c8a9f07ac6132859fcad6fc96c39c
Reviewed-on: http://review.coreboot.org/9247
Reviewed-by: Paul Menzel <paulepanter@users.sourceforge.net>
Tested-by: build bot (Jenkins)
Reviewed-by: David Hendricks <dhendrix@chromium.org>

3 files changed, 492 insertions, 44 deletions

diff --git a/src/soc/nvidia/tegra124/dp.c b/src/soc/nvidia/tegra124/dp.c
index 00474a882a..9ff50ead8d 100644
--- a/src/soc/nvidia/tegra124/dp.c
+++ b/src/soc/nvidia/tegra124/dp.c
@@ -30,6 +30,11 @@
 #include "sor.h"
 #include <soc/nvidia/tegra/displayport.h>
 
+enum {
+	DP_LT_SUCCESS = 0,
+	DP_LT_FAILED = -1,
+};
+
 struct tegra_dc_dp_data dp_data;
 
 static inline u32 tegra_dpaux_readl(struct tegra_dc_dp_data *dp, u32 reg)
@@ -443,6 +448,34 @@ static void tegra_dc_dp_dump_link_cfg(struct tegra_dc_dp_data *dp,
 		link_cfg->vblank_sym);
 }
 
+static int _tegra_dp_lower_link_config(struct tegra_dc_dp_data *dp,
+	struct tegra_dc_dp_link_config *cfg)
+{
+
+	switch (cfg->link_bw){
+	case SOR_LINK_SPEED_G1_62:
+		if (cfg->max_link_bw > SOR_LINK_SPEED_G1_62)
+			cfg->link_bw = SOR_LINK_SPEED_G2_7;
+		cfg->lane_count /= 2;
+		break;
+	case SOR_LINK_SPEED_G2_7:
+		cfg->link_bw = SOR_LINK_SPEED_G1_62;
+		break;
+	case SOR_LINK_SPEED_G5_4:
+		if (cfg->lane_count == 1) {
+			cfg->link_bw = SOR_LINK_SPEED_G2_7;
+			cfg->lane_count = cfg->max_lane_count;
+		} else
+			cfg->lane_count /= 2;
+		break;
+	default:
+		printk(BIOS_ERR,"dp: Error link rate %d\n", cfg->link_bw);
+		return DP_LT_FAILED;
+	}
+
+	return (cfg->lane_count > 0) ? DP_LT_SUCCESS : DP_LT_FAILED;
+}
+
 /* Calcuate if given cfg can meet the mode request. */
 /* Return true if mode is possible, false otherwise. */
 static int tegra_dc_dp_calc_config(struct tegra_dc_dp_data *dp,
@@ -624,6 +657,8 @@ static int tegra_dc_dp_init_max_link_cfg(
 	CHECK_RET(tegra_dc_dp_dpcd_read(dp, NV_DPCD_MAX_LANE_COUNT,
 			&dpcd_data));
 	link_cfg->max_lane_count = dpcd_data & NV_DPCD_MAX_LANE_COUNT_MASK;
+	link_cfg->tps3_supported = (dpcd_data &
+		NV_DPCD_MAX_LANE_COUNT_TPS3_SUPPORTED_YES) ? 1 : 0;
 
 	link_cfg->support_enhanced_framing =
 		(dpcd_data & NV_DPCD_MAX_LANE_COUNT_ENHANCED_FRAMING_YES) ?
@@ -634,6 +669,9 @@ static int tegra_dc_dp_init_max_link_cfg(
 	link_cfg->downspread = (dpcd_data & NV_DPCD_MAX_DOWNSPREAD_VAL_0_5_PCT)?
 				1 : 0;
 
+	CHECK_RET(tegra_dc_dp_dpcd_read(dp, NV_DPCD_TRAINING_AUX_RD_INTERVAL,
+			&link_cfg->aux_rd_interval));
+
 	CHECK_RET(tegra_dc_dp_dpcd_read(dp, NV_DPCD_MAX_LINK_BANDWIDTH,
 			&link_cfg->max_link_bw));
 
@@ -732,6 +770,392 @@ static int tegra_dc_dp_link_trained(struct tegra_dc_dp_data *dp,
 	return 0;
 }
 
+static int tegra_dp_channel_eq_status(struct tegra_dc_dp_data *dp)
+{
+	u32 cnt;
+	u32 n_lanes = dp->link_cfg.lane_count;
+	u8 data;
+	u8 ce_done = 1;
+
+	for (cnt = 0; cnt < n_lanes / 2; cnt++) {
+		tegra_dc_dp_dpcd_read(dp, (NV_DPCD_LANE0_1_STATUS + cnt), &data);
+
+		if (n_lanes == 1) {
+			ce_done = (data &
+					(0x1 << NV_DPCD_STATUS_LANEX_CHN_EQ_DONE_SHIFT)) &&
+			(data &	(0x1 << NV_DPCD_STATUS_LANEX_SYMBOL_LOCKED_SHFIT));
+			break;
+		} else if (!(data & (0x1 << NV_DPCD_STATUS_LANEX_CHN_EQ_DONE_SHIFT)) ||
+			!(data & (0x1 << NV_DPCD_STATUS_LANEX_SYMBOL_LOCKED_SHFIT)) ||
+			!(data & (0x1 << NV_DPCD_STATUS_LANEXPLUS1_CHN_EQ_DONE_SHIFT)) ||
+			!(data & (0x1 << NV_DPCD_STATUS_LANEXPLUS1_SYMBOL_LOCKED_SHIFT)))
+			return 0;
+	}
+
+	if (ce_done) {
+		tegra_dc_dp_dpcd_read(dp, NV_DPCD_LANE_ALIGN_STATUS_UPDATED, &data);
+		if (!(data & NV_DPCD_LANE_ALIGN_STATUS_UPDATED_DONE_YES))
+				ce_done = 0;
+	}
+
+	return ce_done;
+}
+
+static u8 tegra_dp_clock_recovery_status(struct tegra_dc_dp_data *dp)
+{
+	u32 cnt;
+	u32 n_lanes = dp->link_cfg.lane_count;
+	u8 data_ptr;
+
+	for (cnt = 0; cnt < n_lanes / 2; cnt++) {
+		tegra_dc_dp_dpcd_read(dp,
+			(NV_DPCD_LANE0_1_STATUS + cnt), &data_ptr);
+
+		if (n_lanes == 1)
+			return (data_ptr & NV_DPCD_STATUS_LANEX_CR_DONE_YES) ? 1 : 0;
+		else if (!(data_ptr & NV_DPCD_STATUS_LANEX_CR_DONE_YES) ||
+			!(data_ptr &
+			(NV_DPCD_STATUS_LANEXPLUS1_CR_DONE_YES)))
+			return 0;
+	}
+
+	return 1;
+}
+
+static void tegra_dp_lt_adjust(struct tegra_dc_dp_data *dp,
+				u32 pe[4], u32 vs[4], u32 pc[4],
+				u8 pc_supported)
+{
+	size_t cnt;
+	u8 data_ptr;
+	u32 n_lanes = dp->link_cfg.lane_count;
+
+	for (cnt = 0; cnt < n_lanes / 2; cnt++) {
+		tegra_dc_dp_dpcd_read(dp,
+			(NV_DPCD_LANE0_1_ADJUST_REQ + cnt), &data_ptr);
+		pe[2 * cnt] = (data_ptr & NV_DPCD_ADJUST_REQ_LANEX_PE_MASK) >>
+					NV_DPCD_ADJUST_REQ_LANEX_PE_SHIFT;
+		vs[2 * cnt] = (data_ptr & NV_DPCD_ADJUST_REQ_LANEX_DC_MASK) >>
+					NV_DPCD_ADJUST_REQ_LANEX_DC_SHIFT;
+		pe[1 + 2 * cnt] =
+			(data_ptr & NV_DPCD_ADJUST_REQ_LANEXPLUS1_PE_MASK) >>
+					NV_DPCD_ADJUST_REQ_LANEXPLUS1_PE_SHIFT;
+		vs[1 + 2 * cnt] =
+			(data_ptr & NV_DPCD_ADJUST_REQ_LANEXPLUS1_DC_MASK) >>
+					NV_DPCD_ADJUST_REQ_LANEXPLUS1_DC_SHIFT;
+	}
+	if (pc_supported) {
+		tegra_dc_dp_dpcd_read(dp,
+				NV_DPCD_ADJUST_REQ_POST_CURSOR2, &data_ptr);
+		for (cnt = 0; cnt < n_lanes; cnt++) {
+			pc[cnt] = (data_ptr >>
+			NV_DPCD_ADJUST_REQ_POST_CURSOR2_LANE_SHIFT(cnt)) &
+			NV_DPCD_ADJUST_REQ_POST_CURSOR2_LANE_MASK;
+		}
+	}
+}
+
+static inline u32 tegra_dp_wait_aux_training(struct tegra_dc_dp_data *dp,
+							u8 is_clk_recovery)
+{
+	if (!dp->link_cfg.aux_rd_interval)
+		is_clk_recovery ? udelay(200) :
+					udelay(500);
+	else
+		mdelay(dp->link_cfg.aux_rd_interval * 4);
+
+	return dp->link_cfg.aux_rd_interval;
+}
+
+static void tegra_dp_tpg(struct tegra_dc_dp_data *dp, u32 tp, u32 n_lanes)
+{
+	u8 data = (tp == training_pattern_disabled)
+		? (tp | NV_DPCD_TRAINING_PATTERN_SET_SC_DISABLED_F)
+		: (tp | NV_DPCD_TRAINING_PATTERN_SET_SC_DISABLED_T);
+
+	tegra_dc_sor_set_dp_linkctl(&dp->sor, 1, tp, &dp->link_cfg);
+	tegra_dc_dp_dpcd_write(dp, NV_DPCD_TRAINING_PATTERN_SET, data);
+}
+
+static int tegra_dp_link_config(struct tegra_dc_dp_data *dp,
+	const struct tegra_dc_dp_link_config *link_cfg)
+{
+	u8	dpcd_data;
+	u32	retry;
+
+	if (link_cfg->lane_count == 0) {
+		printk(BIOS_ERR, "dp: error: lane count is 0. "
+				"Can not set link config.\n");
+		return DP_LT_FAILED;
+	}
+
+	/* Set power state if it is not in normal level */
+	if(tegra_dc_dp_dpcd_read(dp, NV_DPCD_SET_POWER, &dpcd_data))
+		return DP_LT_FAILED;
+
+	if (dpcd_data == NV_DPCD_SET_POWER_VAL_D3_PWRDWN) {
+		dpcd_data = NV_DPCD_SET_POWER_VAL_D0_NORMAL;
+
+		/* DP spec requires 3 retries */
+		for (retry = 3; retry > 0; --retry){
+			if (tegra_dc_dp_dpcd_write(dp, NV_DPCD_SET_POWER, dpcd_data))
+				break;
+			if (retry == 1){
+				printk(BIOS_ERR, "dp: Failed to set DP panel power\n");
+				return DP_LT_FAILED;
+			}
+		}
+	}
+
+	/* Enable ASSR if possible */
+	if (link_cfg->alt_scramber_reset_cap)
+		if(tegra_dc_dp_set_assr(dp, 1))
+			return DP_LT_FAILED;
+
+	if (tegra_dp_set_link_bandwidth(dp, link_cfg->link_bw)) {
+		printk(BIOS_ERR, "dp: Failed to set link bandwidth\n");
+		return DP_LT_FAILED;
+	}
+	if (tegra_dp_set_lane_count(dp, link_cfg)) {
+		printk(BIOS_ERR, "dp: Failed to set lane count\n");
+		return DP_LT_FAILED;
+	}
+	tegra_dc_sor_set_dp_linkctl(&dp->sor, 1, training_pattern_none,
+					link_cfg);
+	return DP_LT_SUCCESS;
+}
+
+static int tegra_dp_lower_link_config(struct tegra_dc_dp_data *dp,
+	struct tegra_dc_dp_link_config *cfg)
+{
+	struct tegra_dc_dp_link_config tmp_cfg;
+
+	tmp_cfg = dp->link_cfg;
+	cfg->is_valid = 0;
+
+	if (_tegra_dp_lower_link_config(dp, cfg))
+		goto fail;
+
+	if (tegra_dc_dp_calc_config(dp, dp->dc->config, cfg))
+		goto fail;
+	tegra_dp_link_config(dp, cfg);
+
+	return DP_LT_SUCCESS;
+fail:
+	dp->link_cfg = tmp_cfg;
+	tegra_dp_link_config(dp, &tmp_cfg);
+	return DP_LT_FAILED;
+}
+
+static void tegra_dp_lt_config(struct tegra_dc_dp_data *dp,
+				u32 pe[4], u32 vs[4], u32 pc[4])
+{
+	struct tegra_dc_sor_data *sor = &dp->sor;
+	u32 n_lanes = dp->link_cfg.lane_count;
+	u8 pc_supported = dp->link_cfg.tps3_supported;
+	u32 cnt;
+	u32 val;
+
+	for (cnt = 0; cnt < n_lanes; cnt++) {
+		u32 mask = 0;
+		u32 pe_reg, vs_reg, pc_reg;
+		u32 shift = 0;
+
+		switch (cnt) {
+		case 0:
+			mask = NV_SOR_PR_LANE2_DP_LANE0_MASK;
+			shift = NV_SOR_PR_LANE2_DP_LANE0_SHIFT;
+			break;
+		case 1:
+			mask = NV_SOR_PR_LANE1_DP_LANE1_MASK;
+			shift = NV_SOR_PR_LANE1_DP_LANE1_SHIFT;
+			break;
+		case 2:
+			mask = NV_SOR_PR_LANE0_DP_LANE2_MASK;
+			shift = NV_SOR_PR_LANE0_DP_LANE2_SHIFT;
+			break;
+		case 3:
+			mask = NV_SOR_PR_LANE3_DP_LANE3_MASK;
+			shift = NV_SOR_PR_LANE3_DP_LANE3_SHIFT;
+			break;
+		default:
+			printk(BIOS_ERR,
+				"dp: incorrect lane cnt\n");
+		}
+
+		pe_reg = tegra_dp_pe_regs[pc[cnt]][vs[cnt]][pe[cnt]];
+		vs_reg = tegra_dp_vs_regs[pc[cnt]][vs[cnt]][pe[cnt]];
+		pc_reg = tegra_dp_pc_regs[pc[cnt]][vs[cnt]][pe[cnt]];
+
+		tegra_dp_set_pe_vs_pc(sor, mask, pe_reg << shift,
+				vs_reg << shift, pc_reg << shift, pc_supported);
+	}
+
+	tegra_dp_disable_tx_pu(&dp->sor);
+	udelay(20);
+
+	for (cnt = 0; cnt < n_lanes; cnt++) {
+		u32 max_vs_flag = tegra_dp_is_max_vs(pe[cnt], vs[cnt]);
+		u32 max_pe_flag = tegra_dp_is_max_pe(pe[cnt], vs[cnt]);
+
+		val = (vs[cnt] << NV_DPCD_TRAINING_LANEX_SET_DC_SHIFT) |
+			(max_vs_flag ?
+			NV_DPCD_TRAINING_LANEX_SET_DC_MAX_REACHED_T :
+			NV_DPCD_TRAINING_LANEX_SET_DC_MAX_REACHED_F) |
+			(pe[cnt] << NV_DPCD_TRAINING_LANEX_SET_PE_SHIFT) |
+			(max_pe_flag ?
+			NV_DPCD_TRAINING_LANEX_SET_PE_MAX_REACHED_T :
+			NV_DPCD_TRAINING_LANEX_SET_PE_MAX_REACHED_F);
+		tegra_dc_dp_dpcd_write(dp,
+			(NV_DPCD_TRAINING_LANE0_SET + cnt), val);
+	}
+
+	if (pc_supported) {
+		for (cnt = 0; cnt < n_lanes / 2; cnt++) {
+			u32 max_pc_flag0 = tegra_dp_is_max_pc(pc[cnt]);
+			u32 max_pc_flag1 = tegra_dp_is_max_pc(pc[cnt + 1]);
+			val = (pc[cnt] << NV_DPCD_LANEX_SET2_PC2_SHIFT) |
+				(max_pc_flag0 ?
+				NV_DPCD_LANEX_SET2_PC2_MAX_REACHED_T :
+				NV_DPCD_LANEX_SET2_PC2_MAX_REACHED_F) |
+				(pc[cnt + 1] <<
+				NV_DPCD_LANEXPLUS1_SET2_PC2_SHIFT) |
+				(max_pc_flag1 ?
+				NV_DPCD_LANEXPLUS1_SET2_PC2_MAX_REACHED_T :
+				NV_DPCD_LANEXPLUS1_SET2_PC2_MAX_REACHED_F);
+			tegra_dc_dp_dpcd_write(dp,
+				(NV_DPCD_TRAINING_LANE0_1_SET2 + cnt), val);
+		}
+	}
+}
+
+static int _tegra_dp_channel_eq(struct tegra_dc_dp_data *dp, u32 pe[4],
+				u32 vs[4], u32 pc[4], u8 pc_supported,
+				u32 n_lanes)
+{
+	u32 retry_cnt;
+
+	for (retry_cnt = 0; retry_cnt < 4; retry_cnt++) {
+		if (retry_cnt){
+			tegra_dp_lt_adjust(dp, pe, vs, pc, pc_supported);
+			tegra_dp_lt_config(dp, pe, vs, pc);
+		}
+
+		tegra_dp_wait_aux_training(dp, 0);
+
+		if (!tegra_dp_clock_recovery_status(dp)) {
+			printk(BIOS_ERR,"dp: CR failed in channel EQ sequence!\n");
+			break;
+		}
+
+		if (tegra_dp_channel_eq_status(dp))
+			return DP_LT_SUCCESS;
+	}
+
+	return DP_LT_FAILED;
+}
+
+static int tegra_dp_channel_eq(struct tegra_dc_dp_data *dp,
+					u32 pe[4], u32 vs[4], u32 pc[4])
+{
+	u32 n_lanes = dp->link_cfg.lane_count;
+	u8 pc_supported = dp->link_cfg.tps3_supported;
+	int err;
+	u32 tp_src = training_pattern_2;
+
+	if (pc_supported)
+		tp_src = training_pattern_3;
+
+	tegra_dp_tpg(dp, tp_src, n_lanes);
+
+	err = _tegra_dp_channel_eq(dp, pe, vs, pc, pc_supported, n_lanes);
+
+	tegra_dp_tpg(dp, training_pattern_disabled, n_lanes);
+
+	return err;
+}
+
+static int _tegra_dp_clk_recovery(struct tegra_dc_dp_data *dp, u32 pe[4],
+					u32 vs[4], u32 pc[4], u8 pc_supported,
+					u32 n_lanes)
+{
+	u32 vs_temp[4];
+	u32 retry_cnt = 0;
+
+	do {
+		tegra_dp_lt_config(dp, pe, vs, pc);
+		tegra_dp_wait_aux_training(dp, 1);
+
+		if (tegra_dp_clock_recovery_status(dp))
+			return DP_LT_SUCCESS;
+
+		memcpy(vs_temp, vs, sizeof(vs_temp));
+		tegra_dp_lt_adjust(dp, pe, vs, pc, pc_supported);
+
+		if (memcmp(vs_temp, vs, sizeof(vs_temp)))
+			retry_cnt = 0;
+		else
+			++retry_cnt;
+	} while (retry_cnt < 5);
+
+	return DP_LT_FAILED;
+}
+
+static int tegra_dp_clk_recovery(struct tegra_dc_dp_data *dp,
+					u32 pe[4], u32 vs[4], u32 pc[4])
+{
+	u32 n_lanes = dp->link_cfg.lane_count;
+	u8 pc_supported = dp->link_cfg.tps3_supported;
+	int err;
+
+	tegra_dp_tpg(dp, training_pattern_1, n_lanes);
+
+	err = _tegra_dp_clk_recovery(dp, pe, vs, pc, pc_supported, n_lanes);
+	if (err < 0)
+		tegra_dp_tpg(dp, training_pattern_disabled, n_lanes);
+
+	return err;
+}
+
+static int tegra_dc_dp_full_link_training(struct tegra_dc_dp_data *dp)
+{
+	struct tegra_dc_sor_data *sor = &dp->sor;
+	int err;
+	u32 pe[4], vs[4], pc[4];
+
+	tegra_sor_precharge_lanes(sor);
+
+retry_cr:
+	memset(pe, preEmphasis_Disabled, sizeof(pe));
+	memset(vs, driveCurrent_Level0, sizeof(vs));
+	memset(pc, postCursor2_Level0, sizeof(pc));
+
+	err = tegra_dp_clk_recovery(dp, pe, vs, pc);
+	if (err != DP_LT_SUCCESS) {
+		if (!tegra_dp_lower_link_config(dp, &dp->link_cfg))
+			goto retry_cr;
+
+		printk(BIOS_ERR, "dp: clk recovery failed\n");
+		goto fail;
+	}
+
+	err = tegra_dp_channel_eq(dp, pe, vs, pc);
+	if (err != DP_LT_SUCCESS) {
+		if (!tegra_dp_lower_link_config(dp, &dp->link_cfg))
+			goto retry_cr;
+
+		printk(BIOS_ERR,
+			"dp: channel equalization failed\n");
+		goto fail;
+	}
+
+	tegra_dc_dp_dump_link_cfg(dp, &dp->link_cfg);
+
+	return 0;
+
+fail:
+	return err;
+}
 /*
  * All link training functions are ported from kernel dc driver.
  * See more details at drivers/video/tegra/dc/dp.c
@@ -813,59 +1237,23 @@ static int tegra_dc_dp_fast_link_training(struct tegra_dc_dp_data *dp,
 	return 0;
 }
 
-static int tegra_dp_link_config(struct tegra_dc_dp_data *dp,
+static int tegra_dp_do_link_training(struct tegra_dc_dp_data *dp,
 	const struct tegra_dc_dp_link_config *link_cfg)
 {
-	u8	dpcd_data;
 	u8	link_bw;
 	u8	lane_count;
-	u32	retry;
 	int	ret;
 
-	if (link_cfg->lane_count == 0) {
-		printk(BIOS_ERR, "dp: error: lane count is 0. "
-				"Can not set link config.\n");
-		return -1;
-	}
-
-	/* Set power state if it is not in normal level */
-	CHECK_RET(tegra_dc_dp_dpcd_read(dp, NV_DPCD_SET_POWER, &dpcd_data));
-	if (dpcd_data == NV_DPCD_SET_POWER_VAL_D3_PWRDWN) {
-		dpcd_data = NV_DPCD_SET_POWER_VAL_D0_NORMAL;
-		retry = 3;	/* DP spec requires 3 retries */
-		do {
-			ret = tegra_dc_dp_dpcd_write(dp,
-				NV_DPCD_SET_POWER, dpcd_data);
-		} while ((--retry > 0) && ret);
-		if (ret) {
-			printk(BIOS_ERR,
-				"dp: Failed to set DP panel power\n");
-			return ret;
-		}
-	}
-
-	/* Enable ASSR if possible */
-	if (link_cfg->alt_scramber_reset_cap)
-		CHECK_RET(tegra_dc_dp_set_assr(dp, 1));
-
-	ret = tegra_dp_set_link_bandwidth(dp, link_cfg->link_bw);
-	if (ret) {
-		printk(BIOS_ERR, "dp: Failed to set link bandwidth\n");
-		return ret;
-	}
-	ret = tegra_dp_set_lane_count(dp, link_cfg);
-	if (ret) {
-		printk(BIOS_ERR, "dp: Failed to set lane count\n");
-		return ret;
-	}
-	tegra_dc_sor_set_dp_linkctl(&dp->sor, 1, training_pattern_none,
-					link_cfg);
-
 	/* Now do the fast link training for eDP */
 	ret = tegra_dc_dp_fast_link_training(dp, link_cfg);
 	if (ret) {
 		printk(BIOS_ERR, "dp: fast link training failed\n");
-		return ret;
+
+		/* Try full link training then */
+			if (tegra_dc_dp_full_link_training(dp)){
+				printk(BIOS_ERR, "dp: full link training failed\n");
+				return ret;
+			}
 	}
 
 	/* Everything goes well, double check the link config */
@@ -907,7 +1295,8 @@ static int tegra_dc_dp_explore_link_cfg(struct tegra_dc_dp_data *dp,
 	 * set to max link config
 	 */
 	if ((!tegra_dc_dp_calc_config(dp, config, &temp_cfg)) &&
-		(!(tegra_dp_link_config(dp, &temp_cfg))))
+		(!tegra_dp_link_config(dp, &temp_cfg)) &&
+		(!tegra_dp_do_link_training(dp, &temp_cfg)))
 		/* the max link cfg is doable */
 		memcpy(link_cfg, &temp_cfg, sizeof(temp_cfg));
 
diff --git a/src/soc/nvidia/tegra124/sor.c b/src/soc/nvidia/tegra124/sor.c
index 2c059bf066..1f9df6ce06 100644
--- a/src/soc/nvidia/tegra124/sor.c
+++ b/src/soc/nvidia/tegra124/sor.c
@@ -79,6 +79,28 @@ static inline void tegra_sor_write_field(struct tegra_dc_sor_data *sor,
 	tegra_sor_writel(sor, reg, reg_val);
 }
 
+void tegra_dp_disable_tx_pu(struct tegra_dc_sor_data *sor)
+{
+	tegra_sor_write_field(sor,
+			NV_SOR_DP_PADCTL(sor->portnum),
+			NV_SOR_DP_PADCTL_TX_PU_MASK,
+			NV_SOR_DP_PADCTL_TX_PU_DISABLE);
+}
+
+void tegra_dp_set_pe_vs_pc(struct tegra_dc_sor_data *sor, u32 mask,
+				u32 pe_reg, u32 vs_reg, u32 pc_reg, u8 pc_supported)
+{
+	tegra_sor_write_field(sor, NV_SOR_PR(sor->portnum),
+					mask, pe_reg);
+	tegra_sor_write_field(sor, NV_SOR_DC(sor->portnum),
+					mask, vs_reg);
+	if (pc_supported) {
+		tegra_sor_write_field(
+				sor, NV_SOR_POSTCURSOR(sor->portnum),
+				mask, pc_reg);
+	}
+}
+
 static u32 tegra_dc_sor_poll_register(struct tegra_dc_sor_data *sor,
 	u32 reg, u32 mask, u32 exp_val, u32 poll_interval_us, u32 timeout_us)
 {
@@ -873,3 +895,33 @@ void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor)
 				drive_current);
 	tegra_sor_writel(sor, NV_SOR_PR(sor->portnum), pre_emphasis);
 }
+
+void tegra_sor_precharge_lanes(struct tegra_dc_sor_data *sor)
+{
+	const struct tegra_dc_dp_link_config *cfg = sor->link_cfg;
+	u32 val = 0;
+
+	switch (cfg->lane_count) {
+	case 4:
+		val |= (NV_SOR_DP_PADCTL_PD_TXD_3_NO |
+			NV_SOR_DP_PADCTL_PD_TXD_2_NO);
+		/* fall through */
+	case 2:
+		val |= NV_SOR_DP_PADCTL_PD_TXD_1_NO;
+		/* fall through */
+	case 1:
+		val |= NV_SOR_DP_PADCTL_PD_TXD_0_NO;
+		break;
+	default:
+		printk(BIOS_ERR,
+			"dp: invalid lane number %d\n", cfg->lane_count);
+		return;
+	}
+
+	tegra_sor_write_field(sor, NV_SOR_DP_PADCTL(sor->portnum),
+		(0xf << NV_SOR_DP_PADCTL_COMODE_TXD_0_DP_TXD_2_SHIFT),
+		(val << NV_SOR_DP_PADCTL_COMODE_TXD_0_DP_TXD_2_SHIFT));
+	udelay(100);
+	tegra_sor_write_field(sor, NV_SOR_DP_PADCTL(sor->portnum),
+		(0xf << NV_SOR_DP_PADCTL_COMODE_TXD_0_DP_TXD_2_SHIFT), 0);
+}
diff --git a/src/soc/nvidia/tegra124/sor.h b/src/soc/nvidia/tegra124/sor.h
index bf6286871e..4e4211e45e 100644
--- a/src/soc/nvidia/tegra124/sor.h
+++ b/src/soc/nvidia/tegra124/sor.h
@@ -695,6 +695,7 @@
 #define NV_SOR_DP_PADCTL_TX_PU_SHIFT				(22)
 #define NV_SOR_DP_PADCTL_TX_PU_DISABLE				(0 << 22)
 #define NV_SOR_DP_PADCTL_TX_PU_ENABLE				(1 << 22)
+#define NV_SOR_DP_PADCTL_TX_PU_MASK				(1 << 22)
 #define NV_SOR_DP_PADCTL_REG_CTRL_SHIFT				(20)
 #define NV_SOR_DP_PADCTL_REG_CTRL_DEFAULT_MASK			(0x3 << 20)
 #define NV_SOR_DP_PADCTL_VCMMODE_SHIFT				(16)
@@ -878,6 +879,8 @@ struct tegra_dc_dp_link_config {
 	u32	drive_current;
 	u32     preemphasis;
 	u32	postcursor;
+	u8	aux_rd_interval;
+	u8	tps3_supported;
 };
 
 /* TODO: just pull these up into one struct? Need to see how this impacts
@@ -917,4 +920,8 @@ void tegra_dc_sor_attach(struct tegra_dc_sor_data *sor);
 void tegra_dc_sor_set_lane_parm(struct tegra_dc_sor_data *sor,
 			const struct tegra_dc_dp_link_config *link_cfg);
 void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor);
+void tegra_sor_precharge_lanes(struct tegra_dc_sor_data *sor);
+void tegra_dp_disable_tx_pu(struct tegra_dc_sor_data *sor);
+void tegra_dp_set_pe_vs_pc(struct tegra_dc_sor_data *sor, u32 mask,
+				u32 pe_reg, u32 vs_reg, u32 pc_reg, u8 pc_supported);
 #endif /*__TEGRA124_SOR_H__ */