diff --git a/drivers/video/tegra124/displayport.h b/drivers/video/tegra124/displayport.h
index c70bbe3..ace6ab0 100644
--- a/drivers/video/tegra124/displayport.h
+++ b/drivers/video/tegra124/displayport.h
@@ -128,6 +128,183 @@ struct dpaux_ctlr {
 #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,
+	DRIVECURRENT_LEVEL2 = 2,
+	DRIVECURRENT_LEVEL3 = 3,
+};
+
+enum {
+	PREEMPHASIS_DISABLED = 0,
+	PREEMPHASIS_LEVEL1   = 1,
+	PREEMPHASIS_LEVEL2   = 2,
+	PREEMPHASIS_LEVEL3   = 3,
+};
+
+enum {
+	POSTCURSOR2_LEVEL0 = 0,
+	POSTCURSOR2_LEVEL1 = 1,
+	POSTCURSOR2_LEVEL2 = 2,
+	POSTCURSOR2_LEVEL3 = 3,
+	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;
+}
+
 /* DPCD definitions which are not defined in drm_dp_helper.h */
 #define DP_DPCD_REV_MAJOR_SHIFT				4
 #define DP_DPCD_REV_MAJOR_MASK				(0xf << 4)
@@ -141,8 +318,16 @@ struct dpaux_ctlr {
 #define DP_MAX_LANE_COUNT_LANE_1			0x1
 #define DP_MAX_LANE_COUNT_LANE_2			0x2
 #define DP_MAX_LANE_COUNT_LANE_4			0x4
+#define DP_MAX_LANE_COUNT_TPS3_SUPPORTED_YES		(1 << 6)
 #define DP_MAX_LANE_COUNT_ENHANCED_FRAMING_YES		(1 << 7)
 
+#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 DP_MAX_DOWNSPREAD_VAL_NONE			0
 #define DP_MAX_DOWNSPREAD_VAL_0_5_PCT			1
 #define DP_MAX_DOWNSPREAD_NO_AUX_HANDSHAKE_LT_T		(1 << 6)
@@ -153,6 +338,8 @@ struct dpaux_ctlr {
 #define DP_LANE_COUNT_SET_ENHANCEDFRAMING_T		(1 << 7)
 
 #define DP_TRAINING_PATTERN_SET_SC_DISABLED_T		(1 << 5)
+#define NV_DPCD_TRAINING_PATTERN_SET_SC_DISABLED_F	(0x00000000 << 5)
+#define NV_DPCD_TRAINING_PATTERN_SET_SC_DISABLED_T	(0x00000001 << 5)
 
 #define DP_MAIN_LINK_CHANNEL_CODING_SET_ASC_RESET_DISABLE	0
 #define DP_MAIN_LINK_CHANNEL_CODING_SET_ASC_RESET_ENABLE	1
@@ -160,7 +347,11 @@ struct dpaux_ctlr {
 #define NV_DPCD_TRAINING_LANE0_1_SET2			0x10f
 #define NV_DPCD_TRAINING_LANE2_3_SET2			0x110
 #define NV_DPCD_LANEX_SET2_PC2_MAX_REACHED_T		(1 << 2)
+#define NV_DPCD_LANEX_SET2_PC2_MAX_REACHED_F		(0 << 2)
 #define NV_DPCD_LANEXPLUS1_SET2_PC2_MAX_REACHED_T	(1 << 6)
+#define NV_DPCD_LANEXPLUS1_SET2_PC2_MAX_REACHED_F	(0 << 6)
+#define NV_DPCD_LANEX_SET2_PC2_SHIFT			0
+#define NV_DPCD_LANEXPLUS1_SET2_PC2_SHIFT		4
 
 #define NV_DPCD_STATUS_LANEX_CR_DONE_SHIFT		0
 #define NV_DPCD_STATUS_LANEX_CR_DONE_NO			(0x00000000)
@@ -181,4 +372,41 @@ struct dpaux_ctlr {
 #define NV_DPCD_STATUS_LANEXPLUS1_SYMBOL_LOCKED_NO	(0x00000000 << 6)
 #define NV_DPCD_STATUS_LANEXPLUS1_SYMBOL_LOCKED_YES	(0x00000001 << 6)
 
+#define NV_DPCD_LANE_ALIGN_STATUS_UPDATED		(0x00000204)
+#define NV_DPCD_LANE_ALIGN_STATUS_UPDATED_DONE_NO	(0x00000000)
+#define NV_DPCD_LANE_ALIGN_STATUS_UPDATED_DONE_YES	(0x00000001)
+
+#define NV_DPCD_STATUS_LANEX_CR_DONE_SHIFT		0
+#define NV_DPCD_STATUS_LANEX_CR_DONE_NO			(0x00000000)
+#define NV_DPCD_STATUS_LANEX_CR_DONE_YES		(0x00000001)
+#define NV_DPCD_STATUS_LANEX_CHN_EQ_DONE_SHIFT		1
+#define NV_DPCD_STATUS_LANEX_CHN_EQ_DONE_NO		(0x00000000 << 1)
+#define NV_DPCD_STATUS_LANEX_CHN_EQ_DONE_YES		(0x00000001 << 1)
+#define NV_DPCD_STATUS_LANEX_SYMBOL_LOCKED_SHFIT	2
+#define NV_DPCD_STATUS_LANEX_SYMBOL_LOCKED_NO		(0x00000000 << 2)
+#define NV_DPCD_STATUS_LANEX_SYMBOL_LOCKED_YES		(0x00000001 << 2)
+#define NV_DPCD_STATUS_LANEXPLUS1_CR_DONE_SHIFT		4
+#define NV_DPCD_STATUS_LANEXPLUS1_CR_DONE_NO		(0x00000000 << 4)
+#define NV_DPCD_STATUS_LANEXPLUS1_CR_DONE_YES		(0x00000001 << 4)
+#define NV_DPCD_STATUS_LANEXPLUS1_CHN_EQ_DONE_SHIFT	5
+#define NV_DPCD_STATUS_LANEXPLUS1_CHN_EQ_DONE_NO	(0x00000000 << 5)
+#define NV_DPCD_STATUS_LANEXPLUS1_CHN_EQ_DONE_YES	(0x00000001 << 5)
+#define NV_DPCD_STATUS_LANEXPLUS1_SYMBOL_LOCKED_SHIFT	6
+#define NV_DPCD_STATUS_LANEXPLUS1_SYMBOL_LOCKED_NO	(0x00000000 << 6)
+#define NV_DPCD_STATUS_LANEXPLUS1_SYMBOL_LOCKED_YES	(0x00000001 << 6)
+
+#define NV_DPCD_ADJUST_REQ_LANEX_DC_SHIFT		0
+#define NV_DPCD_ADJUST_REQ_LANEX_DC_MASK		0x3
+#define NV_DPCD_ADJUST_REQ_LANEX_PE_SHIFT		2
+#define NV_DPCD_ADJUST_REQ_LANEX_PE_MASK		(0x3 << 2)
+#define NV_DPCD_ADJUST_REQ_LANEXPLUS1_DC_SHIFT		4
+#define NV_DPCD_ADJUST_REQ_LANEXPLUS1_DC_MASK		(0x3 << 4)
+#define NV_DPCD_ADJUST_REQ_LANEXPLUS1_PE_SHIFT		6
+#define NV_DPCD_ADJUST_REQ_LANEXPLUS1_PE_MASK		(0x3 << 6)
+#define NV_DPCD_ADJUST_REQ_POST_CURSOR2			(0x0000020C)
+#define NV_DPCD_ADJUST_REQ_POST_CURSOR2_LANE_MASK	0x3
+#define NV_DPCD_ADJUST_REQ_POST_CURSOR2_LANE_SHIFT(i)	(i*2)
+
+#define NV_DPCD_TRAINING_AUX_RD_INTERVAL		(0x0000000E)
+#define NV_DPCD_TRAINING_LANEX_SET_DC_MAX_REACHED_F     (0x00000000 << 2)
 #endif
diff --git a/drivers/video/tegra124/dp.c b/drivers/video/tegra124/dp.c
index cfe76ed..a712a3a 100644
--- a/drivers/video/tegra124/dp.c
+++ b/drivers/video/tegra124/dp.c
@@ -440,6 +440,34 @@ static void tegra_dc_dp_dump_link_cfg(struct tegra_dp_priv *dp,
 }
 #endif
 
+static int _tegra_dp_lower_link_config(struct tegra_dp_priv *dp,
+				       struct tegra_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:
+		debug("dp: Error link rate %d\n", cfg->link_bw);
+		return -ENOLINK;
+	}
+
+	return (cfg->lane_count > 0) ? 0 : -ENOLINK;
+}
+
 /*
  * Calcuate if given cfg can meet the mode request.
  * Return 0 if mode is possible, -1 otherwise
@@ -629,6 +657,8 @@ static int tegra_dc_dp_init_max_link_cfg(
 	if (ret)
 		return ret;
 	link_cfg->max_lane_count = dpcd_data & DP_MAX_LANE_COUNT_MASK;
+	link_cfg->tps3_supported = (dpcd_data &
+			DP_MAX_LANE_COUNT_TPS3_SUPPORTED_YES) ? 1 : 0;
 
 	link_cfg->support_enhanced_framing =
 		(dpcd_data & DP_MAX_LANE_COUNT_ENHANCED_FRAMING_YES) ?
@@ -640,6 +670,10 @@ static int tegra_dc_dp_init_max_link_cfg(
 	link_cfg->downspread = (dpcd_data & DP_MAX_DOWNSPREAD_VAL_0_5_PCT) ?
 				1 : 0;
 
+	ret = tegra_dc_dp_dpcd_read(dp, NV_DPCD_TRAINING_AUX_RD_INTERVAL,
+				    &link_cfg->aux_rd_interval);
+	if (ret)
+		return ret;
 	ret = tegra_dc_dp_dpcd_read(dp, DP_MAX_LINK_RATE,
 				    &link_cfg->max_link_bw);
 	if (ret)
@@ -749,13 +783,431 @@ static int tegra_dc_dp_link_trained(struct tegra_dp_priv *dp,
 	return 0;
 }
 
+static int tegra_dp_channel_eq_status(struct tegra_dp_priv *dp,
+				      const struct tegra_dp_link_config *cfg)
+{
+	u32 cnt;
+	u32 n_lanes = cfg->lane_count;
+	u8 data;
+	u8 ce_done = 1;
+
+	for (cnt = 0; cnt < n_lanes / 2; cnt++) {
+		tegra_dc_dp_dpcd_read(dp, (DP_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_dp_priv *dp,
+					 const struct tegra_dp_link_config *cfg)
+{
+	u32 cnt;
+	u32 n_lanes = cfg->lane_count;
+	u8 data_ptr;
+
+	for (cnt = 0; cnt < n_lanes / 2; cnt++) {
+		tegra_dc_dp_dpcd_read(dp, (DP_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_dp_priv *dp, u32 pe[4], u32 vs[4],
+			       u32 pc[4], u8 pc_supported,
+			       const struct tegra_dp_link_config *cfg)
+{
+	size_t cnt;
+	u8 data_ptr;
+	u32 n_lanes = cfg->lane_count;
+
+	for (cnt = 0; cnt < n_lanes / 2; cnt++) {
+		tegra_dc_dp_dpcd_read(dp, (DP_ADJUST_REQUEST_LANE0_1 + 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_dp_priv *dp,
+					u8 is_clk_recovery,
+					const struct tegra_dp_link_config *cfg)
+{
+	if (!cfg->aux_rd_interval)
+		is_clk_recovery ? udelay(200) :
+					udelay(500);
+	else
+		mdelay(cfg->aux_rd_interval * 4);
+
+	return cfg->aux_rd_interval;
+}
+
+static void tegra_dp_tpg(struct tegra_dp_priv *dp, u32 tp, u32 n_lanes,
+			 const struct tegra_dp_link_config *cfg)
+{
+	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, cfg);
+	tegra_dc_dp_dpcd_write(dp, DP_TRAINING_PATTERN_SET, data);
+}
+
+static int tegra_dp_link_config(struct tegra_dp_priv *dp,
+				const struct tegra_dp_link_config *link_cfg)
+{
+	u8 dpcd_data;
+	u32 retry;
+	int ret;
+
+	if (link_cfg->lane_count == 0) {
+		debug("dp: error: lane count is 0. Can not set link config.\n");
+		return -ENOLINK;
+	}
+
+	/* Set power state if it is not in normal level */
+	ret = tegra_dc_dp_dpcd_read(dp, DP_SET_POWER, &dpcd_data);
+	if (ret)
+		return ret;
+
+	if (dpcd_data == DP_SET_POWER_D3) {
+		dpcd_data = DP_SET_POWER_D0;
+
+		/* DP spec requires 3 retries */
+		for (retry = 3; retry > 0; --retry) {
+			ret = tegra_dc_dp_dpcd_write(dp, DP_SET_POWER,
+						     dpcd_data);
+			if (!ret)
+				break;
+			if (retry == 1) {
+				debug("dp: Failed to set DP panel power\n");
+				return ret;
+			}
+		}
+	}
+
+	/* Enable ASSR if possible */
+	if (link_cfg->alt_scramber_reset_cap) {
+		ret = tegra_dc_dp_set_assr(dp, &dp->sor, 1);
+		if (ret)
+			return ret;
+	}
+
+	ret = tegra_dp_set_link_bandwidth(dp, &dp->sor, link_cfg->link_bw);
+	if (ret) {
+		debug("dp: Failed to set link bandwidth\n");
+		return ret;
+	}
+	ret = tegra_dp_set_lane_count(dp, link_cfg, &dp->sor);
+	if (ret) {
+		debug("dp: Failed to set lane count\n");
+		return ret;
+	}
+	tegra_dc_sor_set_dp_linkctl(&dp->sor, 1, training_pattern_none,
+				    link_cfg);
+
+	return 0;
+}
+
+static int tegra_dp_lower_link_config(struct tegra_dp_priv *dp,
+				      const struct display_timing *timing,
+				      struct tegra_dp_link_config *cfg)
+{
+	struct tegra_dp_link_config tmp_cfg;
+	int ret;
+
+	tmp_cfg = *cfg;
+	cfg->is_valid = 0;
+
+	ret = _tegra_dp_lower_link_config(dp, cfg);
+	if (!ret)
+		ret = tegra_dc_dp_calc_config(dp, timing, cfg);
+	if (!ret)
+		ret = tegra_dp_link_config(dp, cfg);
+	if (ret)
+		goto fail;
+
+	return 0;
+
+fail:
+	*cfg = tmp_cfg;
+	tegra_dp_link_config(dp, &tmp_cfg);
+	return ret;
+}
+
+static int tegra_dp_lt_config(struct tegra_dp_priv *dp, u32 pe[4], u32 vs[4],
+			      u32 pc[4], const struct tegra_dp_link_config *cfg)
+{
+	struct tegra_dc_sor_data *sor = &dp->sor;
+	u32 n_lanes = cfg->lane_count;
+	u8 pc_supported = 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 = PR_LANE2_DP_LANE0_MASK;
+			shift = PR_LANE2_DP_LANE0_SHIFT;
+			break;
+		case 1:
+			mask = PR_LANE1_DP_LANE1_MASK;
+			shift = PR_LANE1_DP_LANE1_SHIFT;
+			break;
+		case 2:
+			mask = PR_LANE0_DP_LANE2_MASK;
+			shift = PR_LANE0_DP_LANE2_SHIFT;
+			break;
+		case 3:
+			mask = PR_LANE3_DP_LANE3_MASK;
+			shift = PR_LANE3_DP_LANE3_SHIFT;
+			break;
+		default:
+			debug("dp: incorrect lane cnt\n");
+			return -EINVAL;
+		}
+
+		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, (DP_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);
+		}
+	}
+
+	return 0;
+}
+
+static int _tegra_dp_channel_eq(struct tegra_dp_priv *dp, u32 pe[4],
+				u32 vs[4], u32 pc[4], u8 pc_supported,
+				u32 n_lanes,
+				const struct tegra_dp_link_config *cfg)
+{
+	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, cfg);
+			tegra_dp_lt_config(dp, pe, vs, pc, cfg);
+		}
+
+		tegra_dp_wait_aux_training(dp, 0, cfg);
+
+		if (!tegra_dp_clock_recovery_status(dp, cfg)) {
+			debug("dp: CR failed in channel EQ sequence!\n");
+			break;
+		}
+
+		if (tegra_dp_channel_eq_status(dp, cfg))
+			return 0;
+	}
+
+	return -EIO;
+}
+
+static int tegra_dp_channel_eq(struct tegra_dp_priv *dp, u32 pe[4], u32 vs[4],
+			       u32 pc[4],
+			       const struct tegra_dp_link_config *cfg)
+{
+	u32 n_lanes = cfg->lane_count;
+	u8 pc_supported = 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, cfg);
+
+	err = _tegra_dp_channel_eq(dp, pe, vs, pc, pc_supported, n_lanes, cfg);
+
+	tegra_dp_tpg(dp, training_pattern_disabled, n_lanes, cfg);
+
+	return err;
+}
+
+static int _tegra_dp_clk_recovery(struct tegra_dp_priv *dp, u32 pe[4],
+				  u32 vs[4], u32 pc[4], u8 pc_supported,
+				  u32 n_lanes,
+				  const struct tegra_dp_link_config *cfg)
+{
+	u32 vs_temp[4];
+	u32 retry_cnt = 0;
+
+	do {
+		tegra_dp_lt_config(dp, pe, vs, pc, cfg);
+		tegra_dp_wait_aux_training(dp, 1, cfg);
+
+		if (tegra_dp_clock_recovery_status(dp, cfg))
+			return 0;
+
+		memcpy(vs_temp, vs, sizeof(vs_temp));
+		tegra_dp_lt_adjust(dp, pe, vs, pc, pc_supported, cfg);
+
+		if (memcmp(vs_temp, vs, sizeof(vs_temp)))
+			retry_cnt = 0;
+		else
+			++retry_cnt;
+	} while (retry_cnt < 5);
+
+	return -EIO;
+}
+
+static int tegra_dp_clk_recovery(struct tegra_dp_priv *dp, u32 pe[4],
+				 u32 vs[4], u32 pc[4],
+				 const struct tegra_dp_link_config *cfg)
+{
+	u32 n_lanes = cfg->lane_count;
+	u8 pc_supported = cfg->tps3_supported;
+	int err;
+
+	tegra_dp_tpg(dp, training_pattern_1, n_lanes, cfg);
+
+	err = _tegra_dp_clk_recovery(dp, pe, vs, pc, pc_supported, n_lanes,
+				     cfg);
+	if (err < 0)
+		tegra_dp_tpg(dp, training_pattern_disabled, n_lanes, cfg);
+
+	return err;
+}
+
+static int tegra_dc_dp_full_link_training(struct tegra_dp_priv *dp,
+					  const struct display_timing *timing,
+					  struct tegra_dp_link_config *cfg)
+{
+	struct tegra_dc_sor_data *sor = &dp->sor;
+	int err;
+	u32 pe[4], vs[4], pc[4];
+
+	tegra_sor_precharge_lanes(sor, cfg);
+
+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, cfg);
+	if (err) {
+		if (!tegra_dp_lower_link_config(dp, timing, cfg))
+			goto retry_cr;
+
+		debug("dp: clk recovery failed\n");
+		goto fail;
+	}
+
+	err = tegra_dp_channel_eq(dp, pe, vs, pc, cfg);
+	if (err) {
+		if (!tegra_dp_lower_link_config(dp, timing, cfg))
+			goto retry_cr;
+
+		debug("dp: channel equalization failed\n");
+		goto fail;
+	}
+#ifdef DEBUG
+	tegra_dc_dp_dump_link_cfg(dp, cfg);
+#endif
+	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
  */
 static int tegra_dc_dp_fast_link_training(struct tegra_dp_priv *dp,
-	const struct tegra_dp_link_config *link_cfg,
-	struct tegra_dc_sor_data *sor)
+		const struct tegra_dp_link_config *link_cfg,
+		struct tegra_dc_sor_data *sor)
 {
 	u8	link_bw;
 	u8	lane_count;
@@ -818,72 +1270,44 @@ static int tegra_dc_dp_fast_link_training(struct tegra_dp_priv *dp,
 		return -EFAULT;
 	}
 
-	debug("Fast link trainging succeeded, link bw %d, lane %d\n",
+	debug("Fast link training succeeded, link bw %d, lane %d\n",
 	      link_cfg->link_bw, link_cfg->lane_count);
 
 	return 0;
 }
 
-static int tegra_dp_link_config(struct tegra_dp_priv *dp,
-	const struct tegra_dp_link_config *link_cfg,
-	struct tegra_dc_sor_data *sor)
+static int tegra_dp_do_link_training(struct tegra_dp_priv *dp,
+		struct tegra_dp_link_config *link_cfg,
+		const struct display_timing *timing,
+		struct tegra_dc_sor_data *sor)
 {
-	u8	dpcd_data;
 	u8	link_bw;
 	u8	lane_count;
-	u32	retry;
 	int	ret;
 
-	if (link_cfg->lane_count == 0) {
-		debug("dp: error: lane count is 0. Can not set link config.\n");
-		return -1;
-	}
+	/* Now do the fast link training for eDP */
+	ret = tegra_dc_dp_fast_link_training(dp, link_cfg, sor);
+	if (ret) {
+		debug("dp: fast link training failed\n");
 
-	/* Set power state if it is not in normal level */
-	ret = tegra_dc_dp_dpcd_read(dp, DP_SET_POWER, &dpcd_data);
-	if (ret)
-		return ret;
-	if (dpcd_data == DP_SET_POWER_D3) {
-		dpcd_data = DP_SET_POWER_D0;
-		retry = 3;	/* DP spec requires 3 retries */
-		do {
-			ret = tegra_dc_dp_dpcd_write(dp,
-				DP_SET_POWER, dpcd_data);
-		} while ((--retry > 0) && ret);
+		/* Try full link training then */
+		ret = tegra_dc_dp_full_link_training(dp, timing, link_cfg);
 		if (ret) {
-			debug("dp: Failed to set DP panel power\n");
+			debug("dp: full link training failed\n");
 			return ret;
 		}
-	}
-
-	/* Enable ASSR if possible */
-	if (link_cfg->alt_scramber_reset_cap) {
-		ret = tegra_dc_dp_set_assr(dp, sor, 1);
+	} else {
+		/*
+		 * set to a known-good drive setting if fast link succeeded.
+		 * Ignore any error.
+		 */
+		ret = tegra_dc_sor_set_voltage_swing(&dp->sor, link_cfg);
 		if (ret)
-			return ret;
+			debug("Failed to set voltage swing\n");
 	}
 
-	ret = tegra_dp_set_link_bandwidth(dp, sor, link_cfg->link_bw);
-	if (ret) {
-		debug("dp: Failed to set link bandwidth\n");
-		return ret;
-	}
-	ret = tegra_dp_set_lane_count(dp, link_cfg, sor);
-	if (ret) {
-		debug("dp: Failed to set lane count\n");
-		return ret;
-	}
-	tegra_dc_sor_set_dp_linkctl(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, sor);
-	if (ret) {
-		debug("dp: fast link training failed\n");
-		return ret;
-	}
 
-	/* Everything goes well, double check the link config */
-	/* TODO: record edc/c2 data for debugging */
+	/* Everything is good; double check the link config */
 	tegra_dc_sor_read_link_config(sor, &link_bw, &lane_count);
 
 	if ((link_cfg->link_bw == link_bw) &&
@@ -921,7 +1345,8 @@ static int tegra_dc_dp_explore_link_cfg(struct tegra_dp_priv *dp,
 	 * set to max link config
 	 */
 	if ((!tegra_dc_dp_calc_config(dp, timing, &temp_cfg)) &&
-	    (!(tegra_dp_link_config(dp, &temp_cfg, sor))))
+	    (!tegra_dp_link_config(dp, &temp_cfg)) &&
+		(!tegra_dp_do_link_training(dp, &temp_cfg, timing, sor)))
 		/* the max link cfg is doable */
 		memcpy(link_cfg, &temp_cfg, sizeof(temp_cfg));
 
diff --git a/drivers/video/tegra124/sor.c b/drivers/video/tegra124/sor.c
index 8d9204a..e97810d 100644
--- a/drivers/video/tegra124/sor.c
+++ b/drivers/video/tegra124/sor.c
@@ -57,6 +57,23 @@ 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, DP_PADCTL(sor->portnum),
+			      DP_PADCTL_TX_PU_MASK, 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, PR(sor->portnum), mask, pe_reg);
+	tegra_sor_write_field(sor, DC(sor->portnum), mask, vs_reg);
+	if (pc_supported) {
+		tegra_sor_write_field(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)
@@ -809,12 +826,36 @@ void tegra_dc_sor_set_lane_parm(struct tegra_dc_sor_data *sor,
 	tegra_sor_write_field(sor, DP_PADCTL(sor->portnum), 0xf0, 0x0);
 }
 
+int tegra_dc_sor_set_voltage_swing(struct tegra_dc_sor_data *sor,
+				    const struct tegra_dp_link_config *link_cfg)
+{
+	u32 drive_current = 0;
+	u32 pre_emphasis = 0;
+
+	/* Set to a known-good pre-calibrated setting */
+	switch (link_cfg->link_bw) {
+	case SOR_LINK_SPEED_G1_62:
+	case SOR_LINK_SPEED_G2_7:
+		drive_current = 0x13131313;
+		pre_emphasis = 0;
+		break;
+	case SOR_LINK_SPEED_G5_4:
+		debug("T124 does not support 5.4G link clock.\n");
+	default:
+		debug("Invalid sor link bandwidth: %d\n", link_cfg->link_bw);
+		return -ENOLINK;
+	}
+
+	tegra_sor_writel(sor, LANE_DRIVE_CURRENT(sor->portnum), drive_current);
+	tegra_sor_writel(sor, PR(sor->portnum), pre_emphasis);
+
+	return 0;
+}
+
 void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor,
 			const struct tegra_dp_link_config *link_cfg)
 {
 	u32 pad_ctrl = 0;
-	u32 drive_current = 0;
-	u32 pre_emphasis = 0;
 	int err = 0;
 
 	switch (link_cfg->lane_count) {
@@ -849,25 +890,38 @@ void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor,
 		debug("Wait for lane power down failed: %d\n", err);
 		return;
 	}
+}
 
-	/* Set to a known-good pre-calibrated setting */
-	switch (link_cfg->link_bw) {
-	case SOR_LINK_SPEED_G1_62:
-	case SOR_LINK_SPEED_G2_7:
-		drive_current = 0x13131313;
-		pre_emphasis = 0;
+int tegra_sor_precharge_lanes(struct tegra_dc_sor_data *sor,
+			      const struct tegra_dp_link_config *cfg)
+{
+	u32 val = 0;
+
+	switch (cfg->lane_count) {
+	case 4:
+		val |= (DP_PADCTL_PD_TXD_3_NO |
+			DP_PADCTL_PD_TXD_2_NO);
+		/* fall through */
+	case 2:
+		val |= DP_PADCTL_PD_TXD_1_NO;
+		/* fall through */
+	case 1:
+		val |= DP_PADCTL_PD_TXD_0_NO;
 		break;
-	case SOR_LINK_SPEED_G5_4:
-		drive_current = 0x19191919;
-		pre_emphasis = 0x09090909;
 	default:
-		printf("Invalid sor link bandwidth: %d\n", link_cfg->link_bw);
-		return;
+		debug("dp: invalid lane number %d\n", cfg->lane_count);
+		return -EINVAL;
 	}
 
-	tegra_sor_writel(sor, LANE_DRIVE_CURRENT(sor->portnum),
-			 drive_current);
-	tegra_sor_writel(sor, PR(sor->portnum), pre_emphasis);
+	tegra_sor_write_field(sor, DP_PADCTL(sor->portnum),
+			      (0xf << DP_PADCTL_COMODE_TXD_0_DP_TXD_2_SHIFT),
+			      (val << DP_PADCTL_COMODE_TXD_0_DP_TXD_2_SHIFT));
+	udelay(100);
+	tegra_sor_write_field(sor, DP_PADCTL(sor->portnum),
+			      (0xf << DP_PADCTL_COMODE_TXD_0_DP_TXD_2_SHIFT),
+			      0);
+
+	return 0;
 }
 
 int tegra_dc_sor_init(struct tegra_dc_sor_data **sorp)
diff --git a/drivers/video/tegra124/sor.h b/drivers/video/tegra124/sor.h
index 26c5817..610207b 100644
--- a/drivers/video/tegra124/sor.h
+++ b/drivers/video/tegra124/sor.h
@@ -685,6 +685,7 @@
 #define DP_PADCTL_TX_PU_SHIFT				22
 #define DP_PADCTL_TX_PU_DISABLE				(0 << 22)
 #define DP_PADCTL_TX_PU_ENABLE				(1 << 22)
+#define DP_PADCTL_TX_PU_MASK				(1 << 22)
 #define DP_PADCTL_REG_CTRL_SHIFT			20
 #define DP_PADCTL_REG_CTRL_DEFAULT_MASK			(3 << 20)
 #define DP_PADCTL_VCMMODE_SHIFT				16
@@ -867,6 +868,8 @@ struct tegra_dp_link_config {
 	u32	drive_current;
 	u32     preemphasis;
 	u32	postcursor;
+	u8	aux_rd_interval;
+	u8	tps3_supported;
 };
 
 struct tegra_dc_sor_data {
@@ -895,6 +898,13 @@ void tegra_dc_sor_set_lane_parm(struct tegra_dc_sor_data *sor,
 			const struct tegra_dp_link_config *link_cfg);
 void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor,
 			const struct tegra_dp_link_config *link_cfg);
+int tegra_dc_sor_set_voltage_swing(struct tegra_dc_sor_data *sor,
+				const struct tegra_dp_link_config *link_cfg);
+int tegra_sor_precharge_lanes(struct tegra_dc_sor_data *sor,
+			      const struct tegra_dp_link_config *cfg);
+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);
 
 int tegra_dc_sor_attach(struct tegra_dc_sor_data *sor,
 			const struct display_timing *timing,
