3 files changed, 29 insertions, 23 deletions

diff --git a/src/soc/nvidia/tegra124/dp.c b/src/soc/nvidia/tegra124/dp.c
index 9ff50ead8d..35a5c73036 100644
--- a/src/soc/nvidia/tegra124/dp.c
+++ b/src/soc/nvidia/tegra124/dp.c
@@ -1254,6 +1254,9 @@ static int tegra_dp_do_link_training(struct tegra_dc_dp_data *dp,
 				printk(BIOS_ERR, "dp: full link training failed\n");
 				return ret;
 			}
+	} else {
+		/* set to a known-good drive setting if fast link succeeded */
+		tegra_dc_sor_set_voltage_swing(&dp->sor);
 	}
 
 	/* Everything goes well, double check the link config */
diff --git a/src/soc/nvidia/tegra124/sor.c b/src/soc/nvidia/tegra124/sor.c
index 1f9df6ce06..3eaae0e8f9 100644
--- a/src/soc/nvidia/tegra124/sor.c
+++ b/src/soc/nvidia/tegra124/sor.c
@@ -832,11 +832,34 @@ void tegra_dc_sor_set_lane_parm(struct tegra_dc_sor_data *sor,
 		0xf0, 0x0);
 }
 
-void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor)
+void tegra_dc_sor_set_voltage_swing(struct tegra_dc_sor_data *sor)
 {
-	u32 pad_ctrl = 0;
 	u32 drive_current = 0;
 	u32 pre_emphasis = 0;
+
+	/* Set to a known-good pre-calibrated setting */
+	switch (sor->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:
+		printk(BIOS_WARNING, "T124 does not support 5.4G link clock.\n");
+	default:
+		printk(BIOS_WARNING, "Invalid sor link bandwidth: %d\n",
+			sor->link_cfg->link_bw);
+		return;
+	}
+
+	tegra_sor_writel(sor, NV_SOR_LANE_DRIVE_CURRENT(sor->portnum),
+				drive_current);
+	tegra_sor_writel(sor, NV_SOR_PR(sor->portnum), pre_emphasis);
+}
+
+void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor)
+{
+	u32 pad_ctrl = 0;
 	int err = 0;
 
 	switch (sor->link_cfg->lane_count) {
@@ -873,27 +896,6 @@ void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor)
 			"Wait for lane power down failed: %d\n", err);
 		return;
 	}
-
-	/* Set to a known-good pre-calibrated setting */
-	switch (sor->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:
-		drive_current = 0x19191919;
-		pre_emphasis = 0x09090909;
-		break;
-	default:
-		printk(BIOS_ERR, "Invalid sor link bandwidth: %d\n",
-			sor->link_cfg->link_bw);
-		return;
-	}
-
-	tegra_sor_writel(sor, NV_SOR_LANE_DRIVE_CURRENT(sor->portnum),
-				drive_current);
-	tegra_sor_writel(sor, NV_SOR_PR(sor->portnum), pre_emphasis);
 }
 
 void tegra_sor_precharge_lanes(struct tegra_dc_sor_data *sor)
diff --git a/src/soc/nvidia/tegra124/sor.h b/src/soc/nvidia/tegra124/sor.h
index 4e4211e45e..569cdb970b 100644
--- a/src/soc/nvidia/tegra124/sor.h
+++ b/src/soc/nvidia/tegra124/sor.h
@@ -920,6 +920,7 @@ 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_dc_sor_set_voltage_swing(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,