diff --git a/msm/dp/dp_altmode.c b/msm/dp/dp_altmode.c
index f274017a95..98d3a1e98e 100644
--- a/msm/dp/dp_altmode.c
+++ b/msm/dp/dp_altmode.c
@@ -40,13 +40,14 @@ enum dp_altmode_pin_assignment {
 	DPAM_HPD_F,
 };
 
-static int dp_altmode_release_ss_lanes(struct dp_altmode_private *altmode)
+static int dp_altmode_set_usb_dp_mode(struct dp_altmode_private *altmode)
 {
 	int rc;
 	struct device_node *np;
 	struct device_node *usb_node;
 	struct platform_device *usb_pdev;
 	int timeout = 250;
+	u32 lanes = 4;
 
 	if (!altmode || !altmode->dev) {
 		DP_ERR("invalid args\n");
@@ -55,6 +56,9 @@ static int dp_altmode_release_ss_lanes(struct dp_altmode_private *altmode)
 
 	np = altmode->dev->of_node;
 
+	if (altmode->connected && altmode->dp_altmode.base.force_multi_func)
+		lanes = 2;
+
 	usb_node = of_parse_phandle(np, "usb-controller", 0);
 	if (!usb_node) {
 		DP_ERR("unable to get usb node\n");
@@ -69,7 +73,7 @@ static int dp_altmode_release_ss_lanes(struct dp_altmode_private *altmode)
 	}
 
 	while (timeout) {
-		rc = dwc3_msm_release_ss_lane(&usb_pdev->dev);
+		rc = dwc3_msm_set_dp_mode(&usb_pdev->dev, altmode->connected, lanes);
 		if (rc != -EBUSY)
 			break;
 
@@ -150,6 +154,10 @@ static int dp_altmode_notify(void *priv, void *data, size_t len)
 			altmode->dp_altmode.base.orientation = ORIENTATION_NONE;
 			if (altmode->dp_cb && altmode->dp_cb->disconnect)
 				altmode->dp_cb->disconnect(altmode->dev);
+
+			rc = dp_altmode_set_usb_dp_mode(altmode);
+			if (rc)
+				DP_ERR("failed to clear usb dp mode, rc: %d\n", rc);
 		}
 		goto ack;
 	}
@@ -177,11 +185,9 @@ static int dp_altmode_notify(void *priv, void *data, size_t len)
 
 		altmode->dp_altmode.base.orientation = orientation;
 
-		if (!altmode->dp_altmode.base.multi_func) {
-			rc = dp_altmode_release_ss_lanes(altmode);
-			if (rc)
-				goto ack;
-		}
+		rc = dp_altmode_set_usb_dp_mode(altmode);
+		if (rc)
+			goto ack;
 
 		if (altmode->dp_cb && altmode->dp_cb->configure)
 			altmode->dp_cb->configure(altmode->dev);