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>

4 files changed, 652 insertions, 44 deletions

diff --git a/src/soc/nvidia/tegra/displayport.h b/src/soc/nvidia/tegra/displayport.h
index 338ab775c1..5bf8423d11 100644
--- a/src/soc/nvidia/tegra/displayport.h
+++ b/src/soc/nvidia/tegra/displayport.h
@@ -138,6 +138,146 @@
 #define DP_AUX_TIMEOUT_MS		40
 #define DP_DPCP_RETRY_SLEEP_NS		400
 
+static const u32 tegra_dp_vs_regs[][4][4] = {
+	/* postcursor2 L0 */
+	{
+		/* pre-emphasis: L0, L1, L2, L3 */
+		{0x13, 0x19, 0x1e, 0x28}, /* voltage swing: L0 */
+		{0x1e, 0x25, 0x2d}, /* L1 */
+		{0x28, 0x32}, /* L2 */
+		{0x3c}, /* L3 */
+	},
+
+	/* postcursor2 L1 */
+	{
+		{0x12, 0x17, 0x1b, 0x25},
+		{0x1c, 0x23, 0x2a},
+		{0x25, 0x2f},
+		{0x39},
+	},
+
+	/* postcursor2 L2 */
+	{
+		{0x12, 0x16, 0x1a, 0x22},
+		{0x1b, 0x20, 0x27},
+		{0x24, 0x2d},
+		{0x36},
+	},
+
+	/* postcursor2 L3 */
+	{
+		{0x11, 0x14, 0x17, 0x1f},
+		{0x19, 0x1e, 0x24},
+		{0x22, 0x2a},
+		{0x32},
+	},
+};
+
+static const u32 tegra_dp_pe_regs[][4][4] = {
+	/* postcursor2 L0 */
+	{
+		/* pre-emphasis: L0, L1, L2, L3 */
+		{0x00, 0x09, 0x13, 0x25}, /* voltage swing: L0 */
+		{0x00, 0x0f, 0x1e}, /* L1 */
+		{0x00, 0x14}, /* L2 */
+		{0x00}, /* L3 */
+	},
+
+	/* postcursor2 L1 */
+	{
+		{0x00, 0x0a, 0x14, 0x28},
+		{0x00, 0x0f, 0x1e},
+		{0x00, 0x14},
+		{0x00},
+	},
+
+	/* postcursor2 L2 */
+	{
+		{0x00, 0x0a, 0x14, 0x28},
+		{0x00, 0x0f, 0x1e},
+		{0x00, 0x14},
+		{0x00},
+	},
+
+	/* postcursor2 L3 */
+	{
+		{0x00, 0x0a, 0x14, 0x28},
+		{0x00, 0x0f, 0x1e},
+		{0x00, 0x14},
+		{0x00},
+	},
+};
+
+static const u32 tegra_dp_pc_regs[][4][4] = {
+	/* postcursor2 L0 */
+	{
+		/* pre-emphasis: L0, L1, L2, L3 */
+		{0x00, 0x00, 0x00, 0x00}, /* voltage swing: L0 */
+		{0x00, 0x00, 0x00}, /* L1 */
+		{0x00, 0x00}, /* L2 */
+		{0x00}, /* L3 */
+	},
+
+	/* postcursor2 L1 */
+	{
+		{0x02, 0x02, 0x04, 0x05},
+		{0x02, 0x04, 0x05},
+		{0x04, 0x05},
+		{0x05},
+	},
+
+	/* postcursor2 L2 */
+	{
+		{0x04, 0x05, 0x08, 0x0b},
+		{0x05, 0x09, 0x0b},
+		{0x08, 0x0a},
+		{0x0b},
+	},
+
+	/* postcursor2 L3 */
+	{
+		{0x05, 0x09, 0x0b, 0x12},
+		{0x09, 0x0d, 0x12},
+		{0x0b, 0x0f},
+		{0x12},
+	},
+};
+
+static const u32 tegra_dp_tx_pu[][4][4] = {
+	/* postcursor2 L0 */
+	{
+		/* pre-emphasis: L0, L1, L2, L3 */
+		{0x20, 0x30, 0x40, 0x60}, /* voltage swing: L0 */
+		{0x30, 0x40, 0x60}, /* L1 */
+		{0x40, 0x60}, /* L2 */
+		{0x60}, /* L3 */
+	},
+
+	/* postcursor2 L1 */
+	{
+		{0x20, 0x20, 0x30, 0x50},
+		{0x30, 0x40, 0x50},
+		{0x40, 0x50},
+		{0x60},
+	},
+
+	/* postcursor2 L2 */
+	{
+		{0x20, 0x20, 0x30, 0x40},
+		{0x30, 0x30, 0x40},
+		{0x40, 0x50},
+		{0x60},
+	},
+
+	/* postcursor2 L3 */
+	{
+		{0x20, 0x20, 0x20, 0x40},
+		{0x30, 0x30, 0x40},
+		{0x40, 0x40},
+		{0x60},
+	},
+};
+
 enum {
 	driveCurrent_Level0 = 0,
 	driveCurrent_Level1 = 1,
@@ -160,6 +300,20 @@ enum {
 	postCursor2_Supported
 };
 
+static inline int tegra_dp_is_max_vs(u32 pe, u32 vs)
+{
+	return (vs < (driveCurrent_Level3 - pe)) ? 0 : 1;
+}
+
+static inline int tegra_dp_is_max_pe(u32 pe, u32 vs)
+{
+	return (pe < (preEmphasis_Level3 - vs)) ? 0 : 1;
+}
+
+static inline int tegra_dp_is_max_pc(u32 pc)
+{
+	return (pc < postCursor2_Level3) ? 0 : 1;
+}
 
 /* the +10ms is the time for power rail going up from 10-90% or
    90%-10% on powerdown */
@@ -200,6 +354,7 @@ struct tegra_dc_dp_data {
 #define NV_DPCD_MAX_LANE_COUNT_LANE_1			(0x00000001)
 #define NV_DPCD_MAX_LANE_COUNT_LANE_2			(0x00000002)
 #define NV_DPCD_MAX_LANE_COUNT_LANE_4			(0x00000004)
+#define NV_DPCD_MAX_LANE_COUNT_TPS3_SUPPORTED_YES       (0x00000001 << 6)
 #define NV_DPCD_MAX_LANE_COUNT_ENHANCED_FRAMING_NO	(0x00000000 << 7)
 #define NV_DPCD_MAX_LANE_COUNT_ENHANCED_FRAMING_YES	(0x00000001 << 7)
 #define NV_DPCD_MAX_DOWNSPREAD				(0x00000003)
@@ -212,6 +367,7 @@ struct tegra_dc_dp_data {
 #define NV_DPCD_EDP_CONFIG_CAP_ASC_RESET_YES		(0x00000001)
 #define NV_DPCD_EDP_CONFIG_CAP_FRAMING_CHANGE_NO	(0x00000000 << 1)
 #define NV_DPCD_EDP_CONFIG_CAP_FRAMING_CHANGE_YES	(0x00000001 << 1)
+#define NV_DPCD_TRAINING_AUX_RD_INTERVAL				(0x0000000E)
 #define NV_DPCD_LINK_BANDWIDTH_SET			(0x00000100)
 #define NV_DPCD_LANE_COUNT_SET				(0x00000101)
 #define NV_DPCD_LANE_COUNT_SET_ENHANCEDFRAMING_F	(0x00000000 << 7)
@@ -230,8 +386,10 @@ struct tegra_dc_dp_data {
 #define NV_DPCD_TRAINING_LANE3_SET			(0x00000106)
 #define NV_DPCD_TRAINING_LANEX_SET_DC_SHIFT		0
 #define NV_DPCD_TRAINING_LANEX_SET_DC_MAX_REACHED_T	(0x00000001 << 2)
+#define NV_DPCD_TRAINING_LANEX_SET_DC_MAX_REACHED_F     (0x00000000 << 2)
 #define NV_DPCD_TRAINING_LANEX_SET_PE_SHIFT		3
 #define NV_DPCD_TRAINING_LANEX_SET_PE_MAX_REACHED_T	(0x00000001 << 5)
+#define NV_DPCD_TRAINING_LANEX_SET_PE_MAX_REACHED_F	(0x00000000 << 5)
 #define NV_DPCD_DOWNSPREAD_CTRL				(0x00000107)
 #define NV_DPCD_DOWNSPREAD_CTRL_SPREAD_AMP_NONE		(0x00000000 << 4)
 #define NV_DPCD_DOWNSPREAD_CTRL_SPREAD_AMP_LT_0_5	(0x00000001 << 4)
@@ -246,8 +404,10 @@ struct tegra_dc_dp_data {
 #define NV_DPCD_TRAINING_LANE2_3_SET2			(0x00000110)
 #define NV_DPCD_LANEX_SET2_PC2_SHIFT			0
 #define NV_DPCD_LANEX_SET2_PC2_MAX_REACHED_T		(0x00000001 << 2)
+#define NV_DPCD_LANEX_SET2_PC2_MAX_REACHED_F		(0x00000000 << 2)
 #define NV_DPCD_LANEXPLUS1_SET2_PC2_SHIFT		4
 #define NV_DPCD_LANEXPLUS1_SET2_PC2_MAX_REACHED_T	(0x00000001 << 6)
+#define NV_DPCD_LANEXPLUS1_SET2_PC2_MAX_REACHED_F	(0x00000000 << 6)
 #define NV_DPCD_SINK_COUNT				(0x00000200)
 #define NV_DPCD_DEVICE_SERVICE_IRQ_VECTOR		(0x00000201)
 #define NV_DPCD_DEVICE_SERVICE_IRQ_VECTOR_AUTO_TEST_NO	(0x00000000 << 1)
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__ */