1 files changed, 433 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));