Merge "msm: mdss: add support to configure transfer unit for DP"

5 files changed, 902 insertions, 106 deletions

diff --git a/drivers/video/fbdev/msm/mdss_dp.c b/drivers/video/fbdev/msm/mdss_dp.c
index 07e3445c51f7..a99ae97cdb80 100644
--- a/drivers/video/fbdev/msm/mdss_dp.c
+++ b/drivers/video/fbdev/msm/mdss_dp.c
@@ -199,6 +199,97 @@ exit:
 	return rc;
 } /* mdss_dp_get_dt_clk_data */
 
+static int mdss_dp_clk_init(struct mdss_dp_drv_pdata *dp_drv,
+				struct device *dev, bool initialize)
+{
+	struct dss_module_power *core_power_data = NULL;
+	struct dss_module_power *ctrl_power_data = NULL;
+	int rc = 0;
+
+	if (!dp_drv || !dev) {
+		pr_err("invalid input\n");
+		rc = -EINVAL;
+		goto exit;
+	}
+
+	core_power_data = &dp_drv->power_data[DP_CORE_PM];
+	ctrl_power_data = &dp_drv->power_data[DP_CTRL_PM];
+
+	if (!core_power_data || !ctrl_power_data) {
+		pr_err("invalid power_data\n");
+		rc = -EINVAL;
+		goto exit;
+	}
+
+	if (initialize) {
+		rc = msm_dss_get_clk(dev, core_power_data->clk_config,
+			core_power_data->num_clk);
+		if (rc) {
+			DEV_ERR("Failed to get %s clk. Err=%d\n",
+				__mdss_dp_pm_name(DP_CORE_PM), rc);
+			goto exit;
+		}
+
+		rc = msm_dss_get_clk(dev, ctrl_power_data->clk_config,
+			ctrl_power_data->num_clk);
+		if (rc) {
+			DEV_ERR("Failed to get %s clk. Err=%d\n",
+				__mdss_dp_pm_name(DP_CTRL_PM), rc);
+			goto ctrl_get_error;
+		}
+
+	} else {
+		msm_dss_put_clk(ctrl_power_data->clk_config,
+					ctrl_power_data->num_clk);
+		msm_dss_put_clk(core_power_data->clk_config,
+					core_power_data->num_clk);
+	}
+
+	return rc;
+
+ctrl_get_error:
+	msm_dss_put_clk(core_power_data->clk_config,
+				core_power_data->num_clk);
+
+exit:
+	return rc;
+}
+
+static int mdss_dp_clk_set_rate_enable(
+		struct dss_module_power *power_data,
+		bool enable)
+{
+	int ret = 0;
+
+	if (enable) {
+		ret = msm_dss_clk_set_rate(
+			power_data->clk_config,
+			power_data->num_clk);
+		if (ret) {
+			pr_err("failed to set clks rate.\n");
+			goto exit;
+		}
+
+		ret = msm_dss_enable_clk(
+			power_data->clk_config,
+			power_data->num_clk, 1);
+		if (ret) {
+			pr_err("failed to enable clks\n");
+			goto exit;
+		}
+	} else {
+		ret = msm_dss_enable_clk(
+			power_data->clk_config,
+			power_data->num_clk, 0);
+		if (ret) {
+			pr_err("failed to disable clks\n");
+				goto exit;
+		}
+	}
+exit:
+	return ret;
+}
+
 /*
  * This clock control function supports enabling/disabling
  * of core and ctrl power module clocks
@@ -232,35 +323,27 @@ static int mdss_dp_clk_ctrl(struct mdss_dp_drv_pdata *dp_drv,
 			&& (!dp_drv->core_clks_on)) {
 			pr_debug("Need to enable core clks before link clks\n");
 
-			ret = msm_dss_enable_clk(
-				dp_drv->power_data[DP_CORE_PM].clk_config,
-				dp_drv->power_data[DP_CORE_PM].num_clk, 1);
+			ret = mdss_dp_clk_set_rate_enable(
+				&dp_drv->power_data[DP_CORE_PM],
+				enable);
 			if (ret) {
-				pr_err("failed to enable clks for %s\n",
-					__mdss_dp_pm_name(pm_type));
+				pr_err("failed to enable clks: %s. err=%d\n",
+					__mdss_dp_pm_name(DP_CORE_PM), ret);
 				goto error;
 			} else {
 				dp_drv->core_clks_on = true;
 			}
 		}
+	}
 
-		ret = msm_dss_enable_clk(
-			dp_drv->power_data[pm_type].clk_config,
-			dp_drv->power_data[pm_type].num_clk, 1);
-		if (ret) {
-			pr_err("failed to enable clks for %s\n",
-				 __mdss_dp_pm_name(pm_type));
-				goto error;
-		}
-	} else {
-		ret = msm_dss_enable_clk(
-			dp_drv->power_data[pm_type].clk_config,
-			dp_drv->power_data[pm_type].num_clk, 0);
-		if (ret) {
-			pr_err("failed to disable clks for %s\n",
-				__mdss_dp_pm_name(pm_type));
-				goto error;
-		}
+	ret = mdss_dp_clk_set_rate_enable(
+		&dp_drv->power_data[pm_type],
+		enable);
+	if (ret) {
+		pr_err("failed to '%s' clks for: %s. err=%d\n",
+			enable ? "enable" : "disable",
+			__mdss_dp_pm_name(pm_type), ret);
+			goto error;
 	}
 
 	if (pm_type == DP_CORE_PM)
@@ -275,7 +358,7 @@ error:
 static int mdss_dp_regulator_ctrl(struct mdss_dp_drv_pdata *dp_drv,
 					bool enable)
 {
-	int i, ret = 0;
+	int ret = 0, i = 0, j = 0;
 
 	if (dp_drv->core_power == enable) {
 		pr_debug("regulators already %s\n",
@@ -283,27 +366,23 @@ static int mdss_dp_regulator_ctrl(struct mdss_dp_drv_pdata *dp_drv,
 		return 0;
 	}
 
-	if (enable) {
-		for (i = DP_CORE_PM; i < DP_MAX_PM; i++) {
-			ret = msm_dss_enable_vreg(
-				dp_drv->power_data[i].vreg_config,
-				dp_drv->power_data[i].num_vreg, 1);
-			if (ret) {
-				pr_err("failed to enable vregs for %s\n",
+	for (i = DP_CORE_PM; i < DP_MAX_PM; i++) {
+		ret = msm_dss_enable_vreg(
+			dp_drv->power_data[i].vreg_config,
+			dp_drv->power_data[i].num_vreg, enable);
+		if (ret) {
+			pr_err("failed to '%s' vregs for %s\n",
+					enable ? "enable" : "disable",
 					__mdss_dp_pm_name(i));
-				goto error;
-			}
-		}
-	} else {
-		for (i = DP_CORE_PM; i < DP_MAX_PM; i++) {
-			ret = msm_dss_enable_vreg(
-				dp_drv->power_data[i].vreg_config,
-				dp_drv->power_data[i].num_vreg, 1);
-			if (ret) {
-				pr_err("failed to disable vregs for %s\n",
-					__mdss_dp_pm_name(i));
-				goto error;
+			if (enable) {
+				/* Disabling the enabled vregs */
+				for (j = i-1; j >= DP_CORE_PM; j--) {
+					msm_dss_enable_vreg(
+					dp_drv->power_data[j].vreg_config,
+					dp_drv->power_data[j].num_vreg, 0);
+				}
 			}
+			goto error;
 		}
 	}
 
@@ -472,6 +551,218 @@ static int mdss_dp_regulator_init(struct platform_device *pdev,
 	return rc;
 }
 
+static int mdss_dp_pinctrl_set_state(
+	struct mdss_dp_drv_pdata *dp,
+	bool active)
+{
+	struct pinctrl_state *pin_state;
+	int rc = -EFAULT;
+
+	if (IS_ERR_OR_NULL(dp->pin_res.pinctrl))
+		return PTR_ERR(dp->pin_res.pinctrl);
+
+	pin_state = active ? dp->pin_res.state_active
+				: dp->pin_res.state_suspend;
+	if (!IS_ERR_OR_NULL(pin_state)) {
+		rc = pinctrl_select_state(dp->pin_res.pinctrl,
+				pin_state);
+		if (rc)
+			pr_err("can not set %s pins\n",
+			       active ? "mdss_dp_active"
+			       : "mdss_dp_sleep");
+	} else {
+		pr_err("invalid '%s' pinstate\n",
+		       active ? "mdss_dp_active"
+		       : "mdss_dp_sleep");
+	}
+	return rc;
+}
+
+static int mdss_dp_pinctrl_init(struct platform_device *pdev,
+			struct mdss_dp_drv_pdata *dp)
+{
+	dp->pin_res.pinctrl = devm_pinctrl_get(&pdev->dev);
+	if (IS_ERR_OR_NULL(dp->pin_res.pinctrl)) {
+		pr_err("failed to get pinctrl\n");
+		return PTR_ERR(dp->pin_res.pinctrl);
+	}
+
+	dp->pin_res.state_active
+		= pinctrl_lookup_state(dp->pin_res.pinctrl,
+				"mdss_dp_active");
+	if (IS_ERR_OR_NULL(dp->pin_res.state_active)) {
+		pr_err("can not get dp active pinstate\n");
+		return PTR_ERR(dp->pin_res.state_active);
+	}
+
+	dp->pin_res.state_suspend
+		= pinctrl_lookup_state(dp->pin_res.pinctrl,
+				"mdss_dp_sleep");
+	if (IS_ERR_OR_NULL(dp->pin_res.state_suspend)) {
+		pr_err("can not get dp sleep pinstate\n");
+		return PTR_ERR(dp->pin_res.state_suspend);
+	}
+
+	return 0;
+}
+
+static int mdss_dp_request_gpios(struct mdss_dp_drv_pdata *dp)
+{
+	int rc = 0;
+	struct device *dev = NULL;
+
+	if (!dp) {
+		pr_err("invalid input\n");
+		return -EINVAL;
+	}
+
+	dev = &dp->pdev->dev;
+	if (gpio_is_valid(dp->aux_en_gpio)) {
+		rc = devm_gpio_request(dev, dp->aux_en_gpio,
+						"aux_enable");
+		if (rc) {
+			pr_err("request aux_en gpio failed, rc=%d\n",
+				       rc);
+			goto aux_en_gpio_err;
+		}
+	}
+	if (gpio_is_valid(dp->aux_sel_gpio)) {
+		rc = devm_gpio_request(dev, dp->aux_sel_gpio, "aux_sel");
+		if (rc) {
+			pr_err("request aux_sel gpio failed, rc=%d\n",
+				rc);
+			goto aux_sel_gpio_err;
+		}
+	}
+	if (gpio_is_valid(dp->usbplug_cc_gpio)) {
+		rc = devm_gpio_request(dev, dp->usbplug_cc_gpio,
+						"usbplug_cc");
+		if (rc) {
+			pr_err("request usbplug_cc gpio failed, rc=%d\n",
+				rc);
+			goto usbplug_cc_gpio_err;
+		}
+	}
+	if (gpio_is_valid(dp->hpd_gpio)) {
+		rc = devm_gpio_request(dev, dp->hpd_gpio, "hpd");
+		if (rc) {
+			pr_err("request hpd gpio failed, rc=%d\n",
+				rc);
+			goto hpd_gpio_err;
+		}
+	}
+	return rc;
+
+hpd_gpio_err:
+	if (gpio_is_valid(dp->usbplug_cc_gpio))
+		gpio_free(dp->usbplug_cc_gpio);
+usbplug_cc_gpio_err:
+	if (gpio_is_valid(dp->aux_sel_gpio))
+		gpio_free(dp->aux_sel_gpio);
+aux_sel_gpio_err:
+	if (gpio_is_valid(dp->aux_en_gpio))
+		gpio_free(dp->aux_en_gpio);
+aux_en_gpio_err:
+	return rc;
+}
+
+static int mdss_dp_config_gpios(struct mdss_dp_drv_pdata *dp, bool enable)
+{
+	int rc = 0;
+
+	if (enable == true) {
+		rc = mdss_dp_request_gpios(dp);
+		if (rc) {
+			pr_err("gpio request failed\n");
+			return rc;
+		}
+
+		if (gpio_is_valid(dp->aux_en_gpio)) {
+			rc = gpio_direction_output(
+				dp->aux_en_gpio, 0);
+			if (rc)
+				pr_err("unable to set dir for aux_en gpio\n");
+		}
+		if (gpio_is_valid(dp->aux_sel_gpio)) {
+			rc = gpio_direction_output(
+				dp->aux_sel_gpio, 0);
+			if (rc)
+				pr_err("unable to set dir for aux_sel gpio\n");
+		}
+		if (gpio_is_valid(dp->usbplug_cc_gpio)) {
+			gpio_set_value(
+				dp->usbplug_cc_gpio, 0);
+		}
+		if (gpio_is_valid(dp->hpd_gpio)) {
+			gpio_set_value(
+				dp->hpd_gpio, 1);
+		}
+	} else {
+		if (gpio_is_valid(dp->aux_en_gpio)) {
+			gpio_set_value((dp->aux_en_gpio), 0);
+			gpio_free(dp->aux_en_gpio);
+		}
+		if (gpio_is_valid(dp->aux_sel_gpio)) {
+			gpio_set_value((dp->aux_sel_gpio), 0);
+			gpio_free(dp->aux_sel_gpio);
+		}
+		if (gpio_is_valid(dp->usbplug_cc_gpio)) {
+			gpio_set_value((dp->usbplug_cc_gpio), 0);
+			gpio_free(dp->usbplug_cc_gpio);
+		}
+		if (gpio_is_valid(dp->hpd_gpio)) {
+			gpio_set_value((dp->hpd_gpio), 0);
+			gpio_free(dp->hpd_gpio);
+		}
+	}
+	return 0;
+}
+
+static int mdss_dp_parse_gpio_params(struct platform_device *pdev,
+	struct mdss_dp_drv_pdata *dp)
+{
+	dp->aux_en_gpio = of_get_named_gpio(
+			pdev->dev.of_node,
+			"qcom,aux-en-gpio", 0);
+
+	if (!gpio_is_valid(dp->aux_en_gpio)) {
+		pr_err("%d, Aux_en gpio not specified\n",
+					__LINE__);
+		return -EINVAL;
+	}
+
+	dp->aux_sel_gpio = of_get_named_gpio(
+			pdev->dev.of_node,
+			"qcom,aux-sel-gpio", 0);
+
+	if (!gpio_is_valid(dp->aux_sel_gpio)) {
+		pr_err("%d, Aux_sel gpio not specified\n",
+					__LINE__);
+		return -EINVAL;
+	}
+
+	dp->usbplug_cc_gpio = of_get_named_gpio(
+			pdev->dev.of_node,
+			"qcom,usbplug-cc-gpio", 0);
+
+	if (!gpio_is_valid(dp->usbplug_cc_gpio)) {
+		pr_err("%d,usbplug_cc gpio not specified\n",
+					__LINE__);
+		return -EINVAL;
+	}
+
+	dp->hpd_gpio = of_get_named_gpio(
+			pdev->dev.of_node,
+			"qcom,hpd-gpio", 0);
+
+	if (!gpio_is_valid(dp->hpd_gpio)) {
+		pr_info("%d,hpd gpio not specified\n",
+					__LINE__);
+	}
+
+	return 0;
+}
+
 void mdss_dp_phy_initialize(struct mdss_dp_drv_pdata *dp)
 {
 	/*
@@ -587,6 +878,8 @@ int mdss_dp_on(struct mdss_panel_data *pdata)
 {
 	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");
@@ -596,7 +889,15 @@ int mdss_dp_on(struct mdss_panel_data *pdata)
 	dp_drv = container_of(pdata, struct mdss_dp_drv_pdata,
 			panel_data);
 
-	pr_debug("++ cont_splash=%d\n", dp_drv->cont_splash);
+	/* wait until link training is completed */
+	mutex_lock(&dp_drv->host_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);
@@ -606,15 +907,52 @@ int mdss_dp_on(struct mdss_panel_data *pdata)
 		}
 		mdss_dp_phy_reset(&dp_drv->ctrl_io);
 		mdss_dp_aux_reset(&dp_drv->ctrl_io);
-		mdss_dp_mainlink_reset(&dp_drv->ctrl_io);
 		mdss_dp_aux_ctrl(&dp_drv->ctrl_io, true);
 		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");
+			}
+
+		}
+
 		mdss_dp_phy_aux_setup(&dp_drv->phy_io);
 
 		mdss_dp_irq_enable(dp_drv);
 		pr_debug("irq enabled\n");
 		mdss_dp_dpcd_cap_read(dp_drv);
+		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");
+			return -EINVAL;
+		}
+
+		pr_debug("link_rate = 0x%x\n", dp_drv->link_rate);
+
+		dp_drv->power_data[DP_CTRL_PM].clk_config[0].rate =
+				dp_drv->link_rate * DP_LINK_RATE_MULTIPLIER;
+
+		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;
+
 		ret = mdss_dp_clk_ctrl(dp_drv, DP_CTRL_PM, true);
 		if (ret) {
 			mdss_dp_clk_ctrl(dp_drv, DP_CORE_PM, false);
@@ -624,6 +962,7 @@ int mdss_dp_on(struct mdss_panel_data *pdata)
 
 		mdss_dp_mainlink_reset(&dp_drv->ctrl_io);
 
+		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);
@@ -645,7 +984,9 @@ int mdss_dp_on(struct mdss_panel_data *pdata)
 	if (mdss_dp_mainlink_ready(dp_drv, BIT(0)))
 		pr_debug("mainlink ready\n");
 
+	mutex_unlock(&dp_drv->host_mutex);
 	pr_debug("End-\n");
+
 	return ret;
 }
 
@@ -680,6 +1021,9 @@ int mdss_dp_off(struct mdss_panel_data *pdata)
 	mdss_dp_mainlink_reset(&dp_drv->ctrl_io);
 	mdss_dp_mainlink_ctrl(&dp_drv->ctrl_io, false);
 
+	mdss_dp_config_gpios(dp_drv, false);
+	mdss_dp_pinctrl_set_state(dp_drv, false);
+
 	mdss_dp_aux_ctrl(&dp_drv->ctrl_io, false);
 	mdss_dp_clk_ctrl(dp_drv, DP_CTRL_PM, false);
 	mdss_dp_clk_ctrl(dp_drv, DP_CORE_PM, false);
@@ -712,6 +1056,9 @@ static int mdss_dp_host_init(struct mdss_panel_data *pdata)
 		goto vreg_error;
 	}
 
+	mdss_dp_pinctrl_set_state(dp_drv, true);
+	mdss_dp_config_gpios(dp_drv, true);
+
 	ret = mdss_dp_clk_ctrl(dp_drv, DP_CORE_PM, true);
 	if (ret) {
 		pr_err("Unabled to start core clocks\n");
@@ -725,6 +1072,10 @@ static int mdss_dp_host_init(struct mdss_panel_data *pdata)
 	mdss_dp_phy_initialize(dp_drv);
 	mdss_dp_aux_ctrl(&dp_drv->ctrl_io, true);
 
+	pr_debug("Ctrl_hw_rev =0x%x, phy hw_rev =0x%x\n",
+	       mdss_dp_get_ctrl_hw_version(&dp_drv->ctrl_io),
+	       mdss_dp_get_phy_hw_version(&dp_drv->phy_io));
+
 	return ret;
 
 clk_error:
@@ -865,7 +1216,7 @@ static void mdss_dp_event_work(struct work_struct *work)
 	struct mdss_dp_drv_pdata *dp = NULL;
 	struct delayed_work *dw = to_delayed_work(work);
 	unsigned long flag;
-	u32 todo = 0;
+	u32 todo = 0, dp_config_pkt[2];
 
 	if (!dw) {
 		pr_err("invalid work structure\n");
@@ -882,24 +1233,47 @@ static void mdss_dp_event_work(struct work_struct *work)
 	pr_debug("todo=%x\n", todo);
 
 	switch (todo) {
-	case (EV_EDID_READ):
+	case EV_EDID_READ:
 		mdss_dp_edid_read(dp, 0);
 		break;
-	case (EV_DPCD_CAP_READ):
+	case EV_DPCD_CAP_READ:
 		mdss_dp_dpcd_cap_read(dp);
 		break;
-	case (EV_DPCD_STATUS_READ):
+	case EV_DPCD_STATUS_READ:
 		mdss_dp_dpcd_status_read(dp);
 		break;
-	case (EV_LINK_TRAIN):
+	case EV_LINK_TRAIN:
 		mdss_dp_do_link_train(dp);
 		break;
-	case (EV_VIDEO_READY):
+	case EV_VIDEO_READY:
 		mdss_dp_video_ready(dp);
 		break;
-	case (EV_IDLE_PATTERNS_SENT):
+	case EV_IDLE_PATTERNS_SENT:
 		mdss_dp_idle_patterns_sent(dp);
 		break;
+	case EV_USBPD_DISCOVER_MODES:
+		usbpd_send_svdm(dp->pd, USB_C_DP_SID, USBPD_SVDM_DISCOVER_MODES,
+			SVDM_CMD_TYPE_INITIATOR, 0x0, 0x0, 0x0);
+		break;
+	case EV_USBPD_ENTER_MODE:
+		usbpd_send_svdm(dp->pd, USB_C_DP_SID, USBPD_SVDM_ENTER_MODE,
+			SVDM_CMD_TYPE_INITIATOR, 0x1, 0x0, 0x0);
+		break;
+	case EV_USBPD_EXIT_MODE:
+		usbpd_send_svdm(dp->pd, USB_C_DP_SID, USBPD_SVDM_EXIT_MODE,
+			SVDM_CMD_TYPE_INITIATOR, 0x1, 0x0, 0x0);
+		break;
+	case EV_USBPD_DP_STATUS:
+		usbpd_send_svdm(dp->pd, USB_C_DP_SID, DP_VDM_STATUS,
+			SVDM_CMD_TYPE_INITIATOR, 0x1, 0x0, 0x0);
+		break;
+	case EV_USBPD_DP_CONFIGURE:
+		dp_config_pkt[0] = SVDM_HDR(USB_C_DP_SID, VDM_VERSION, 0x1,
+			SVDM_CMD_TYPE_INITIATOR, DP_VDM_CONFIGURE);
+		dp_config_pkt[1] = mdss_dp_usbpd_gen_config_pkt(dp);
+		usbpd_send_svdm(dp->pd, USB_C_DP_SID, DP_VDM_CONFIGURE,
+			SVDM_CMD_TYPE_INITIATOR, 0x1, dp_config_pkt, 0x2);
+		break;
 	default:
 		pr_err("Unknown event:%d\n", todo);
 	}
@@ -987,6 +1361,165 @@ static int mdss_dp_event_setup(struct mdss_dp_drv_pdata *dp)
 	return 0;
 }
 
+static void usbpd_connect_callback(struct usbpd_svid_handler *hdlr)
+{
+	struct mdss_dp_drv_pdata *dp_drv;
+
+	dp_drv = container_of(hdlr, struct mdss_dp_drv_pdata, svid_handler);
+	if (!dp_drv->pd) {
+		pr_err("get_usbpd phandle failed\n");
+		return;
+	}
+
+	mutex_lock(&dp_drv->pd_msg_mutex);
+	dp_drv->cable_connected = true;
+	dp_send_events(dp_drv, EV_USBPD_DISCOVER_MODES);
+	mutex_unlock(&dp_drv->pd_msg_mutex);
+	pr_debug("discover_mode event sent\n");
+}
+
+static void usbpd_disconnect_callback(struct usbpd_svid_handler *hdlr)
+{
+	struct mdss_dp_drv_pdata *dp_drv;
+
+	dp_drv = container_of(hdlr, struct mdss_dp_drv_pdata, svid_handler);
+	if (!dp_drv->pd) {
+		pr_err("get_usbpd phandle failed\n");
+		return;
+	}
+
+	pr_debug("cable disconnected\n");
+	mutex_lock(&dp_drv->pd_msg_mutex);
+	dp_drv->cable_connected = false;
+	mutex_unlock(&dp_drv->pd_msg_mutex);
+}
+
+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)
+{
+	struct mdss_dp_drv_pdata *dp_drv;
+
+	dp_drv = container_of(hdlr, struct mdss_dp_drv_pdata, svid_handler);
+	if (!dp_drv->pd) {
+		pr_err("get_usbpd phandle failed\n");
+		return;
+	}
+
+	pr_debug("callback -> cmd: 0x%x, *vdos = 0x%x, num_vdos = %d\n",
+				cmd, *vdos, num_vdos);
+
+	switch (cmd) {
+	case USBPD_SVDM_DISCOVER_MODES:
+		if (cmd_type == SVDM_CMD_TYPE_RESP_ACK) {
+			dp_drv->alt_mode.dp_cap.response = *vdos;
+			mdss_dp_usbpd_ext_capabilities
+					(&dp_drv->alt_mode.dp_cap);
+			dp_drv->alt_mode.current_state = DISCOVER_MODES_DONE;
+			dp_send_events(dp_drv, EV_USBPD_ENTER_MODE);
+		} else {
+			pr_err("unknown response: %d for Discover_modes\n",
+			       cmd_type);
+		}
+		break;
+	case USBPD_SVDM_ENTER_MODE:
+		if (cmd_type == SVDM_CMD_TYPE_RESP_ACK) {
+			dp_drv->alt_mode.current_state = ENTER_MODE_DONE;
+			dp_send_events(dp_drv, EV_USBPD_DP_STATUS);
+		} else {
+			pr_err("unknown response: %d for Enter_mode\n",
+			       cmd_type);
+		}
+		break;
+	case USBPD_SVDM_ATTENTION:
+		if (cmd_type == SVDM_CMD_TYPE_INITIATOR) {
+			pr_debug("Attention. cmd_type=%d\n",
+			       cmd_type);
+			if (!dp_drv->alt_mode.current_state
+						== ENTER_MODE_DONE) {
+				pr_debug("sending discover_mode\n");
+				dp_send_events(dp_drv, EV_USBPD_DISCOVER_MODES);
+				break;
+			}
+			if (num_vdos == 1) {
+				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) {
+					pr_debug("HPD high\n");
+					dp_drv->alt_mode.current_state =
+							DP_STATUS_DONE;
+					dp_send_events
+						(dp_drv, EV_USBPD_DP_CONFIGURE);
+				}
+			}
+		} else {
+			pr_debug("unknown response: %d for Attention\n",
+			       cmd_type);
+		}
+		break;
+	case DP_VDM_STATUS:
+		if (cmd_type == SVDM_CMD_TYPE_RESP_ACK) {
+			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) {
+				pr_debug("HDP high\n");
+				dp_drv->alt_mode.current_state =
+						DP_STATUS_DONE;
+				dp_send_events(dp_drv, EV_USBPD_DP_CONFIGURE);
+			}
+		} else {
+			pr_err("unknown response: %d for DP_Status\n",
+			       cmd_type);
+		}
+		break;
+	case DP_VDM_CONFIGURE:
+		if ((dp_drv->cable_connected == true)
+				|| (cmd_type == SVDM_CMD_TYPE_RESP_ACK)) {
+			dp_drv->alt_mode.current_state = DP_CONFIGURE_DONE;
+			pr_debug("config USBPD to DP done\n");
+			mdss_dp_host_init(&dp_drv->panel_data);
+		} else {
+			pr_err("unknown response: %d for DP_Configure\n",
+			       cmd_type);
+		}
+		break;
+	default:
+		pr_err("unknown cmd: %d\n", cmd);
+		break;
+	}
+}
+
+static int mdss_dp_usbpd_setup(struct mdss_dp_drv_pdata *dp_drv)
+{
+	int ret = 0;
+	const char *pd_phandle = "qcom,dp-usbpd-detection";
+
+	dp_drv->pd = devm_usbpd_get_by_phandle(&dp_drv->pdev->dev,
+						    pd_phandle);
+
+	if (IS_ERR(dp_drv->pd)) {
+		pr_err("get_usbpd phandle failed (%ld)\n",
+				PTR_ERR(dp_drv->pd));
+		return PTR_ERR(dp_drv->pd);
+	}
+
+	dp_drv->svid_handler.svid = USB_C_DP_SID;
+	dp_drv->svid_handler.vdm_received = NULL;
+	dp_drv->svid_handler.connect = &usbpd_connect_callback;
+	dp_drv->svid_handler.svdm_received = &usbpd_response_callback;
+	dp_drv->svid_handler.disconnect = &usbpd_disconnect_callback;
+
+	ret = usbpd_register_svid(dp_drv->pd, &dp_drv->svid_handler);
+	if (ret) {
+		pr_err("usbpd registration failed\n");
+		return -ENODEV;
+	}
+
+	return ret;
+}
+
 static int mdss_dp_probe(struct platform_device *pdev)
 {
 	int ret, i;
@@ -1030,8 +1563,17 @@ static int mdss_dp_probe(struct platform_device *pdev)
 	dp_drv->mask1 = EDP_INTR_MASK1;
 	dp_drv->mask2 = EDP_INTR_MASK2;
 	mutex_init(&dp_drv->emutex);
+	mutex_init(&dp_drv->host_mutex);
+	mutex_init(&dp_drv->pd_msg_mutex);
 	spin_lock_init(&dp_drv->lock);
 
+	if (mdss_dp_usbpd_setup(dp_drv)) {
+		pr_err("Error usbpd setup!\n");
+		devm_kfree(&pdev->dev, dp_drv);
+		dp_drv = NULL;
+		return -EPROBE_DEFER;
+	}
+
 	ret = mdss_retrieve_dp_ctrl_resources(pdev, dp_drv);
 	if (ret)
 		goto probe_err;
@@ -1062,6 +1604,14 @@ static int mdss_dp_probe(struct platform_device *pdev)
 	if (ret)
 		goto probe_err;
 
+	ret = mdss_dp_clk_init(dp_drv,
+				&pdev->dev, true);
+	if (ret) {
+		DEV_ERR("clk_init failed.ret=%d\n",
+				ret);
+		goto probe_err;
+	}
+
 	ret = mdss_dp_irq_setup(dp_drv);
 	if (ret)
 		goto probe_err;
@@ -1070,33 +1620,33 @@ static int mdss_dp_probe(struct platform_device *pdev)
 	if (ret)
 		goto probe_err;
 
-	ret = mdss_dp_clk_ctrl(dp_drv, DP_CORE_PM, true);
+	dp_drv->cont_splash = dp_drv->mdss_util->panel_intf_status(DISPLAY_1,
+		MDSS_PANEL_INTF_EDP) ? true : false;
+
+	platform_set_drvdata(pdev, dp_drv);
+
+	ret = mdss_dp_pinctrl_init(pdev, dp_drv);
 	if (ret) {
-		pr_err("Unabled to enable core clocks\n");
+		pr_err("pinctrl init failed, ret=%d\n",
+						ret);
 		goto probe_err;
 	}
 
-	pr_info("ctrl_hw_rev =0x%x, phy hw_rev =0x%x\n",
-	       mdss_dp_get_ctrl_hw_version(&dp_drv->ctrl_io),
-	       mdss_dp_get_phy_hw_version(&dp_drv->phy_io));
-
-	ret = mdss_dp_clk_ctrl(dp_drv, DP_CORE_PM, false);
+	ret = mdss_dp_parse_gpio_params(pdev, dp_drv);
 	if (ret) {
-		pr_err("Unabled to disable core clocks\n");
+		pr_err("failed to parse gpio params, ret=%d\n",
+						ret);
 		goto probe_err;
 	}
 
-	dp_drv->cont_splash = dp_drv->mdss_util->panel_intf_status(DISPLAY_1,
-		MDSS_PANEL_INTF_EDP) ? true : false;
-
-	platform_set_drvdata(pdev, dp_drv);
-
 	mdss_dp_device_register(dp_drv);
 
 	dp_drv->inited = true;
 
 	pr_debug("done\n");
 
+	dp_send_events(dp_drv, EV_USBPD_DISCOVER_MODES);
+
 	return 0;
 
 probe_err:
diff --git a/drivers/video/fbdev/msm/mdss_dp.h b/drivers/video/fbdev/msm/mdss_dp.h
index b63318dcca06..008e7d687dbd 100644
--- a/drivers/video/fbdev/msm/mdss_dp.h
+++ b/drivers/video/fbdev/msm/mdss_dp.h
@@ -20,6 +20,7 @@
 #include <linux/pinctrl/consumer.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
+#include <linux/usb/usbpd.h>
 
 #include "mdss_hdmi_util.h"
 #include "video/msm_hdmi_modes.h"
@@ -103,10 +104,6 @@
 
 #define EDP_INTR_MASK2		(EDP_INTR_STATUS2 << 2)
 
-
-#define EDP_PHY_EDPPHY_GLB_VM_CFG0	0x510
-#define EDP_PHY_EDPPHY_GLB_VM_CFG1	0x514
-
 struct edp_cmd {
 	char read;	/* 1 == read, 0 == write */
 	char i2c;	/* 1 == i2c cmd, 0 == native cmd */
@@ -126,6 +123,10 @@ struct edp_buf {
 	char i2c;	/* 1 == i2c cmd, 0 == native cmd */
 };
 
+/* USBPD-TypeC specific Macros */
+#define VDM_VERSION		0x0
+#define USB_C_DP_SID		0xFF01
+
 enum dp_pm_type {
 	DP_CORE_PM,
 	DP_CTRL_PM,
@@ -133,6 +134,64 @@ enum dp_pm_type {
 	DP_MAX_PM
 };
 
+#define PIN_ASSIGN_A		BIT(0)
+#define PIN_ASSIGN_B		BIT(1)
+#define PIN_ASSIGN_C		BIT(2)
+#define PIN_ASSIGN_D		BIT(3)
+#define PIN_ASSIGN_E		BIT(4)
+#define PIN_ASSIGN_F		BIT(5)
+
+#define SVDM_HDR(svid, ver, mode, cmd_type, cmd) \
+	(((svid) << 16) | (1 << 15) | ((ver) <<  13) \
+	| ((mode) << 8) | ((cmd_type) << 6) | (cmd))
+
+/* DP specific VDM commands */
+#define DP_VDM_STATUS		0x10
+#define DP_VDM_CONFIGURE	0x11
+
+enum dp_port_cap {
+	PORT_NONE = 0,
+	PORT_UFP_D,
+	PORT_DFP_D,
+	PORT_D_UFP_D,
+};
+
+struct usbpd_dp_capabilities {
+	u32 response;
+	enum dp_port_cap s_port;
+	bool receptacle_state;
+	u8 ulink_pin_config;
+	u8 dlink_pin_config;
+};
+
+struct usbpd_dp_status {
+	u32 response;
+	enum dp_port_cap c_port;
+	bool low_pow_st;
+	bool adaptor_dp_en;
+	bool multi_func;
+	bool switch_to_usb_config;
+	bool exit_dp_mode;
+	bool hpd_high;
+	bool hpd_irq;
+};
+
+enum dp_alt_mode_state {
+	ALT_MODE_INIT_STATE = 0,
+	DISCOVER_MODES_DONE,
+	ENTER_MODE_DONE,
+	DP_STATUS_DONE,
+	DP_CONFIGURE_DONE,
+	UNKNOWN_STATE,
+};
+
+struct dp_alt_mode {
+	struct usbpd_dp_capabilities dp_cap;
+	struct usbpd_dp_status dp_status;
+	u32 usbpd_dp_config;
+	enum dp_alt_mode_state current_state;
+};
+
 #define DPCD_ENHANCED_FRAME	BIT(0)
 #define DPCD_TPS3	BIT(1)
 #define DPCD_MAX_DOWNSPREAD_0_5	BIT(2)
@@ -145,18 +204,26 @@ enum dp_pm_type {
 #define EV_DPCD_CAP_READ		BIT(2)
 #define EV_DPCD_STATUS_READ		BIT(3)
 #define EV_LINK_TRAIN			BIT(4)
-#define EV_IDLE_PATTERNS_SENT		BIT(30)
-#define EV_VIDEO_READY			BIT(31)
+#define EV_IDLE_PATTERNS_SENT		BIT(5)
+#define EV_VIDEO_READY			BIT(6)
 
-/* edp state ctrl */
+#define EV_USBPD_DISCOVER_MODES		BIT(7)
+#define EV_USBPD_ENTER_MODE		BIT(8)
+#define EV_USBPD_DP_STATUS		BIT(9)
+#define EV_USBPD_DP_CONFIGURE		BIT(10)
+#define EV_USBPD_CC_PIN_POLARITY	BIT(11)
+#define EV_USBPD_EXIT_MODE		BIT(12)
+
+/* dp state ctrl */
 #define ST_TRAIN_PATTERN_1		BIT(0)
 #define ST_TRAIN_PATTERN_2		BIT(1)
 #define ST_TRAIN_PATTERN_3		BIT(2)
-#define ST_SYMBOL_ERR_RATE_MEASUREMENT	BIT(3)
-#define ST_PRBS7			BIT(4)
-#define ST_CUSTOM_80_BIT_PATTERN	BIT(5)
-#define ST_SEND_VIDEO			BIT(6)
-#define ST_PUSH_IDLE			BIT(7)
+#define ST_TRAIN_PATTERN_4		BIT(3)
+#define ST_SYMBOL_ERR_RATE_MEASUREMENT	BIT(4)
+#define ST_PRBS7			BIT(5)
+#define ST_CUSTOM_80_BIT_PATTERN	BIT(6)
+#define ST_SEND_VIDEO			BIT(7)
+#define ST_PUSH_IDLE			BIT(8)
 
 /* sink power state  */
 #define SINK_POWER_ON		1
@@ -167,6 +234,8 @@ enum dp_pm_type {
 #define DP_LINK_RATE_540	20	/* 5.40G = 270M * 20 */
 #define DP_LINK_RATE_MAX	DP_LINK_RATE_540
 
+#define DP_LINK_RATE_MULTIPLIER	27000000
+
 struct dpcd_cap {
 	char major;
 	char minor;
@@ -254,10 +323,16 @@ struct dp_statistic {
 	u32 aux_native_rx;
 };
 
-
 #define DPCD_LINK_VOLTAGE_MAX	4
 #define DPCD_LINK_PRE_EMPHASIS_MAX	4
 
+struct dp_pinctrl_res {
+	struct pinctrl *pinctrl;
+	struct pinctrl_state *state_active;
+	struct pinctrl_state *state_hpd_active;
+	struct pinctrl_state *state_suspend;
+};
+
 irqreturn_t dp_isr(int irq, void *ptr);
 
 struct mdss_dp_drv_pdata {
@@ -266,6 +341,10 @@ struct mdss_dp_drv_pdata {
 	int (*off) (struct mdss_panel_data *pdata);
 	struct platform_device *pdev;
 
+	struct usbpd *pd;
+	struct usbpd_svid_handler svid_handler;
+	struct dp_alt_mode alt_mode;
+
 	struct mutex emutex;
 	int clk_cnt;
 	int cont_splash;
@@ -302,6 +381,11 @@ struct mdss_dp_drv_pdata {
 
 	/* regulators */
 	struct dss_module_power power_data[DP_MAX_PM];
+	struct dp_pinctrl_res pin_res;
+	int aux_sel_gpio;
+	int aux_en_gpio;
+	int usbplug_cc_gpio;
+	int hpd_gpio;
 	int clk_on;
 
 	/* hpd */
@@ -316,6 +400,9 @@ struct mdss_dp_drv_pdata {
 	struct completion video_comp;
 	struct mutex aux_mutex;
 	struct mutex train_mutex;
+	struct mutex host_mutex;
+	struct mutex pd_msg_mutex;
+	bool cable_connected;
 	u32 aux_cmd_busy;
 	u32 aux_cmd_i2c;
 	int aux_trans_num;
@@ -379,5 +466,6 @@ void mdss_dp_fill_link_cfg(struct mdss_dp_drv_pdata *ep);
 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);
 
 #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 39f11a8c35d1..7b14a7efb9dc 100644
--- a/drivers/video/fbdev/msm/mdss_dp_aux.c
+++ b/drivers/video/fbdev/msm/mdss_dp_aux.c
@@ -484,6 +484,37 @@ void dp_extract_edid_feature(struct edp_edid *edid, char *buf)
 			edid->dpm, edid->color_format);
 };
 
+char mdss_dp_gen_link_clk(struct mdss_panel_info *pinfo, char lane_cnt)
+{
+	const u32 encoding_factx10 = 8;
+	const u32 ln_to_link_ratio = 10;
+	u32 min_link_rate;
+	char calc_link_rate = 0;
+
+	pr_debug("clk_rate=%llu, bpp= %d, lane_cnt=%d\n",
+	       pinfo->clk_rate, pinfo->bpp, lane_cnt);
+	min_link_rate = (pinfo->clk_rate * 10) /
+		(lane_cnt * encoding_factx10);
+	min_link_rate = (min_link_rate * pinfo->bpp)
+				/ (DP_LINK_RATE_MULTIPLIER);
+	min_link_rate /= ln_to_link_ratio;
+
+	pr_debug("min_link_rate = %d\n", min_link_rate);
+
+	if (min_link_rate <= DP_LINK_RATE_162)
+		calc_link_rate = DP_LINK_RATE_162;
+	else if (min_link_rate <= DP_LINK_RATE_270)
+		calc_link_rate = DP_LINK_RATE_270;
+	else if (min_link_rate <= DP_LINK_RATE_540)
+		calc_link_rate = DP_LINK_RATE_540;
+	else {
+		pr_err("link_rate = %d is unsupported\n", min_link_rate);
+		calc_link_rate = 0;
+	}
+
+	return calc_link_rate;
+}
+
 void dp_extract_edid_detailed_timing_description(struct edp_edid *edid,
 		char *buf)
 {
@@ -1037,23 +1068,37 @@ char vm_voltage_swing[4][4] = {
 	{0x1E, 0xFF, 0xFF, 0xFF}  /* sw1, 1.2 v, optional */
 };
 
-static void dp_voltage_pre_emphasise_set(struct mdss_dp_drv_pdata *ep)
+static void dp_voltage_pre_emphasise_set(struct mdss_dp_drv_pdata *dp)
 {
 	u32 value0 = 0;
 	u32 value1 = 0;
 
-	pr_debug("v=%d p=%d\n", ep->v_level, ep->p_level);
+	pr_debug("v=%d p=%d\n", dp->v_level, dp->p_level);
 
-	value0 = vm_pre_emphasis[(int)(ep->v_level)][(int)(ep->p_level)];
-	value1 = vm_voltage_swing[(int)(ep->v_level)][(int)(ep->p_level)];
+	value0 = vm_voltage_swing[(int)(dp->v_level)][(int)(dp->p_level)];
+	value1 = vm_pre_emphasis[(int)(dp->v_level)][(int)(dp->p_level)];
 
+	/* Enable MUX to use Cursor values from these registers */
+	value0 |= BIT(5);
+	value1 |= BIT(5);
 	/* Configure host and panel only if both values are allowed */
 	if (value0 != 0xFF && value1 != 0xFF) {
-		dp_write(ep->base + EDP_PHY_EDPPHY_GLB_VM_CFG0, value0);
-		dp_write(ep->base + EDP_PHY_EDPPHY_GLB_VM_CFG1, value1);
+		dp_write(dp->phy_io.base +
+			QSERDES_TX0_OFFSET + TXn_TX_DRV_LVL,
+			value0);
+		dp_write(dp->phy_io.base +
+			QSERDES_TX1_OFFSET + TXn_TX_DRV_LVL,
+			value0);
+		dp_write(dp->phy_io.base +
+			QSERDES_TX0_OFFSET + TXn_TX_EMP_POST1_LVL,
+			value1);
+		dp_write(dp->phy_io.base +
+			QSERDES_TX1_OFFSET + TXn_TX_EMP_POST1_LVL,
+			value1);
+
 		pr_debug("value0=0x%x value1=0x%x",
 						value0, value1);
-		dp_lane_set_write(ep, ep->v_level, ep->p_level);
+		dp_lane_set_write(dp, dp->v_level, dp->p_level);
 	}
 
 }
@@ -1212,34 +1257,36 @@ static void dp_clear_training_pattern(struct mdss_dp_drv_pdata *ep)
 	usleep_range(usleep_time, usleep_time);
 }
 
-static int dp_aux_link_train(struct mdss_dp_drv_pdata *ep)
+static int dp_aux_link_train(struct mdss_dp_drv_pdata *dp)
 {
 	int ret = 0;
 	int usleep_time;
 
-	ret = dp_aux_chan_ready(ep);
+	ret = dp_aux_chan_ready(dp);
 	if (ret) {
 		pr_err("LINK Train failed: aux chan NOT ready\n");
-		complete(&ep->train_comp);
+		complete(&dp->train_comp);
 		return ret;
 	}
 
-	dp_write(ep->base + DP_MAINLINK_CTRL, 0x1);
+	dp_write(dp->base + DP_MAINLINK_CTRL, 0x1);
 
-	mdss_dp_sink_power_state(ep, SINK_POWER_ON);
+	mdss_dp_sink_power_state(dp, SINK_POWER_ON);
 
 train_start:
-	ep->v_level = 0; /* start from default level */
-	ep->p_level = 0;
-	dp_cap_lane_rate_set(ep);
-
-	dp_clear_training_pattern(ep);
-	usleep_time = ep->dpcd.training_read_interval;
+	dp->v_level = 0; /* start from default level */
+	dp->p_level = 0;
+	dp_cap_lane_rate_set(dp);
+	mdss_dp_config_ctrl(dp);
+
+	mdss_dp_state_ctrl(&dp->ctrl_io, 0);
+	dp_clear_training_pattern(dp);
+	usleep_time = dp->dpcd.training_read_interval;
 	usleep_range(usleep_time, usleep_time);
 
-	ret = dp_start_link_train_1(ep);
+	ret = dp_start_link_train_1(dp);
 	if (ret < 0) {
-		if (dp_link_rate_down_shift(ep) == 0) {
+		if (dp_link_rate_down_shift(dp) == 0) {
 			goto train_start;
 		} else {
 			pr_err("Training 1 failed\n");
@@ -1250,10 +1297,11 @@ train_start:
 
 	pr_debug("Training 1 completed successfully\n");
 
-	dp_clear_training_pattern(ep);
-	ret = dp_start_link_train_2(ep);
+	mdss_dp_state_ctrl(&dp->ctrl_io, 0);
+	dp_clear_training_pattern(dp);
+	ret = dp_start_link_train_2(dp);
 	if (ret < 0) {
-		if (dp_link_rate_down_shift(ep) == 0) {
+		if (dp_link_rate_down_shift(dp) == 0) {
 			goto train_start;
 		} else {
 			pr_err("Training 2 failed\n");
@@ -1264,10 +1312,16 @@ train_start:
 
 	pr_debug("Training 2 completed successfully\n");
 
+
 clear:
-	dp_clear_training_pattern(ep);
+	dp_clear_training_pattern(dp);
+	if (ret != -1) {
+		mdss_dp_setup_tr_unit(&dp->ctrl_io);
+		mdss_dp_state_ctrl(&dp->ctrl_io, ST_SEND_VIDEO);
+		pr_debug("State_ctrl set to SEND_VIDEO\n");
+	}
 
-	complete(&ep->train_comp);
+	complete(&dp->train_comp);
 	return ret;
 }
 
diff --git a/drivers/video/fbdev/msm/mdss_dp_util.c b/drivers/video/fbdev/msm/mdss_dp_util.c
index c1d29987a5fa..f7b27d1e56a1 100644
--- a/drivers/video/fbdev/msm/mdss_dp_util.c
+++ b/drivers/video/fbdev/msm/mdss_dp_util.c
@@ -214,14 +214,24 @@ void mdss_dp_sw_mvid_nvid(struct dss_io_data *ctrl_io)
 	writel_relaxed(0x3c, ctrl_io->base + DP_SOFTWARE_NVID);
 }
 
+void mdss_dp_setup_tr_unit(struct dss_io_data *ctrl_io)
+{
+	/* Current Tr unit configuration supports only 1080p */
+	writel_relaxed(0x21, ctrl_io->base + DP_MISC1_MISC0);
+	writel_relaxed(0x0f0016, ctrl_io->base + DP_VALID_BOUNDARY);
+	writel_relaxed(0x1f, ctrl_io->base + DP_TU);
+	writel_relaxed(0x0, ctrl_io->base + DP_VALID_BOUNDARY_2);
+}
+
 void mdss_dp_ctrl_lane_mapping(struct dss_io_data *ctrl_io,
 				struct lane_mapping l_map)
 {
 	u8 bits_per_lane = 2;
 	u32 lane_map = ((l_map.lane0 << (bits_per_lane * 0))
-			    || (l_map.lane1 << (bits_per_lane * 1))
-			    || (l_map.lane2 << (bits_per_lane * 2))
-			    || (l_map.lane3 << (bits_per_lane * 3)));
+			    | (l_map.lane1 << (bits_per_lane * 1))
+			    | (l_map.lane2 << (bits_per_lane * 2))
+			    | (l_map.lane3 << (bits_per_lane * 3)));
+	pr_debug("%s: lane mapping reg = 0x%x\n", __func__, lane_map);
 	writel_relaxed(lane_map,
 		ctrl_io->base + DP_LOGICAL2PHYSCIAL_LANE_MAPPING);
 }
@@ -282,3 +292,81 @@ void mdss_dp_irq_disable(struct mdss_dp_drv_pdata *dp_drv)
 
 	dp_drv->mdss_util->disable_irq(&mdss_dp_hw);
 }
+
+static void mdss_dp_initialize_s_port(enum dp_port_cap *s_port, int port)
+{
+	switch (port) {
+	case 0:
+		*s_port = PORT_NONE;
+		break;
+	case 1:
+		*s_port = PORT_UFP_D;
+		break;
+	case 2:
+		*s_port = PORT_DFP_D;
+		break;
+	case 3:
+		*s_port = PORT_D_UFP_D;
+		break;
+	default:
+		*s_port = PORT_NONE;
+	}
+}
+
+void mdss_dp_usbpd_ext_capabilities(struct usbpd_dp_capabilities *dp_cap)
+{
+	u32 buf = dp_cap->response;
+	int port = buf & 0x3;
+
+	dp_cap->receptacle_state =
+			(buf & BIT(6)) ? true : false;
+
+	dp_cap->dlink_pin_config =
+			(buf >> 8) & 0xff;
+
+	dp_cap->ulink_pin_config =
+			(buf >> 16) & 0xff;
+
+	mdss_dp_initialize_s_port(&dp_cap->s_port, port);
+}
+
+void mdss_dp_usbpd_ext_dp_status(struct usbpd_dp_status *dp_status)
+{
+	u32 buf = dp_status->response;
+	int port = buf & 0x3;
+
+	dp_status->low_pow_st =
+			(buf & BIT(2)) ? true : false;
+
+	dp_status->adaptor_dp_en =
+			(buf & BIT(3)) ? true : false;
+
+	dp_status->multi_func =
+			(buf & BIT(4)) ? true : false;
+
+	dp_status->switch_to_usb_config =
+			(buf & BIT(5)) ? true : false;
+
+	dp_status->exit_dp_mode =
+			(buf & BIT(6)) ? true : false;
+
+	dp_status->hpd_high =
+			(buf & BIT(7)) ? true : false;
+
+	dp_status->hpd_irq =
+			(buf & BIT(8)) ? true : false;
+
+	mdss_dp_initialize_s_port(&dp_status->c_port, port);
+}
+
+u32 mdss_dp_usbpd_gen_config_pkt(struct mdss_dp_drv_pdata *dp)
+{
+	u32 config = 0;
+
+	config |= (dp->alt_mode.dp_cap.dlink_pin_config << 8);
+	config |= (0x1 << 2); /* configure for DPv1.3 */
+	config |= 0x2; /* Configuring for UFP_D */
+
+	pr_debug("DP config = 0x%x\n", config);
+	return config;
+}
diff --git a/drivers/video/fbdev/msm/mdss_dp_util.h b/drivers/video/fbdev/msm/mdss_dp_util.h
index d9064cafad9a..8ef00dd7248e 100644
--- a/drivers/video/fbdev/msm/mdss_dp_util.h
+++ b/drivers/video/fbdev/msm/mdss_dp_util.h
@@ -48,10 +48,13 @@
 #define	DP_START_HOR_VER_FROM_SYNC		(0x00000420)
 #define	DP_HSYNC_VSYNC_WIDTH_POLARITY		(0x00000424)
 #define	DP_ACTIVE_HOR_VER			(0x00000428)
-
+#define	DP_MISC1_MISC0				(0x0000042C)
+#define	DP_VALID_BOUNDARY			(0x00000430)
+#define	DP_VALID_BOUNDARY_2			(0x00000434)
 #define	DP_LOGICAL2PHYSCIAL_LANE_MAPPING	(0x00000438)
 
 #define	DP_MAINLINK_READY			(0x00000440)
+#define	DP_TU					(0x0000044C)
 
 /*DP PHY Register offsets */
 #define DP_PHY_REVISION_ID0                     (0x00000000)
@@ -76,6 +79,12 @@
 #define DP_PHY_AUX_INTERRUPT_MASK               (0x00000044)
 #define DP_PHY_AUX_INTERRUPT_CLEAR              (0x00000048)
 
+#define QSERDES_TX0_OFFSET			0x0400
+#define QSERDES_TX1_OFFSET			0x0800
+
+#define TXn_TX_EMP_POST1_LVL			0x000C
+#define TXn_TX_DRV_LVL				0x001C
+
 #define TCSR_USB3_DP_PHYMODE			0x48
 
 struct lane_mapping {
@@ -85,6 +94,7 @@ struct lane_mapping {
 	char lane3;
 };
 
+void mdss_dp_state_ctrl(struct dss_io_data *ctrl_io, u32 data);
 u32 mdss_dp_get_ctrl_hw_version(struct dss_io_data *ctrl_io);
 u32 mdss_dp_get_phy_hw_version(struct dss_io_data *phy_io);
 void mdss_dp_aux_reset(struct dss_io_data *ctrl_io);
@@ -92,6 +102,7 @@ void mdss_dp_mainlink_reset(struct dss_io_data *ctrl_io);
 void mdss_dp_phy_reset(struct dss_io_data *ctrl_io);
 void mdss_dp_switch_usb3_phy_to_dp_mode(struct dss_io_data *tcsr_reg_io);
 void mdss_dp_assert_phy_reset(struct dss_io_data *ctrl_io, bool assert);
+void mdss_dp_setup_tr_unit(struct dss_io_data *ctrl_io);
 void mdss_dp_phy_aux_setup(struct dss_io_data *phy_io);
 void mdss_dp_hpd_configure(struct dss_io_data *ctrl_io, bool enable);
 void mdss_dp_aux_ctrl(struct dss_io_data *ctrl_io, bool enable);
@@ -107,5 +118,10 @@ int mdss_dp_irq_setup(struct mdss_dp_drv_pdata *dp_drv);
 void mdss_dp_irq_enable(struct mdss_dp_drv_pdata *dp_drv);
 void mdss_dp_irq_disable(struct mdss_dp_drv_pdata *dp_drv);
 void mdss_dp_sw_mvid_nvid(struct dss_io_data *ctrl_io);
+void mdss_dp_usbpd_ext_capabilities(struct usbpd_dp_capabilities *dp_cap);
+void mdss_dp_usbpd_ext_dp_status(struct usbpd_dp_status *dp_status);
+u32 mdss_dp_usbpd_gen_config_pkt(struct mdss_dp_drv_pdata *dp);
+void mdss_dp_ctrl_lane_mapping(struct dss_io_data *ctrl_io,
+					struct lane_mapping l_map);
 
 #endif /* __DP_UTIL_H__ */