drm/msm/dp: Add DP compliance tests on Snapdragon Chipsets

add event thread to execute events serially from event queue. Also
timeout mode is supported  which allow an event be deferred to be
executed at later time. Both link and phy compliant tests had been
done successfully.

Changes in v2:
-- Fix potential deadlock by removing redundant connect_mutex
-- Check and enable link clock during modeset
-- Drop unused code and fix function prototypes.
-- set sink power to normal operation state (D0) before DPCD read

Changes in v3:
-- push idle pattern at main link before timing generator off
-- add timeout handles for both connect and disconnect

Changes in v4:
-- add ST_SUSPEND_PENDING to handles suspend/modeset test operations
-- clear dp phy aux interrupt status when ERR_DPPHY_AUX error
-- send segment addr during edid read
-- clear bpp depth before MISC register write

Changes in v5:
-- add ST_SUSPENDED to fix crash at resume

Changes in v6:
-- at msm_dp_display_enable() do not return until resume_done to avoid
   kms commit timeout

Signed-off-by: Kuogee Hsieh <khsieh@codeaurora.org>
Signed-off-by: Rob Clark <robdclark@chromium.org>

1 files changed, 263 insertions, 107 deletions

diff --git a/drivers/gpu/drm/msm/dp/dp_ctrl.c b/drivers/gpu/drm/msm/dp/dp_ctrl.c
index ae07e43b541b..70b0e06953f6 100644
--- a/drivers/gpu/drm/msm/dp/dp_ctrl.c
+++ b/drivers/gpu/drm/msm/dp/dp_ctrl.c
@@ -26,6 +26,13 @@
 #define MR_LINK_SYMBOL_ERM 0x80
 #define MR_LINK_PRBS7 0x100
 #define MR_LINK_CUSTOM80 0x200
+#define MR_LINK_TRAINING4  0x40
+
+enum {
+	DP_TRAINING_NONE,
+	DP_TRAINING_1,
+	DP_TRAINING_2,
+};
 
 struct dp_tu_calc_input {
 	u64 lclk;        /* 162, 270, 540 and 810 */
@@ -58,7 +65,6 @@ struct dp_vc_tu_mapping_table {
 
 struct dp_ctrl_private {
 	struct dp_ctrl dp_ctrl;
-
 	struct device *dev;
 	struct drm_dp_aux *aux;
 	struct dp_panel *panel;
@@ -68,10 +74,16 @@ struct dp_ctrl_private {
 	struct dp_catalog *catalog;
 
 	struct completion idle_comp;
-	struct mutex push_idle_mutex;
 	struct completion video_comp;
 };
 
+struct dp_cr_status {
+	u8 lane_0_1;
+	u8 lane_2_3;
+};
+
+#define DP_LANE0_1_CR_DONE	0x11
+
 static int dp_aux_link_configure(struct drm_dp_aux *aux,
 					struct dp_link_info *link)
 {
@@ -97,8 +109,6 @@ void dp_ctrl_push_idle(struct dp_ctrl *dp_ctrl)
 
 	ctrl = container_of(dp_ctrl, struct dp_ctrl_private, dp_ctrl);
 
-	mutex_lock(&ctrl->push_idle_mutex);
-
 	reinit_completion(&ctrl->idle_comp);
 	dp_catalog_ctrl_state_ctrl(ctrl->catalog, DP_STATE_CTRL_PUSH_IDLE);
 
@@ -106,7 +116,6 @@ void dp_ctrl_push_idle(struct dp_ctrl *dp_ctrl)
 			IDLE_PATTERN_COMPLETION_TIMEOUT_JIFFIES))
 		pr_warn("PUSH_IDLE pattern timedout\n");
 
-	mutex_unlock(&ctrl->push_idle_mutex);
 	pr_debug("mainlink off done\n");
 }
 
@@ -979,7 +988,7 @@ static int dp_ctrl_wait4video_ready(struct dp_ctrl_private *ctrl)
 
 	if (!wait_for_completion_timeout(&ctrl->video_comp,
 				WAIT_FOR_VIDEO_READY_TIMEOUT_JIFFIES)) {
-		DRM_ERROR("Link Train timedout\n");
+		DRM_ERROR("wait4video timedout\n");
 		ret = -ETIMEDOUT;
 	}
 	return ret;
@@ -1000,13 +1009,13 @@ static int dp_ctrl_update_vx_px(struct dp_ctrl_private *ctrl)
 	if (ret)
 		return ret;
 
-	if (voltage_swing_level > DP_TRAIN_VOLTAGE_SWING_MAX) {
+	if (voltage_swing_level >= DP_TRAIN_VOLTAGE_SWING_MAX) {
 		DRM_DEBUG_DP("max. voltage swing level reached %d\n",
 				voltage_swing_level);
 		max_level_reached |= DP_TRAIN_MAX_SWING_REACHED;
 	}
 
-	if (pre_emphasis_level == DP_TRAIN_PRE_EMPHASIS_MAX) {
+	if (pre_emphasis_level >= DP_TRAIN_PRE_EMPHASIS_MAX) {
 		DRM_DEBUG_DP("max. pre-emphasis level reached %d\n",
 				pre_emphasis_level);
 		max_level_reached  |= DP_TRAIN_MAX_PRE_EMPHASIS_REACHED;
@@ -1038,8 +1047,11 @@ static bool dp_ctrl_train_pattern_set(struct dp_ctrl_private *ctrl,
 	DRM_DEBUG_DP("sink: pattern=%x\n", pattern);
 
 	buf = pattern;
-	ret = drm_dp_dpcd_writeb(ctrl->aux,
-					DP_TRAINING_PATTERN_SET, buf);
+
+	if (pattern && pattern != DP_TRAINING_PATTERN_4)
+		buf |= DP_LINK_SCRAMBLING_DISABLE;
+
+	ret = drm_dp_dpcd_writeb(ctrl->aux, DP_TRAINING_PATTERN_SET, buf);
 	return ret == 1;
 }
 
@@ -1065,19 +1077,23 @@ static int dp_ctrl_read_link_status(struct dp_ctrl_private *ctrl,
 	return -ETIMEDOUT;
 }
 
-static int dp_ctrl_link_train_1(struct dp_ctrl_private *ctrl)
+static int dp_ctrl_link_train_1(struct dp_ctrl_private *ctrl,
+		struct dp_cr_status *cr, int *training_step)
 {
 	int tries, old_v_level, ret = 0;
 	u8 link_status[DP_LINK_STATUS_SIZE];
-	int const maximum_retries = 5;
+	int const maximum_retries = 4;
 
 	dp_catalog_ctrl_state_ctrl(ctrl->catalog, 0);
 
+	*training_step = DP_TRAINING_1;
+
 	ret = dp_catalog_ctrl_set_pattern(ctrl->catalog, DP_TRAINING_PATTERN_1);
 	if (ret)
 		return ret;
 	dp_ctrl_train_pattern_set(ctrl, DP_TRAINING_PATTERN_1 |
 		DP_LINK_SCRAMBLING_DISABLE);
+
 	ret = dp_ctrl_update_vx_px(ctrl);
 	if (ret)
 		return ret;
@@ -1091,12 +1107,15 @@ static int dp_ctrl_link_train_1(struct dp_ctrl_private *ctrl)
 		if (ret)
 			return ret;
 
+		cr->lane_0_1 = link_status[0];
+		cr->lane_2_3 = link_status[1];
+
 		if (drm_dp_clock_recovery_ok(link_status,
 			ctrl->link->link_params.num_lanes)) {
-			return ret;
+			return 0;
 		}
 
-		if (ctrl->link->phy_params.v_level >
+		if (ctrl->link->phy_params.v_level >=
 			DP_TRAIN_VOLTAGE_SWING_MAX) {
 			DRM_ERROR_RATELIMITED("max v_level reached\n");
 			return -EAGAIN;
@@ -1119,8 +1138,10 @@ static int dp_ctrl_link_train_1(struct dp_ctrl_private *ctrl)
 	return -ETIMEDOUT;
 }
 
-static void dp_ctrl_link_rate_down_shift(struct dp_ctrl_private *ctrl)
+static int dp_ctrl_link_rate_down_shift(struct dp_ctrl_private *ctrl)
 {
+	int ret = 0;
+
 	switch (ctrl->link->link_params.rate) {
 	case 810000:
 		ctrl->link->link_params.rate = 540000;
@@ -1129,13 +1150,33 @@ static void dp_ctrl_link_rate_down_shift(struct dp_ctrl_private *ctrl)
 		ctrl->link->link_params.rate = 270000;
 		break;
 	case 270000:
+		ctrl->link->link_params.rate = 162000;
+		break;
 	case 162000:
 	default:
-		ctrl->link->link_params.rate = 162000;
+		ret = -EINVAL;
 		break;
 	};
 
-	DRM_DEBUG_DP("new rate=0x%x\n", ctrl->link->link_params.rate);
+	if (!ret)
+		DRM_DEBUG_DP("new rate=0x%x\n", ctrl->link->link_params.rate);
+
+	return ret;
+}
+
+static int dp_ctrl_link_lane_down_shift(struct dp_ctrl_private *ctrl)
+{
+
+	if (ctrl->link->link_params.num_lanes == 1)
+		return -1;
+
+	ctrl->link->link_params.num_lanes /= 2;
+	ctrl->link->link_params.rate = ctrl->panel->link_info.rate;
+
+	ctrl->link->phy_params.p_level = 0;
+	ctrl->link->phy_params.v_level = 0;
+
+	return 0;
 }
 
 static void dp_ctrl_clear_training_pattern(struct dp_ctrl_private *ctrl)
@@ -1144,7 +1185,8 @@ static void dp_ctrl_clear_training_pattern(struct dp_ctrl_private *ctrl)
 	drm_dp_link_train_channel_eq_delay(ctrl->panel->dpcd);
 }
 
-static int dp_ctrl_link_training_2(struct dp_ctrl_private *ctrl)
+static int dp_ctrl_link_train_2(struct dp_ctrl_private *ctrl,
+		struct dp_cr_status *cr, int *training_step)
 {
 	int tries = 0, ret = 0;
 	char pattern;
@@ -1153,6 +1195,8 @@ static int dp_ctrl_link_training_2(struct dp_ctrl_private *ctrl)
 
 	dp_catalog_ctrl_state_ctrl(ctrl->catalog, 0);
 
+	*training_step = DP_TRAINING_2;
+
 	if (drm_dp_tps3_supported(ctrl->panel->dpcd))
 		pattern = DP_TRAINING_PATTERN_3;
 	else
@@ -1174,10 +1218,13 @@ static int dp_ctrl_link_training_2(struct dp_ctrl_private *ctrl)
 		ret = dp_ctrl_read_link_status(ctrl, link_status);
 		if (ret)
 			return ret;
+		cr->lane_0_1 = link_status[0];
+		cr->lane_2_3 = link_status[1];
 
 		if (drm_dp_channel_eq_ok(link_status,
-			ctrl->link->link_params.num_lanes))
-			return ret;
+			ctrl->link->link_params.num_lanes)) {
+			return 0;
+		}
 
 		dp_link_adjust_levels(ctrl->link, link_status);
 		ret = dp_ctrl_update_vx_px(ctrl);
@@ -1189,15 +1236,15 @@ static int dp_ctrl_link_training_2(struct dp_ctrl_private *ctrl)
 	return -ETIMEDOUT;
 }
 
-static int dp_ctrl_link_train(struct dp_ctrl_private *ctrl)
+static int dp_ctrl_reinitialize_mainlink(struct dp_ctrl_private *ctrl);
+
+static int dp_ctrl_link_train(struct dp_ctrl_private *ctrl,
+		struct dp_cr_status *cr, int *training_step)
 {
 	int ret = 0;
 	u8 encoding = DP_SET_ANSI_8B10B;
 	struct dp_link_info link_info = {0};
 
-	ctrl->link->phy_params.p_level = 0;
-	ctrl->link->phy_params.v_level = 0;
-
 	dp_ctrl_config_ctrl(ctrl);
 
 	link_info.num_lanes = ctrl->link->link_params.num_lanes;
@@ -1208,7 +1255,7 @@ static int dp_ctrl_link_train(struct dp_ctrl_private *ctrl)
 	drm_dp_dpcd_write(ctrl->aux, DP_MAIN_LINK_CHANNEL_CODING_SET,
 				&encoding, 1);
 
-	ret = dp_ctrl_link_train_1(ctrl);
+	ret = dp_ctrl_link_train_1(ctrl, cr, training_step);
 	if (ret) {
 		DRM_ERROR("link training #1 failed. ret=%d\n", ret);
 		goto end;
@@ -1217,7 +1264,7 @@ static int dp_ctrl_link_train(struct dp_ctrl_private *ctrl)
 	/* print success info as this is a result of user initiated action */
 	DRM_DEBUG_DP("link training #1 successful\n");
 
-	ret = dp_ctrl_link_training_2(ctrl);
+	ret = dp_ctrl_link_train_2(ctrl, cr, training_step);
 	if (ret) {
 		DRM_ERROR("link training #2 failed. ret=%d\n", ret);
 		goto end;
@@ -1229,58 +1276,36 @@ static int dp_ctrl_link_train(struct dp_ctrl_private *ctrl)
 end:
 	dp_catalog_ctrl_state_ctrl(ctrl->catalog, 0);
 
-	dp_ctrl_clear_training_pattern(ctrl);
 	return ret;
 }
 
-static int dp_ctrl_setup_main_link(struct dp_ctrl_private *ctrl, bool train)
+static int dp_ctrl_setup_main_link(struct dp_ctrl_private *ctrl,
+		struct dp_cr_status *cr, int *training_step)
 {
-	bool mainlink_ready = false;
 	int ret = 0;
 
 	dp_catalog_ctrl_mainlink_ctrl(ctrl->catalog, true);
 
-	ret = dp_link_psm_config(ctrl->link, &ctrl->panel->link_info, false);
-	if (ret)
-		return ret;
-
 	if (ctrl->link->sink_request & DP_TEST_LINK_PHY_TEST_PATTERN)
 		return ret;
 
-	if (train) {
-		/*
-		 * As part of previous calls, DP controller state might have
-		 * transitioned to PUSH_IDLE. In order to start transmitting
-		 * a link training pattern, we have to first do soft reset.
-		 */
-		dp_catalog_ctrl_reset(ctrl->catalog);
-
-		ret = dp_ctrl_link_train(ctrl);
-		if (ret)
-			return ret;
-	}
-
 	/*
-	 * Set up transfer unit values and set controller state to send
-	 * video.
+	 * As part of previous calls, DP controller state might have
+	 * transitioned to PUSH_IDLE. In order to start transmitting
+	 * a link training pattern, we have to first do soft reset.
 	 */
-	dp_ctrl_setup_tr_unit(ctrl);
-	dp_catalog_ctrl_state_ctrl(ctrl->catalog, DP_STATE_CTRL_SEND_VIDEO);
+	dp_catalog_ctrl_reset(ctrl->catalog);
 
-	ret = dp_ctrl_wait4video_ready(ctrl);
-	if (ret)
-		return ret;
+	ret = dp_ctrl_link_train(ctrl, cr, training_step);
 
-	mainlink_ready = dp_catalog_ctrl_mainlink_ready(ctrl->catalog);
-	DRM_DEBUG_DP("mainlink %s\n", mainlink_ready ? "READY" : "NOT READY");
 	return ret;
 }
 
 static void dp_ctrl_set_clock_rate(struct dp_ctrl_private *ctrl,
-				   char *name, u32 rate)
+			enum dp_pm_type module, char *name, u32 rate)
 {
-	u32 num = ctrl->parser->mp[DP_CTRL_PM].num_clk;
-	struct dss_clk *cfg = ctrl->parser->mp[DP_CTRL_PM].clk_config;
+	u32 num = ctrl->parser->mp[module].num_clk;
+	struct dss_clk *cfg = ctrl->parser->mp[module].clk_config;
 
 	while (num && strcmp(cfg->clk_name, name)) {
 		num--;
@@ -1302,16 +1327,33 @@ static int dp_ctrl_enable_mainlink_clocks(struct dp_ctrl_private *ctrl)
 
 	dp_power_set_link_clk_parent(ctrl->power);
 
-	dp_ctrl_set_clock_rate(ctrl, "ctrl_link",
+	dp_ctrl_set_clock_rate(ctrl, DP_CTRL_PM, "ctrl_link",
 					ctrl->link->link_params.rate);
 
-	dp_ctrl_set_clock_rate(ctrl, "stream_pixel",
-					ctrl->dp_ctrl.pixel_rate);
-
 	ret = dp_power_clk_enable(ctrl->power, DP_CTRL_PM, true);
 	if (ret)
 		DRM_ERROR("Unable to start link clocks. ret=%d\n", ret);
 
+	DRM_DEBUG_DP("link rate=%d pixel_clk=%d\n",
+		ctrl->link->link_params.rate, ctrl->dp_ctrl.pixel_rate);
+
+	return ret;
+}
+
+static int dp_ctrl_enable_stream_clocks(struct dp_ctrl_private *ctrl)
+{
+	int ret = 0;
+
+	dp_ctrl_set_clock_rate(ctrl, DP_STREAM_PM, "stream_pixel",
+					ctrl->dp_ctrl.pixel_rate);
+
+	ret = dp_power_clk_enable(ctrl->power, DP_STREAM_PM, true);
+	if (ret)
+		DRM_ERROR("Unabled to start pixel clocks. ret=%d\n", ret);
+
+	DRM_DEBUG_DP("link rate=%d pixel_clk=%d\n",
+			ctrl->link->link_params.rate, ctrl->dp_ctrl.pixel_rate);
+
 	return ret;
 }
 
@@ -1401,37 +1443,30 @@ static int dp_ctrl_reinitialize_mainlink(struct dp_ctrl_private *ctrl)
 		return ret;
 	}
 
-	dp_ctrl_configure_source_params(ctrl);
-	dp_catalog_ctrl_config_msa(ctrl->catalog,
-		ctrl->link->link_params.rate,
-		ctrl->dp_ctrl.pixel_rate, dp_ctrl_use_fixed_nvid(ctrl));
-	reinit_completion(&ctrl->idle_comp);
-
 	return ret;
 }
 
 static int dp_ctrl_link_maintenance(struct dp_ctrl_private *ctrl)
 {
 	int ret = 0;
-	int tries;
+	struct dp_cr_status cr;
+	int training_step = DP_TRAINING_NONE;
 
 	dp_ctrl_push_idle(&ctrl->dp_ctrl);
 	dp_catalog_ctrl_reset(ctrl->catalog);
 
 	ctrl->dp_ctrl.pixel_rate = ctrl->panel->dp_mode.drm_mode.clock;
 
-	for (tries = 0; tries < 10; tries++) {
-		ret = dp_ctrl_reinitialize_mainlink(ctrl);
-		if (ret) {
-			DRM_ERROR("Failed to reinitialize mainlink. ret=%d\n",
-					ret);
-			break;
-		}
+	ret = dp_ctrl_setup_main_link(ctrl, &cr, &training_step);
+	if (ret)
+		goto end;
 
-		ret = dp_ctrl_setup_main_link(ctrl, true);
-		if (ret == -EAGAIN) /* try with lower link rate */
-			dp_ctrl_link_rate_down_shift(ctrl);
-	}
+	dp_ctrl_clear_training_pattern(ctrl);
+
+	dp_catalog_ctrl_state_ctrl(ctrl->catalog, DP_STATE_CTRL_SEND_VIDEO);
+
+	ret = dp_ctrl_wait4video_ready(ctrl);
+end:
 	return ret;
 }
 
@@ -1444,22 +1479,22 @@ static int dp_ctrl_process_phy_test_request(struct dp_ctrl_private *ctrl)
 		return ret;
 	}
 
-	dp_ctrl_push_idle(&ctrl->dp_ctrl);
 	/*
 	 * The global reset will need DP link related clocks to be
 	 * running. Add the global reset just before disabling the
 	 * link clocks and core clocks.
 	 */
-	dp_catalog_ctrl_reset(ctrl->catalog);
 	ret = dp_ctrl_off(&ctrl->dp_ctrl);
 	if (ret) {
 		DRM_ERROR("failed to disable DP controller\n");
 		return ret;
 	}
 
-	ret = dp_ctrl_on(&ctrl->dp_ctrl);
-	if (ret)
-		DRM_ERROR("failed to enable DP controller\n");
+	ret = dp_ctrl_on_link(&ctrl->dp_ctrl);
+	if (!ret)
+		ret = dp_ctrl_on_stream(&ctrl->dp_ctrl);
+	else
+		DRM_ERROR("failed to enable DP link controller\n");
 
 	return ret;
 }
@@ -1479,27 +1514,33 @@ static bool dp_ctrl_send_phy_test_pattern(struct dp_ctrl_private *ctrl)
 		return false;
 	}
 	dp_catalog_ctrl_send_phy_pattern(ctrl->catalog, pattern_requested);
+	dp_ctrl_update_vx_px(ctrl);
 	dp_link_send_test_response(ctrl->link);
 
 	pattern_sent = dp_catalog_ctrl_read_phy_pattern(ctrl->catalog);
 
 	switch (pattern_sent) {
 	case MR_LINK_TRAINING1:
-		success = pattern_requested ==
-				DP_LINK_QUAL_PATTERN_D10_2;
+		success = (pattern_requested ==
+				DP_PHY_TEST_PATTERN_D10_2);
 		break;
 	case MR_LINK_SYMBOL_ERM:
-		success = (pattern_requested ==
-				DP_LINK_QUAL_PATTERN_ERROR_RATE)
-			|| (pattern_requested ==
-				DP_LINK_QUAL_PATTERN_HBR2_EYE);
+		success = ((pattern_requested ==
+			DP_PHY_TEST_PATTERN_ERROR_COUNT) ||
+				(pattern_requested ==
+				DP_PHY_TEST_PATTERN_CP2520));
 		break;
 	case MR_LINK_PRBS7:
-		success = pattern_requested == DP_LINK_QUAL_PATTERN_PRBS7;
+		success = (pattern_requested ==
+				DP_PHY_TEST_PATTERN_PRBS7);
 		break;
 	case MR_LINK_CUSTOM80:
-		success = pattern_requested ==
-				DP_LINK_QUAL_PATTERN_80BIT_CUSTOM;
+		success = (pattern_requested ==
+				DP_PHY_TEST_PATTERN_80BIT_CUSTOM);
+		break;
+	case MR_LINK_TRAINING4:
+		success = (pattern_requested ==
+				DP_PHY_TEST_PATTERN_SEL_MASK);
 		break;
 	default:
 		success = false;
@@ -1531,12 +1572,12 @@ void dp_ctrl_handle_sink_request(struct dp_ctrl *dp_ctrl)
 		}
 	}
 
-	if (sink_request & DP_LINK_STATUS_UPDATED)
+	if (sink_request & DP_LINK_STATUS_UPDATED) {
 		if (dp_ctrl_link_maintenance(ctrl)) {
-			DRM_ERROR("LM failed: STATUS_UPDATED\n");
+			DRM_ERROR("LM failed: TEST_LINK_TRAINING\n");
 			return;
 		}
-
+	}
 
 	if (sink_request & DP_TEST_LINK_TRAINING) {
 		dp_link_send_test_response(ctrl->link);
@@ -1547,13 +1588,15 @@ void dp_ctrl_handle_sink_request(struct dp_ctrl *dp_ctrl)
 	}
 }
 
-int dp_ctrl_on(struct dp_ctrl *dp_ctrl)
+int dp_ctrl_on_link(struct dp_ctrl *dp_ctrl)
 {
 	int rc = 0;
 	struct dp_ctrl_private *ctrl;
 	u32 rate = 0;
-	u32 link_train_max_retries = 10;
+	int link_train_max_retries = 5;
 	u32 const phy_cts_pixel_clk_khz = 148500;
+	struct dp_cr_status cr;
+	unsigned int training_step;
 
 	if (!dp_ctrl)
 		return -EINVAL;
@@ -1587,6 +1630,9 @@ int dp_ctrl_on(struct dp_ctrl *dp_ctrl)
 	if (rc)
 		return rc;
 
+	ctrl->link->phy_params.p_level = 0;
+	ctrl->link->phy_params.v_level = 0;
+
 	while (--link_train_max_retries &&
 		!atomic_read(&ctrl->dp_ctrl.aborted)) {
 		rc = dp_ctrl_reinitialize_mainlink(ctrl);
@@ -1595,19 +1641,125 @@ int dp_ctrl_on(struct dp_ctrl *dp_ctrl)
 					rc);
 			break;
 		}
-		rc = dp_ctrl_setup_main_link(ctrl, true);
-		if (!rc)
+
+		training_step = DP_TRAINING_NONE;
+		rc = dp_ctrl_setup_main_link(ctrl, &cr, &training_step);
+		if (rc == 0) {
+			/* training completed successfully */
 			break;
-		/* try with lower link rate */
-		dp_ctrl_link_rate_down_shift(ctrl);
+		} else if (training_step == DP_TRAINING_1) {
+			/* link train_1 failed */
+			rc = dp_ctrl_link_rate_down_shift(ctrl);
+			if (rc < 0) { /* already in RBR = 1.6G */
+				if (cr.lane_0_1 & DP_LANE0_1_CR_DONE) {
+					/*
+					 * some lanes are ready,
+					 * reduce lane number
+					 */
+					rc = dp_ctrl_link_lane_down_shift(ctrl);
+					if (rc < 0) { /* lane == 1 already */
+						/* end with failure */
+						break;
+					}
+				} else {
+					/* end with failure */
+					break; /* lane == 1 already */
+				}
+			}
+		} else if (training_step == DP_TRAINING_2) {
+			/* link train_2 failed, lower lane rate */
+			rc = dp_ctrl_link_lane_down_shift(ctrl);
+			if (rc < 0) {
+				/* end with failure */
+				break; /* lane == 1 already */
+			}
+		}
 	}
 
 	if (ctrl->link->sink_request & DP_TEST_LINK_PHY_TEST_PATTERN)
-		dp_ctrl_send_phy_test_pattern(ctrl);
+		return rc;
+
+	/* stop txing train pattern */
+	dp_ctrl_clear_training_pattern(ctrl);
+
+	/*
+	 * keep transmitting idle pattern until video ready
+	 * to avoid main link from loss of sync
+	 */
+	if (rc == 0)  /* link train successfully */
+		dp_ctrl_push_idle(dp_ctrl);
 
 	return rc;
 }
 
+int dp_ctrl_on_stream(struct dp_ctrl *dp_ctrl)
+{
+	u32 rate = 0;
+	int ret = 0;
+	bool mainlink_ready = false;
+	struct dp_ctrl_private *ctrl;
+
+	if (!dp_ctrl)
+		return -EINVAL;
+
+	ctrl = container_of(dp_ctrl, struct dp_ctrl_private, dp_ctrl);
+
+	rate = ctrl->panel->link_info.rate;
+
+	ctrl->link->link_params.rate = rate;
+	ctrl->link->link_params.num_lanes = ctrl->panel->link_info.num_lanes;
+	ctrl->dp_ctrl.pixel_rate = ctrl->panel->dp_mode.drm_mode.clock;
+
+	DRM_DEBUG_DP("rate=%d, num_lanes=%d, pixel_rate=%d\n",
+		ctrl->link->link_params.rate,
+		ctrl->link->link_params.num_lanes, ctrl->dp_ctrl.pixel_rate);
+
+	if (!dp_power_clk_status(ctrl->power, DP_CTRL_PM)) { /* link clk is off */
+		ret = dp_ctrl_enable_mainlink_clocks(ctrl);
+		if (ret) {
+			DRM_ERROR("Failed to start link clocks. ret=%d\n", ret);
+			goto end;
+		}
+	}
+
+	ret = dp_ctrl_enable_stream_clocks(ctrl);
+	if (ret) {
+		DRM_ERROR("Failed to start pixel clocks. ret=%d\n", ret);
+		goto end;
+	}
+
+	if (ctrl->link->sink_request & DP_TEST_LINK_PHY_TEST_PATTERN) {
+		dp_ctrl_send_phy_test_pattern(ctrl);
+		return 0;
+	}
+
+	/*
+	 * Set up transfer unit values and set controller state to send
+	 * video.
+	 */
+	dp_ctrl_configure_source_params(ctrl);
+
+	dp_catalog_ctrl_config_msa(ctrl->catalog,
+		ctrl->link->link_params.rate,
+		ctrl->dp_ctrl.pixel_rate, dp_ctrl_use_fixed_nvid(ctrl));
+
+	reinit_completion(&ctrl->video_comp);
+
+	dp_ctrl_setup_tr_unit(ctrl);
+
+	dp_catalog_ctrl_state_ctrl(ctrl->catalog, DP_STATE_CTRL_SEND_VIDEO);
+
+	ret = dp_ctrl_wait4video_ready(ctrl);
+	if (ret)
+		return ret;
+
+	mainlink_ready = dp_catalog_ctrl_mainlink_ready(ctrl->catalog);
+	DRM_DEBUG_DP("mainlink %s\n", mainlink_ready ? "READY" : "NOT READY");
+
+end:
+	return ret;
+}
+
 int dp_ctrl_off(struct dp_ctrl *dp_ctrl)
 {
 	struct dp_ctrl_private *ctrl;
@@ -1619,11 +1771,16 @@ int dp_ctrl_off(struct dp_ctrl *dp_ctrl)
 	ctrl = container_of(dp_ctrl, struct dp_ctrl_private, dp_ctrl);
 
 	dp_catalog_ctrl_mainlink_ctrl(ctrl->catalog, false);
+
 	dp_catalog_ctrl_reset(ctrl->catalog);
+
+	ret = dp_power_clk_enable(ctrl->power, DP_STREAM_PM, false);
+	if (ret)
+		DRM_ERROR("Failed to disable pixel clocks. ret=%d\n", ret);
+
 	ret = dp_power_clk_enable(ctrl->power, DP_CTRL_PM, false);
 	if (ret) {
-		DRM_ERROR("Failed to disable clocks. ret=%d\n", ret);
-		return ret;
+		DRM_ERROR("Failed to disable link clocks. ret=%d\n", ret);
 	}
 
 	DRM_DEBUG_DP("DP off done\n");
@@ -1674,7 +1831,6 @@ struct dp_ctrl *dp_ctrl_get(struct device *dev, struct dp_link *link,
 
 	init_completion(&ctrl->idle_comp);
 	init_completion(&ctrl->video_comp);
-	mutex_init(&ctrl->push_idle_mutex);
 
 	/* in parameters */
 	ctrl->parser   = parser;