msm: mdss: dp: add support for link re-training

Add support for link re-training after the main link is already
trained and active. Parse the requested lane count and link
bandwidth from the DPCD when hpd_irq is high, and re-train
the main link once the display, and therefore timing generator,
has been turned off.

CRs-Fixed: 1076516
Change-Id: Ifa1b609c532aa601f30e334e87a768bdda78958d
Signed-off-by: Tatenda Chipeperekwa <tatendac@codeaurora.org>

3 files changed, 711 insertions, 132 deletions

diff --git a/drivers/video/fbdev/msm/mdss_dp.c b/drivers/video/fbdev/msm/mdss_dp.c
index b246204f3181..bbd4642b34da 100644
--- a/drivers/video/fbdev/msm/mdss_dp.c
+++ b/drivers/video/fbdev/msm/mdss_dp.c
@@ -857,6 +857,18 @@ int mdss_dp_wait4train(struct mdss_dp_drv_pdata *dp_drv)
 	return ret;
 }
 
+static void mdss_dp_update_cable_status(struct mdss_dp_drv_pdata *dp,
+		bool connected)
+{
+	mutex_lock(&dp->pd_msg_mutex);
+	pr_debug("cable_connected to %d\n", connected);
+	if (dp->cable_connected != connected)
+		dp->cable_connected = connected;
+	else
+		pr_debug("no change in cable status\n");
+	mutex_unlock(&dp->pd_msg_mutex);
+}
+
 static int dp_get_cable_status(struct platform_device *pdev, u32 vote)
 {
 	struct mdss_dp_drv_pdata *dp_ctrl = platform_get_drvdata(pdev);
@@ -1038,115 +1050,234 @@ static inline void mdss_dp_set_audio_switch_node(
 				val);
 }
 
-int mdss_dp_on(struct mdss_panel_data *pdata)
+/**
+ * mdss_dp_get_lane_mapping() - returns lane mapping based on given orientation
+ * @orientation: usb plug orientation
+ * @lane_map: the configured lane mapping
+ *
+ * Returns 0 when the lane mapping is successfully determined based on the
+ * given usb plug orientation.
+ */
+static int mdss_dp_get_lane_mapping(struct mdss_dp_drv_pdata *dp,
+		enum plug_orientation orientation,
+		struct lane_mapping *lane_map)
 {
-	struct mdss_dp_drv_pdata *dp_drv = NULL;
 	int ret = 0;
-	enum plug_orientation orientation = ORIENTATION_NONE;
-	struct lane_mapping ln_map;
 
-	if (!pdata) {
-		pr_err("Invalid input data\n");
-		return -EINVAL;
+	pr_debug("enter: orientation = %d\n", orientation);
+
+	if (!lane_map) {
+		pr_err("invalid lane map input");
+		ret = -EINVAL;
+		goto exit;
 	}
 
-	dp_drv = container_of(pdata, struct mdss_dp_drv_pdata,
-			panel_data);
+	/* Set the default lane mapping */
+	lane_map->lane0 = 2;
+	lane_map->lane1 = 3;
+	lane_map->lane2 = 1;
+	lane_map->lane3 = 0;
+
+	if (orientation == ORIENTATION_CC2) {
+		lane_map->lane0 = 1;
+		lane_map->lane1 = 0;
+		lane_map->lane2 = 2;
+		lane_map->lane3 = 3;
+
+		if (gpio_is_valid(dp->usbplug_cc_gpio)) {
+			gpio_set_value(dp->usbplug_cc_gpio, 1);
+			pr_debug("Configured cc gpio for new Orientation\n");
+		}
+	}
+
+	pr_debug("lane0 = %d, lane1 = %d, lane2 =%d, lane3 =%d\n",
+			lane_map->lane0, lane_map->lane1, lane_map->lane2,
+			lane_map->lane3);
+
+exit:
+	return ret;
+}
+
+/**
+ * mdss_dp_enable_mainlink_clocks() - enables Display Port main link clocks
+ * @dp: Display Port Driver data
+ *
+ * Returns 0 when the main link clocks are successfully enabled.
+ */
+static int mdss_dp_enable_mainlink_clocks(struct mdss_dp_drv_pdata *dp)
+{
+	int ret = 0;
+
+	dp->power_data[DP_CTRL_PM].clk_config[0].rate =
+		((dp->link_rate * DP_LINK_RATE_MULTIPLIER) / 1000);/* KHz */
+
+	dp->pixel_rate = dp->panel_data.panel_info.clk_rate;
+	dp->power_data[DP_CTRL_PM].clk_config[3].rate =
+		(dp->pixel_rate / 1000);/* KHz */
+
+	ret = mdss_dp_clk_ctrl(dp, DP_CTRL_PM, true);
+	if (ret) {
+		pr_err("Unabled to start link clocks\n");
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/**
+ * mdss_dp_disable_mainlink_clocks() - disables Display Port main link clocks
+ * @dp: Display Port Driver data
+ */
+static void mdss_dp_disable_mainlink_clocks(struct mdss_dp_drv_pdata *dp_drv)
+{
+	mdss_dp_clk_ctrl(dp_drv, DP_CTRL_PM, false);
+}
+
+/**
+ * mdss_dp_configure_source_params() - configures DP transmitter source params
+ * @dp: Display Port Driver data
+ * @lane_map: usb port lane mapping
+ *
+ * Configures the DP transmitter source params including details such as lane
+ * configuration, output format and sink/panel timing information.
+ */
+static void mdss_dp_configure_source_params(struct mdss_dp_drv_pdata *dp,
+		struct lane_mapping *lane_map)
+{
+	mdss_dp_ctrl_lane_mapping(&dp->ctrl_io, *lane_map);
+	mdss_dp_fill_link_cfg(dp);
+	mdss_dp_mainlink_ctrl(&dp->ctrl_io, true);
+	mdss_dp_config_ctrl(dp);
+	mdss_dp_sw_mvid_nvid(&dp->ctrl_io);
+	mdss_dp_timing_cfg(&dp->ctrl_io, &dp->panel_data.panel_info);
+}
+
+/**
+ * mdss_dp_train_main_link() - initiates training of DP main link
+ * @dp: Display Port Driver data
+ *
+ * Initiates training of the DP main link and checks the state of the main
+ * link after the training is complete.
+ */
+static void mdss_dp_train_main_link(struct mdss_dp_drv_pdata *dp)
+{
+	int ready = 0;
+
+	pr_debug("enter\n");
+
+	mdss_dp_link_train(dp);
+	mdss_dp_wait4train(dp);
+
+	ready = mdss_dp_mainlink_ready(dp, BIT(0));
+
+	pr_debug("main link %s\n", ready ? "READY" : "NOT READY");
+}
+
+static int mdss_dp_on_irq(struct mdss_dp_drv_pdata *dp_drv)
+{
+	int ret = 0;
+	enum plug_orientation orientation = ORIENTATION_NONE;
+	struct lane_mapping ln_map;
 
 	/* wait until link training is completed */
 	mutex_lock(&dp_drv->train_mutex);
 
-	pr_debug("Enter++ cont_splash=%d\n", dp_drv->cont_splash);
-	/* Default lane mapping */
-	ln_map.lane0 = 2;
-	ln_map.lane1 = 3;
-	ln_map.lane2 = 1;
-	ln_map.lane3 = 0;
-
-	if (!dp_drv->cont_splash) { /* vote for clocks */
-		ret = mdss_dp_clk_ctrl(dp_drv, DP_CORE_PM, true);
-		if (ret) {
-			pr_err("Unabled to start core clocks\n");
-			goto exit;
-		}
-		mdss_dp_hpd_configure(&dp_drv->ctrl_io, true);
-
-		orientation = usbpd_get_plug_orientation(dp_drv->pd);
-		pr_debug("plug Orientation = %d\n", orientation);
-
-		if (orientation == ORIENTATION_CC2) {
-			/* update lane mapping */
-			ln_map.lane0 = 1;
-			ln_map.lane1 = 0;
-			ln_map.lane2 = 2;
-			ln_map.lane3 = 3;
-
-			if (gpio_is_valid(dp_drv->usbplug_cc_gpio)) {
-				gpio_set_value(
-					dp_drv->usbplug_cc_gpio, 1);
-				pr_debug("Configured cc gpio for new Orientation\n");
-			}
+	pr_debug("enter\n");
 
-		}
+	orientation = usbpd_get_plug_orientation(dp_drv->pd);
+	pr_debug("plug orientation = %d\n", orientation);
 
-		if (dp_drv->new_vic && (dp_drv->new_vic != dp_drv->vic))
-			dp_init_panel_info(dp_drv, dp_drv->new_vic);
+	ret = mdss_dp_get_lane_mapping(dp_drv, orientation, &ln_map);
+	if (ret)
+		goto exit;
 
-		dp_drv->link_rate =
-			mdss_dp_gen_link_clk(&dp_drv->panel_data.panel_info,
-						dp_drv->dpcd.max_lane_count);
+	mdss_dp_phy_share_lane_config(&dp_drv->phy_io,
+			orientation, dp_drv->dpcd.max_lane_count);
 
-		pr_debug("link_rate=0x%x, Max rate supported by sink=0x%x\n",
-			       dp_drv->link_rate, dp_drv->dpcd.max_link_rate);
-		if (!dp_drv->link_rate) {
-			pr_err("Unable to configure required link rate\n");
-			ret = -EINVAL;
-			goto exit;
-		}
+	ret = mdss_dp_enable_mainlink_clocks(dp_drv);
+	if (ret)
+		goto exit;
 
-		mdss_dp_phy_share_lane_config(&dp_drv->phy_io,
-				orientation, dp_drv->dpcd.max_lane_count);
+	mdss_dp_mainlink_reset(&dp_drv->ctrl_io);
 
-		pr_debug("link_rate = 0x%x\n", dp_drv->link_rate);
+	reinit_completion(&dp_drv->idle_comp);
 
-		dp_drv->power_data[DP_CTRL_PM].clk_config[0].rate =
-			((dp_drv->link_rate * DP_LINK_RATE_MULTIPLIER) /
-			1000); /* KHz */
+	mdss_dp_configure_source_params(dp_drv, &ln_map);
 
-		dp_drv->pixel_rate = dp_drv->panel_data.panel_info.clk_rate;
-		dp_drv->power_data[DP_CTRL_PM].clk_config[3].rate =
-						(dp_drv->pixel_rate /
-						1000); /* KHz */
+	mdss_dp_train_main_link(dp_drv);
 
-		ret = mdss_dp_clk_ctrl(dp_drv, DP_CTRL_PM, true);
-		if (ret) {
-			pr_err("Unabled to start link clocks\n");
-			goto exit;
-		}
+	dp_drv->power_on = true;
+	pr_debug("end\n");
 
-		mdss_dp_mainlink_reset(&dp_drv->ctrl_io);
+exit:
+	mutex_unlock(&dp_drv->train_mutex);
+	return ret;
+}
 
-		mdss_dp_ctrl_lane_mapping(&dp_drv->ctrl_io, ln_map);
-		reinit_completion(&dp_drv->idle_comp);
-		mdss_dp_fill_link_cfg(dp_drv);
-		mdss_dp_mainlink_ctrl(&dp_drv->ctrl_io, true);
-		mdss_dp_config_ctrl(dp_drv);
-		mdss_dp_sw_mvid_nvid(&dp_drv->ctrl_io);
-		mdss_dp_timing_cfg(&dp_drv->ctrl_io,
-				&dp_drv->panel_data.panel_info);
-	} else {
+int mdss_dp_on_hpd(struct mdss_dp_drv_pdata *dp_drv)
+{
+	int ret = 0;
+	enum plug_orientation orientation = ORIENTATION_NONE;
+	struct lane_mapping ln_map;
+
+	/* wait until link training is completed */
+	mutex_lock(&dp_drv->train_mutex);
+
+	pr_debug("Enter++ cont_splash=%d\n", dp_drv->cont_splash);
+
+	if (dp_drv->cont_splash) {
 		mdss_dp_aux_ctrl(&dp_drv->ctrl_io, true);
+		goto link_training;
 	}
 
-	pr_debug("call link_training\n");
-	mdss_dp_link_train(dp_drv);
+	ret = mdss_dp_clk_ctrl(dp_drv, DP_CORE_PM, true);
+	if (ret) {
+		pr_err("Unabled to start core clocks\n");
+		goto exit;
+	}
+	mdss_dp_hpd_configure(&dp_drv->ctrl_io, true);
 
-	mdss_dp_wait4train(dp_drv);
+	orientation = usbpd_get_plug_orientation(dp_drv->pd);
+	pr_debug("plug Orientation = %d\n", orientation);
 
-	dp_drv->cont_splash = 0;
+	ret = mdss_dp_get_lane_mapping(dp_drv, orientation, &ln_map);
+	if (ret)
+		goto exit;
+
+	if (dp_drv->new_vic && (dp_drv->new_vic != dp_drv->vic))
+		dp_init_panel_info(dp_drv, dp_drv->new_vic);
+
+	dp_drv->link_rate =
+		mdss_dp_gen_link_clk(&dp_drv->panel_data.panel_info,
+				dp_drv->dpcd.max_lane_count);
+
+	pr_debug("link_rate=0x%x, Max rate supported by sink=0x%x\n",
+			dp_drv->link_rate, dp_drv->dpcd.max_link_rate);
+	if (!dp_drv->link_rate) {
+		pr_err("Unable to configure required link rate\n");
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	mdss_dp_phy_share_lane_config(&dp_drv->phy_io,
+			orientation, dp_drv->dpcd.max_lane_count);
+
+	pr_debug("link_rate = 0x%x\n", dp_drv->link_rate);
+
+	ret = mdss_dp_enable_mainlink_clocks(dp_drv);
+	if (ret)
+		goto exit;
+
+	mdss_dp_mainlink_reset(&dp_drv->ctrl_io);
+
+	reinit_completion(&dp_drv->idle_comp);
+
+	mdss_dp_configure_source_params(dp_drv, &ln_map);
 
-	if (mdss_dp_mainlink_ready(dp_drv, BIT(0)))
-		pr_debug("mainlink ready\n");
+link_training:
+	mdss_dp_train_main_link(dp_drv);
+
+	dp_drv->cont_splash = 0;
 
 	dp_drv->power_on = true;
 	mdss_dp_set_audio_switch_node(dp_drv, true);
@@ -1157,49 +1288,78 @@ exit:
 	return ret;
 }
 
-static void mdss_dp_mainlink_off(struct mdss_panel_data *pdata)
+int mdss_dp_on(struct mdss_panel_data *pdata)
 {
 	struct mdss_dp_drv_pdata *dp_drv = NULL;
-	const int idle_pattern_completion_timeout_ms = 3 * HZ / 100;
 
-	dp_drv = container_of(pdata, struct mdss_dp_drv_pdata,
-				panel_data);
-	if (!dp_drv) {
+	if (!pdata) {
 		pr_err("Invalid input data\n");
-		return;
+		return -EINVAL;
+	}
+
+	dp_drv = container_of(pdata, struct mdss_dp_drv_pdata,
+			panel_data);
+
+	return mdss_dp_on_hpd(dp_drv);
+}
+
+static void mdss_dp_reset_test_data(struct mdss_dp_drv_pdata *dp)
+{
+	dp->test_data = (const struct dpcd_test_request){ 0 };
+}
+
+static bool mdss_dp_is_link_training_requested(struct mdss_dp_drv_pdata *dp)
+{
+	return (dp->test_data.test_requested == TEST_LINK_TRAINING);
+}
+
+static bool mdss_dp_soft_hpd_reset(struct mdss_dp_drv_pdata *dp)
+{
+	return mdss_dp_is_link_training_requested(dp) &&
+		dp->alt_mode.dp_status.hpd_irq;
+}
+
+static int mdss_dp_off_irq(struct mdss_dp_drv_pdata *dp_drv)
+{
+	if (!dp_drv->power_on) {
+		pr_debug("panel already powered off\n");
+		return 0;
 	}
-	pr_debug("Entered++\n");
 
 	/* wait until link training is completed */
 	mutex_lock(&dp_drv->train_mutex);
 
-	reinit_completion(&dp_drv->idle_comp);
-	mdss_dp_state_ctrl(&dp_drv->ctrl_io, ST_PUSH_IDLE);
-	if (!wait_for_completion_timeout(&dp_drv->idle_comp,
-			idle_pattern_completion_timeout_ms))
-		pr_warn("PUSH_IDLE pattern timedout\n");
+	pr_debug("start\n");
+
+	mdss_dp_mainlink_ctrl(&dp_drv->ctrl_io, false);
+
+	mdss_dp_audio_enable(&dp_drv->ctrl_io, false);
+
+	/* Make sure the DP main link is disabled before clk disable */
+	wmb();
+	mdss_dp_disable_mainlink_clocks(dp_drv);
+	dp_drv->power_on = false;
 
 	mutex_unlock(&dp_drv->train_mutex);
-	pr_debug("mainlink off done\n");
+	complete_all(&dp_drv->irq_comp);
+	pr_debug("end\n");
+
+	return 0;
 }
 
-int mdss_dp_off(struct mdss_panel_data *pdata)
+static int mdss_dp_off_hpd(struct mdss_dp_drv_pdata *dp_drv)
 {
-	struct mdss_dp_drv_pdata *dp_drv = NULL;
-
-	dp_drv = container_of(pdata, struct mdss_dp_drv_pdata,
-				panel_data);
-	if (!dp_drv) {
-		pr_err("Invalid input data\n");
-		return -EINVAL;
+	if (!dp_drv->power_on) {
+		pr_debug("panel already powered off\n");
+		return 0;
 	}
-	pr_debug("Entered++, cont_splash=%d\n", dp_drv->cont_splash);
 
 	/* wait until link training is completed */
 	mutex_lock(&dp_drv->train_mutex);
 
-	if (dp_drv->link_clks_on)
-		mdss_dp_mainlink_ctrl(&dp_drv->ctrl_io, false);
+	pr_debug("Entered++, cont_splash=%d\n", dp_drv->cont_splash);
+
+	mdss_dp_mainlink_ctrl(&dp_drv->ctrl_io, false);
 
 	mdss_dp_aux_ctrl(&dp_drv->ctrl_io, false);
 
@@ -1219,7 +1379,7 @@ int mdss_dp_off(struct mdss_panel_data *pdata)
 
 	/* Make sure DP is disabled before clk disable */
 	wmb();
-	mdss_dp_clk_ctrl(dp_drv, DP_CTRL_PM, false);
+	mdss_dp_disable_mainlink_clocks(dp_drv);
 	mdss_dp_clk_ctrl(dp_drv, DP_CORE_PM, false);
 
 	mdss_dp_regulator_ctrl(dp_drv, false);
@@ -1232,6 +1392,23 @@ int mdss_dp_off(struct mdss_panel_data *pdata)
 	return 0;
 }
 
+int mdss_dp_off(struct mdss_panel_data *pdata)
+{
+	struct mdss_dp_drv_pdata *dp = NULL;
+
+	dp = container_of(pdata, struct mdss_dp_drv_pdata,
+				panel_data);
+	if (!dp) {
+		pr_err("Invalid input data\n");
+		return -EINVAL;
+	}
+
+	if (mdss_dp_soft_hpd_reset(dp))
+		return mdss_dp_off_irq(dp);
+	else
+		return mdss_dp_off_hpd(dp);
+}
+
 static void mdss_dp_send_cable_notification(
 	struct mdss_dp_drv_pdata *dp, int val)
 {
@@ -1634,6 +1811,34 @@ static int mdss_dp_sysfs_create(struct mdss_dp_drv_pdata *dp,
 	return 0;
 }
 
+static void mdss_dp_mainlink_push_idle(struct mdss_panel_data *pdata)
+{
+	struct mdss_dp_drv_pdata *dp_drv = NULL;
+	const int idle_pattern_completion_timeout_ms = 3 * HZ / 100;
+
+	dp_drv = container_of(pdata, struct mdss_dp_drv_pdata,
+				panel_data);
+	if (!dp_drv) {
+		pr_err("Invalid input data\n");
+		return;
+	}
+	pr_debug("Entered++\n");
+
+	/* wait until link training is completed */
+	mutex_lock(&dp_drv->train_mutex);
+
+	mdss_dp_aux_set_sink_power_state(dp_drv, SINK_POWER_OFF);
+
+	reinit_completion(&dp_drv->idle_comp);
+	mdss_dp_state_ctrl(&dp_drv->ctrl_io, ST_PUSH_IDLE);
+	if (!wait_for_completion_timeout(&dp_drv->idle_comp,
+			idle_pattern_completion_timeout_ms))
+		pr_warn("PUSH_IDLE pattern timedout\n");
+
+	mutex_unlock(&dp_drv->train_mutex);
+	pr_debug("mainlink off done\n");
+}
+
 static int mdss_dp_event_handler(struct mdss_panel_data *pdata,
 				  int event, void *arg)
 {
@@ -1670,7 +1875,7 @@ static int mdss_dp_event_handler(struct mdss_panel_data *pdata,
 	case MDSS_EVENT_BLANK:
 		if (ops && ops->off)
 			ops->off(dp->hdcp_data);
-		mdss_dp_mainlink_off(pdata);
+		mdss_dp_mainlink_push_idle(pdata);
 		break;
 	case MDSS_EVENT_FB_REGISTERED:
 		fbi = (struct fb_info *)arg;
@@ -1976,10 +2181,8 @@ static void usbpd_connect_callback(struct usbpd_svid_handler *hdlr)
 		return;
 	}
 
-	mutex_lock(&dp_drv->pd_msg_mutex);
-	dp_drv->cable_connected = true;
+	mdss_dp_update_cable_status(dp_drv, true);
 	dp_send_events(dp_drv, EV_USBPD_DISCOVER_MODES);
-	mutex_unlock(&dp_drv->pd_msg_mutex);
 	pr_debug("discover_mode event sent\n");
 }
 
@@ -1994,10 +2197,8 @@ static void usbpd_disconnect_callback(struct usbpd_svid_handler *hdlr)
 	}
 
 	pr_debug("cable disconnected\n");
-	mutex_lock(&dp_drv->pd_msg_mutex);
-	dp_drv->cable_connected = false;
+	mdss_dp_update_cable_status(dp_drv, false);
 	dp_drv->alt_mode.current_state = UNKNOWN_STATE;
-	mutex_unlock(&dp_drv->pd_msg_mutex);
 	mdss_dp_notify_clients(dp_drv, false);
 }
 
@@ -2040,6 +2241,108 @@ end:
 	return ret;
 }
 
+/**
+ * mdss_dp_send_test_response() - sends the test response to the sink
+ * @dp: Display Port Driver data
+ *
+ * This function will send the test response to the sink but only after
+ * any previous link training has been completed.
+ */
+static void mdss_dp_send_test_response(struct mdss_dp_drv_pdata *dp)
+{
+	mutex_lock(&dp->train_mutex);
+	mdss_dp_aux_send_test_response(dp);
+	mutex_unlock(&dp->train_mutex);
+}
+
+/**
+ * mdss_dp_hpd_irq_notify_clients() - notifies DP clients of HPD IRQ tear down
+ * @dp: Display Port Driver data
+ *
+ * This function will send a notification to display/audio clients of DP tear
+ * down during an HPD IRQ. This happens only if HPD IRQ is toggled,
+ * in which case the user space proceeds with shutdown of DP driver, including
+ * mainlink disable, and pushing the controller into idle state.
+ */
+static int mdss_dp_hpd_irq_notify_clients(struct mdss_dp_drv_pdata *dp)
+{
+	const int irq_comp_timeout = HZ * 2;
+	int ret = 0;
+
+	if (dp->hpd_irq_toggled) {
+		mdss_dp_notify_clients(dp, false);
+
+		reinit_completion(&dp->irq_comp);
+		ret = wait_for_completion_timeout(&dp->irq_comp,
+				irq_comp_timeout);
+		if (ret <= 0) {
+			pr_warn("irq_comp timed out\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * mdss_dp_process_hpd_irq_high() - handle HPD IRQ transition to HIGH
+ * @dp: Display Port Driver data
+ *
+ * This function will handle the HPD IRQ state transitions from HIGH to HIGH
+ * or LOW to HIGH, indicating the start of a new test request.
+ */
+static void mdss_dp_process_hpd_irq_high(struct mdss_dp_drv_pdata *dp)
+{
+	pr_debug("enter: HPD IRQ High\n");
+
+	dp->hpd_irq_on = true;
+
+	mdss_dp_aux_parse_test_request(dp);
+
+	mdss_dp_send_test_response(dp);
+
+	if (mdss_dp_is_link_training_requested(dp)) {
+		pr_info("%s requested: link rate = 0x%x, lane count = 0x%x\n",
+				mdss_dp_get_test_name(TEST_LINK_TRAINING),
+				dp->test_data.test_link_rate,
+				dp->test_data.test_lane_count);
+		dp->dpcd.max_lane_count =
+			dp->test_data.test_lane_count;
+		dp->link_rate = dp->test_data.test_link_rate;
+
+		if (mdss_dp_hpd_irq_notify_clients(dp))
+			return;
+
+		mdss_dp_on_irq(dp);
+	}
+
+	mdss_dp_reset_test_data(dp);
+
+	pr_debug("done\n");
+}
+
+/**
+ * mdss_dp_process_hpd_irq_low() - handle HPD IRQ transition to LOW
+ * @dp: Display Port Driver data
+ *
+ * This function will handle the HPD IRQ state transitions from HIGH to LOW,
+ * indicating the end of a test request.
+ */
+static void mdss_dp_process_hpd_irq_low(struct mdss_dp_drv_pdata *dp)
+{
+	pr_debug("enter: HPD IRQ low\n");
+
+	dp->hpd_irq_on = false;
+
+	mdss_dp_update_cable_status(dp, false);
+	mdss_dp_mainlink_push_idle(&dp->panel_data);
+	mdss_dp_off_hpd(dp);
+
+	mdss_dp_reset_test_data(dp);
+
+	pr_debug("done\n");
+}
+
 static void usbpd_response_callback(struct usbpd_svid_handler *hdlr, u8 cmd,
 				enum usbpd_svdm_cmd_type cmd_type,
 				const u32 *vdos, int num_vdos)
@@ -2055,8 +2358,10 @@ static void usbpd_response_callback(struct usbpd_svid_handler *hdlr, u8 cmd,
 	pr_debug("callback -> cmd: 0x%x, *vdos = 0x%x, num_vdos = %d\n",
 				cmd, *vdos, num_vdos);
 
-	if (mdss_dp_validate_callback(cmd, cmd_type, num_vdos))
+	if (mdss_dp_validate_callback(cmd, cmd_type, num_vdos)) {
+		pr_debug("invalid callback received\n");
 		return;
+	}
 
 	switch (cmd) {
 	case USBPD_SVDM_DISCOVER_MODES:
@@ -2073,10 +2378,31 @@ static void usbpd_response_callback(struct usbpd_svid_handler *hdlr, u8 cmd,
 		dp_drv->alt_mode.dp_status.response = *vdos;
 		mdss_dp_usbpd_ext_dp_status(&dp_drv->alt_mode.dp_status);
 
-		if (!dp_drv->alt_mode.dp_status.hpd_high)
-			return;
+		dp_drv->hpd_irq_toggled = dp_drv->hpd_irq_on !=
+			dp_drv->alt_mode.dp_status.hpd_irq;
+
+		if (dp_drv->alt_mode.dp_status.hpd_irq) {
+			mdss_dp_process_hpd_irq_high(dp_drv);
+			break;
+		}
+
+		if (dp_drv->hpd_irq_toggled
+				&& !dp_drv->alt_mode.dp_status.hpd_irq) {
+			mdss_dp_process_hpd_irq_low(dp_drv);
+			break;
+		}
+
+		if (!dp_drv->alt_mode.dp_status.hpd_high) {
+			pr_debug("Attention: HPD low\n");
+			mdss_dp_update_cable_status(dp_drv, false);
+			mdss_dp_notify_clients(dp_drv, false);
+			pr_debug("Attention: Notified clients\n");
+			break;
+		}
+
+		pr_debug("Attention: HPD high\n");
 
-		pr_debug("HPD high\n");
+		mdss_dp_update_cable_status(dp_drv, true);
 
 		dp_drv->alt_mode.current_state |= DP_STATUS_DONE;
 
@@ -2096,7 +2422,7 @@ static void usbpd_response_callback(struct usbpd_svid_handler *hdlr, u8 cmd,
 		break;
 	case DP_VDM_CONFIGURE:
 		dp_drv->alt_mode.current_state |= DP_CONFIGURE_DONE;
-		pr_debug("config USBPD to DP done\n");
+		pr_debug("Configure: config USBPD to DP done\n");
 
 		if (dp_drv->alt_mode.dp_status.hpd_high)
 			mdss_dp_host_init(&dp_drv->panel_data);
@@ -2259,7 +2585,10 @@ static int mdss_dp_probe(struct platform_device *pdev)
 
 	dp_drv->inited = true;
 	dp_drv->wait_for_audio_comp = false;
+	dp_drv->hpd_irq_on = false;
+	mdss_dp_reset_test_data(dp_drv);
 	init_completion(&dp_drv->audio_comp);
+	init_completion(&dp_drv->irq_comp);
 
 	pr_debug("done\n");
 
diff --git a/drivers/video/fbdev/msm/mdss_dp.h b/drivers/video/fbdev/msm/mdss_dp.h
index 8d5af4dc5bf3..a5fb91557f20 100644
--- a/drivers/video/fbdev/msm/mdss_dp.h
+++ b/drivers/video/fbdev/msm/mdss_dp.h
@@ -70,8 +70,6 @@
 #define EDP_INTR_ACK_SHIFT	1
 #define EDP_INTR_MASK_SHIFT	2
 
-#define EDP_MAX_LANE		4
-
 /* isr */
 #define EDP_INTR_HPD		BIT(0)
 #define EDP_INTR_AUX_I2C_DONE	BIT(3)
@@ -105,7 +103,7 @@
 	EDP_INTR_FRAME_END | EDP_INTR_CRC_UPDATED)
 
 #define EDP_INTR_MASK2		(EDP_INTR_STATUS2 << 2)
-#define EV_EVENT_STR(x)		#x
+#define DP_ENUM_STR(x)		#x
 
 struct edp_buf {
 	char *start;	/* buffer start addr */
@@ -256,6 +254,13 @@ struct dpcd_link_status {
 	char req_pre_emphasis[4];
 };
 
+struct dpcd_test_request {
+	u32 test_requested;
+	u32 test_link_rate;
+	u32 test_lane_count;
+	u32 response;
+};
+
 struct display_timing_desc {
 	u32 pclk;
 	u32 h_addressable; /* addressable + boder = active */
@@ -401,6 +406,7 @@ struct mdss_dp_drv_pdata {
 	struct completion idle_comp;
 	struct completion video_comp;
 	struct completion audio_comp;
+	struct completion irq_comp;
 	struct mutex aux_mutex;
 	struct mutex train_mutex;
 	struct mutex pd_msg_mutex;
@@ -426,6 +432,8 @@ struct mdss_dp_drv_pdata {
 	u32 bpp;
 	struct dp_statistic dp_stat;
 	bool wait_for_audio_comp;
+	bool hpd_irq_on;
+	bool hpd_irq_toggled;
 
 	/* event */
 	struct workqueue_struct *workq;
@@ -443,8 +451,46 @@ struct mdss_dp_drv_pdata {
 
 	void *hdcp_data;
 	struct hdcp_ops *hdcp_ops;
+	struct dpcd_test_request test_data;
+};
+
+enum dp_lane_count {
+	DP_LANE_COUNT_1	= 1,
+	DP_LANE_COUNT_2	= 2,
+	DP_LANE_COUNT_4	= 4,
+};
+
+enum test_response {
+	TEST_NACK	= 0x0,
+	TEST_ACK	= 0x1,
+};
+
+static inline char *mdss_dp_get_test_response(u32 test_response)
+{
+	switch (test_response) {
+	case TEST_NACK:		return DP_ENUM_STR(TEST_NACK);
+	case TEST_ACK:		return DP_ENUM_STR(TEST_ACK);
+	default:		return "unknown";
+	}
+}
+
+enum test_type {
+	UNKNOWN_TEST		= 0,
+	TEST_LINK_TRAINING	= BIT(0),
+	TEST_PATTERN		= BIT(1),
+	TEST_EDID_READ		= BIT(2),
 };
 
+static inline char *mdss_dp_get_test_name(u32 test_requested)
+{
+	switch (test_requested) {
+	case TEST_LINK_TRAINING:	return DP_ENUM_STR(TEST_LINK_TRAINING);
+	case TEST_PATTERN:		return DP_ENUM_STR(TEST_PATTERN);
+	case TEST_EDID_READ:		return DP_ENUM_STR(TEST_EDID_READ);
+	default:			return "unknown";
+	}
+}
+
 static inline const char *__mdss_dp_pm_name(enum dp_pm_type module)
 {
 	switch (module) {
@@ -470,19 +516,19 @@ static inline char *mdss_dp_ev_event_to_string(int event)
 {
 	switch (event) {
 	case EV_EDP_AUX_SETUP:
-		return EV_EVENT_STR(EV_EDP_AUX_SETUP);
+		return DP_ENUM_STR(EV_EDP_AUX_SETUP);
 	case EV_EDID_READ:
-		return EV_EVENT_STR(EV_EDID_READ);
+		return DP_ENUM_STR(EV_EDID_READ);
 	case EV_DPCD_CAP_READ:
-		return EV_EVENT_STR(EV_DPCD_CAP_READ);
+		return DP_ENUM_STR(EV_DPCD_CAP_READ);
 	case EV_DPCD_STATUS_READ:
-		return EV_EVENT_STR(EV_DPCD_STATUS_READ);
+		return DP_ENUM_STR(EV_DPCD_STATUS_READ);
 	case EV_LINK_TRAIN:
-		return EV_EVENT_STR(EV_LINK_TRAIN);
+		return DP_ENUM_STR(EV_LINK_TRAIN);
 	case EV_IDLE_PATTERNS_SENT:
-		return EV_EVENT_STR(EV_IDLE_PATTERNS_SENT);
+		return DP_ENUM_STR(EV_IDLE_PATTERNS_SENT);
 	case EV_VIDEO_READY:
-		return EV_EVENT_STR(EV_VIDEO_READY);
+		return DP_ENUM_STR(EV_VIDEO_READY);
 	default:
 		return "unknown";
 	}
@@ -492,6 +538,7 @@ void mdss_dp_phy_initialize(struct mdss_dp_drv_pdata *dp);
 
 void mdss_dp_dpcd_cap_read(struct mdss_dp_drv_pdata *dp);
 int mdss_dp_dpcd_status_read(struct mdss_dp_drv_pdata *dp);
+void mdss_dp_aux_parse_test_request(struct mdss_dp_drv_pdata *dp);
 int mdss_dp_edid_read(struct mdss_dp_drv_pdata *dp);
 int mdss_dp_link_train(struct mdss_dp_drv_pdata *dp);
 void dp_aux_i2c_handler(struct mdss_dp_drv_pdata *dp, u32 isr);
@@ -503,5 +550,7 @@ void mdss_dp_sink_power_down(struct mdss_dp_drv_pdata *ep);
 void mdss_dp_lane_power_ctrl(struct mdss_dp_drv_pdata *ep, int up);
 void mdss_dp_config_ctrl(struct mdss_dp_drv_pdata *ep);
 char mdss_dp_gen_link_clk(struct mdss_panel_info *pinfo, char lane_cnt);
+int mdss_dp_aux_set_sink_power_state(struct mdss_dp_drv_pdata *ep, char state);
+void mdss_dp_aux_send_test_response(struct mdss_dp_drv_pdata *ep);
 
 #endif /* MDSS_DP_H */
diff --git a/drivers/video/fbdev/msm/mdss_dp_aux.c b/drivers/video/fbdev/msm/mdss_dp_aux.c
index 3c525b0dac4f..16de8344b5df 100644
--- a/drivers/video/fbdev/msm/mdss_dp_aux.c
+++ b/drivers/video/fbdev/msm/mdss_dp_aux.c
@@ -952,6 +952,197 @@ static int dp_link_status_read(struct mdss_dp_drv_pdata *ep, int len)
 	return len;
 }
 
+/**
+ * dp_sink_send_test_response() - sends a test response to the sink
+ * @dp: Display Port Driver data
+ */
+static void dp_sink_send_test_response(struct mdss_dp_drv_pdata *dp)
+{
+	char test_response[4];
+
+	test_response[0] = dp->test_data.response;
+
+	pr_debug("sending test response %s",
+			mdss_dp_get_test_response(test_response[0]));
+	dp_aux_write_buf(dp, 0x260, test_response, 1, 0);
+}
+
+/**
+ * dp_is_link_rate_valid() - validates the link rate
+ * @lane_rate: link rate requested by the sink
+ *
+ * Returns true if the requested link rate is supported.
+ */
+static bool dp_is_link_rate_valid(u32 link_rate)
+{
+	return (link_rate == DP_LINK_RATE_162) ||
+		(link_rate == DP_LINK_RATE_270) ||
+		(link_rate == DP_LINK_RATE_540);
+}
+
+/**
+ * dp_is_lane_count_valid() - validates the lane count
+ * @lane_count: lane count requested by the sink
+ *
+ * Returns true if the requested lane count is supported.
+ */
+static bool dp_is_lane_count_valid(u32 lane_count)
+{
+	return (lane_count == DP_LANE_COUNT_1) ||
+		(lane_count == DP_LANE_COUNT_2) ||
+		(lane_count == DP_LANE_COUNT_4);
+}
+
+/**
+ * dp_parse_link_training_params() - parses link training parameters from DPCD
+ * @ep: Display Port Driver data
+ *
+ * Returns 0 if it successfully parses the link rate (Byte 0x219) and lane
+ * count (Byte 0x220), and if these values parse are valid.
+ */
+static int dp_parse_link_training_params(struct mdss_dp_drv_pdata *ep)
+{
+	int ret = 0;
+	char *bp;
+	char data;
+	struct edp_buf *rp;
+	int rlen;
+	int const test_parameter_len = 0x1;
+	int const test_link_rate_addr = 0x219;
+	int const test_lane_count_addr = 0x220;
+
+	/* Read the requested link rate (Byte 0x219). */
+	rlen = dp_aux_read_buf(ep, test_link_rate_addr,
+			test_parameter_len, 0);
+	if (rlen < test_parameter_len) {
+		pr_err("failed to read link rate\n");
+		ret = -EINVAL;
+		goto exit;
+	}
+	rp = &ep->rxp;
+	bp = rp->data;
+	data = *bp++;
+
+	if (!dp_is_link_rate_valid(data)) {
+		pr_err("invalid link rate = 0x%x\n", data);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	ep->test_data.test_link_rate = data;
+	pr_debug("link rate = 0x%x\n", ep->test_data.test_link_rate);
+
+	/* Read the requested lane count (Byte 0x220). */
+	rlen = dp_aux_read_buf(ep, test_lane_count_addr,
+			test_parameter_len, 0);
+	if (rlen < test_parameter_len) {
+		pr_err("failed to read lane count\n");
+		ret = -EINVAL;
+		goto exit;
+	}
+	rp = &ep->rxp;
+	bp = rp->data;
+	data = *bp++;
+	data &= 0x1F;
+
+	if (!dp_is_lane_count_valid(data)) {
+		pr_err("invalid lane count = 0x%x\n", data);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	ep->test_data.test_lane_count = data;
+	pr_debug("lane count = 0x%x\n", ep->test_data.test_lane_count);
+
+exit:
+	return ret;
+}
+
+/**
+ * dp_is_test_supported() - checks if test requested by sink is supported
+ * @test_requested: test requested by the sink
+ *
+ * Returns true if the requested test is supported.
+ */
+static bool dp_is_test_supported(u32 test_requested)
+{
+	return test_requested == TEST_LINK_TRAINING;
+}
+
+/**
+ * dp_sink_parse_test_request() - parses test request parameters from sink
+ * @ep: Display Port Driver data
+ *
+ * Parses the DPCD to check if an automated test is requested (Byte 0x201),
+ * and what type of test automation is being requested (Byte 0x218).
+ */
+static void dp_sink_parse_test_request(struct mdss_dp_drv_pdata *ep)
+{
+	int ret = 0;
+	char *bp;
+	char data;
+	struct edp_buf *rp;
+	int rlen;
+	int const test_parameter_len = 0x1;
+	int const device_service_irq_addr = 0x201;
+	int const test_request_addr = 0x218;
+
+	/**
+	 * Read the device service IRQ vector (Byte 0x201) to determine
+	 * whether an automated test has been requested by the sink.
+	 */
+	rlen = dp_aux_read_buf(ep, device_service_irq_addr,
+			test_parameter_len, 0);
+	if (rlen < test_parameter_len) {
+		pr_err("failed to read device service IRQ vector\n");
+		return;
+	}
+	rp = &ep->rxp;
+	bp = rp->data;
+	data = *bp++;
+
+	pr_debug("device service irq vector = 0x%x\n", data);
+
+	if (!(data & BIT(1))) {
+		pr_debug("no test requested\n");
+		return;
+	}
+
+	/**
+	 * Read the test request byte (Byte 0x218) to determine what type
+	 * of automated test has been requested by the sink.
+	 */
+	rlen = dp_aux_read_buf(ep, test_request_addr,
+			test_parameter_len, 0);
+	if (rlen < test_parameter_len) {
+		pr_err("failed to read test_requested\n");
+		return;
+	}
+	rp = &ep->rxp;
+	bp = rp->data;
+	data = *bp++;
+
+	if (!dp_is_test_supported(data)) {
+		pr_debug("test 0x%x not supported\n", data);
+		return;
+	}
+
+	pr_debug("%s requested\n", mdss_dp_get_test_name(data));
+	ep->test_data.test_requested = data;
+
+	if (ep->test_data.test_requested == TEST_LINK_TRAINING)
+		ret = dp_parse_link_training_params(ep);
+
+	/**
+	 * Send a TEST_ACK if all test parameters are valid, otherwise send
+	 * a TEST_NACK.
+	 */
+	if (ret)
+		ep->test_data.response = TEST_NACK;
+	else
+		ep->test_data.response = TEST_ACK;
+}
+
 static int dp_cap_lane_rate_set(struct mdss_dp_drv_pdata *ep)
 {
 	char buf[4];
@@ -1300,7 +1491,7 @@ static int dp_link_rate_down_shift(struct mdss_dp_drv_pdata *ep)
 	return -EINVAL;
 }
 
-static int mdss_dp_sink_power_state(struct mdss_dp_drv_pdata *ep, char state)
+int mdss_dp_aux_set_sink_power_state(struct mdss_dp_drv_pdata *ep, char state)
 {
 	int ret;
 
@@ -1332,7 +1523,7 @@ int mdss_dp_link_train(struct mdss_dp_drv_pdata *dp)
 
 	dp_write(dp->base + DP_MAINLINK_CTRL, 0x1);
 
-	mdss_dp_sink_power_state(dp, SINK_POWER_ON);
+	mdss_dp_aux_set_sink_power_state(dp, SINK_POWER_ON);
 
 train_start:
 	dp->v_level = 0; /* start from default level */
@@ -1387,6 +1578,16 @@ void mdss_dp_dpcd_cap_read(struct mdss_dp_drv_pdata *ep)
 	dp_sink_capability_read(ep, 16);
 }
 
+void mdss_dp_aux_parse_test_request(struct mdss_dp_drv_pdata *ep)
+{
+	dp_sink_parse_test_request(ep);
+}
+
+void mdss_dp_aux_send_test_response(struct mdss_dp_drv_pdata *ep)
+{
+	dp_sink_send_test_response(ep);
+}
+
 int mdss_dp_dpcd_status_read(struct mdss_dp_drv_pdata *ep)
 {
 	struct dpcd_link_status *sp;