TBT:Add support for exit mode SOP'/SOP'' on chipset transition

When chipset is transitioning to a new state or on sysjump, the EC
re-negotiates to enter an alternate mode on booting up.

This commit adds support for exiting Thunderbolt mode for SOP' and SOP''
for active cable on chipset transition and also moves all the alternate
mode exit rotines to their respective files.

It also delays deleting the SVID data until after the EXIT_MODE message
has ACKed and avoids pd_dfp_exit_mode() from changing the alternate
mode's internal states.

This commit also makes sure that the mux is set to safe state before
exiting the alternate mode and it is reconfigured according to the
port's current data role on receiving ACK/NAK from the cable/port
partner.

BUG=b:151169925, b:159717794
BRANCH=none
TEST=On reboot, able to exit and re-enter into DisplayPort mode,
     Thunderbolt mode with passive cable and thunderbolt mode with
     active cable.

Change-Id: If1e48e9f31cd678e23fe89bd3494551b5d1a78f1
Signed-off-by: Ayushee <ayushee.shah@intel.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2415082
Reviewed-by: Abe Levkoy <alevkoy@chromium.org>
Reviewed-by: Diana Z <dzigterman@chromium.org>

18 files changed, 107 insertions, 112 deletions

diff --git a/baseboard/kukui/usb_pd_policy.c b/baseboard/kukui/usb_pd_policy.c
index b2ff65b849..02e16ae624 100644
--- a/baseboard/kukui/usb_pd_policy.c
+++ b/baseboard/kukui/usb_pd_policy.c
@@ -261,7 +261,6 @@ __override int svdm_dp_attention(int port, uint32_t *payload)
 
 __override void svdm_exit_dp_mode(int port)
 {
-	svdm_safe_dp_mode(port);
 	gpio_set_level(GPIO_USB_C0_HPD_OD, 0);
 #ifdef VARIANT_KUKUI_DP_MUX_GPIO
 	board_set_dp_mux_control(0, 0);
diff --git a/baseboard/trogdor/usb_pd_policy.c b/baseboard/trogdor/usb_pd_policy.c
index f66ea1f276..b3f4b9877e 100644
--- a/baseboard/trogdor/usb_pd_policy.c
+++ b/baseboard/trogdor/usb_pd_policy.c
@@ -253,9 +253,6 @@ __override void svdm_exit_dp_mode(int port)
 	/* Disconnect the DP port selection mux. */
 	gpio_set_level(GPIO_DP_MUX_OE_L, 1);
 
-	/* Below svdm_safe_dp_mode() will disconnect SBU and DP/USB SS lines. */
-	svdm_safe_dp_mode(port);
-
 	/* Signal AP for the HPD low event */
 	usb_mux_hpd_update(port, 0, 0);
 	gpio_set_level(GPIO_DP_HOT_PLUG_DET, 0);
diff --git a/baseboard/zork/usb_pd_policy.c b/baseboard/zork/usb_pd_policy.c
index 513772213e..ad106c8b6b 100644
--- a/baseboard/zork/usb_pd_policy.c
+++ b/baseboard/zork/usb_pd_policy.c
@@ -148,13 +148,18 @@ __override void svdm_dp_post_config(int port)
 	usb_mux_hpd_update(port, 1, 0);
 }
 
-__override void svdm_exit_dp_mode(int port)
+__override void svdm_safe_dp_mode(int port)
 {
+	/* make DP interface safe until configure */
 	dp_flags[port] = 0;
 	dp_status[port] = 0;
 
 	usb_mux_set(port, USB_PD_MUX_NONE, USB_SWITCH_CONNECT,
 		    pd_get_polarity(port));
+}
+
+__override void svdm_exit_dp_mode(int port)
+{
 	gpio_or_ioex_set_level(PORT_TO_HPD(port), 0);
 
 	usb_mux_hpd_update(port, 0, 0);
diff --git a/board/asurada/usb_pd_policy.c b/board/asurada/usb_pd_policy.c
index 8cc8b0a922..c20d4f4746 100644
--- a/board/asurada/usb_pd_policy.c
+++ b/board/asurada/usb_pd_policy.c
@@ -146,7 +146,6 @@ __override int svdm_dp_attention(int port, uint32_t *payload)
 
 __override void svdm_exit_dp_mode(int port)
 {
-	svdm_safe_dp_mode(port);
 #ifdef CONFIG_USB_PD_DP_HPD_GPIO
 	svdm_set_hpd_gpio(port, 0);
 #endif /* CONFIG_USB_PD_DP_HPD_GPIO */
@@ -158,9 +157,6 @@ __override void svdm_exit_dp_mode(int port)
 	if (port == USB_PD_PORT_TCPC_MST)
 		baseboard_mst_enable_control(port, 0);
 #endif
-#ifdef CONFIG_USB_PD_TCPMV2
-	dp_teardown(port);
-#endif
 }
 
 int pd_snk_is_vbus_provided(int port)
diff --git a/board/elm/usb_pd_policy.c b/board/elm/usb_pd_policy.c
index 5cfc60650e..2eeab736d9 100644
--- a/board/elm/usb_pd_policy.c
+++ b/board/elm/usb_pd_policy.c
@@ -87,7 +87,6 @@ __override int svdm_dp_attention(int port, uint32_t *payload)
 
 __override void svdm_exit_dp_mode(int port)
 {
-	svdm_safe_dp_mode(port);
 	anx7688_hpd_disable(port);
 }
 #endif /* CONFIG_USB_PD_ALT_MODE_DFP */
diff --git a/board/flapjack/usb_pd_policy.c b/board/flapjack/usb_pd_policy.c
index 20de95dc3b..3603b6c931 100644
--- a/board/flapjack/usb_pd_policy.c
+++ b/board/flapjack/usb_pd_policy.c
@@ -145,7 +145,6 @@ __override int svdm_dp_attention(int port, uint32_t *payload)
 
 __override void svdm_exit_dp_mode(int port)
 {
-	svdm_safe_dp_mode(port);
 	gpio_set_level(GPIO_USB_C0_HPD_OD, 0);
 	gpio_set_level(GPIO_USB_C0_DP_OE_L, 1);
 	usb_mux_hpd_update(port, 0, 0);
diff --git a/board/oak/usb_pd_policy.c b/board/oak/usb_pd_policy.c
index bf2fca3f49..a4e62ebe88 100644
--- a/board/oak/usb_pd_policy.c
+++ b/board/oak/usb_pd_policy.c
@@ -101,7 +101,6 @@ __override int svdm_dp_attention(int port, uint32_t *payload)
 
 __override void svdm_exit_dp_mode(int port)
 {
-	svdm_safe_dp_mode(port);
 	board_typec_dp_off(port, dp_flags);
 }
 #endif /* CONFIG_USB_PD_ALT_MODE_DFP */
diff --git a/board/pdeval-stm32f072/usb_pd_policy.c b/board/pdeval-stm32f072/usb_pd_policy.c
index 78026c9e7b..f5bbf8d619 100644
--- a/board/pdeval-stm32f072/usb_pd_policy.c
+++ b/board/pdeval-stm32f072/usb_pd_policy.c
@@ -274,7 +274,6 @@ __override int svdm_dp_attention(int port, uint32_t *payload)
 
 __override void svdm_exit_dp_mode(int port)
 {
-	svdm_safe_dp_mode(port);
 #ifdef CONFIG_USB_PD_TCPM_ANX7447
 	anx7447_tcpc_clear_hpd_status(port);
 #endif
diff --git a/board/samus_pd/usb_pd_policy.c b/board/samus_pd/usb_pd_policy.c
index 9eed4f77b5..cdf234a743 100644
--- a/board/samus_pd/usb_pd_policy.c
+++ b/board/samus_pd/usb_pd_policy.c
@@ -147,6 +147,5 @@ __override int svdm_dp_attention(int port, uint32_t *payload)
 
 __override void svdm_exit_dp_mode(int port)
 {
-	svdm_safe_dp_mode(port);
 	gpio_set_level(PORT_TO_HPD(port), 0);
 }
diff --git a/common/mock/dp_alt_mode_mock.c b/common/mock/dp_alt_mode_mock.c
index bc67d57a31..0b68f4a9b3 100644
--- a/common/mock/dp_alt_mode_mock.c
+++ b/common/mock/dp_alt_mode_mock.c
@@ -29,9 +29,3 @@ void dp_init(int port)
 {
 	CPRINTS("C%d: DP init", port);
 }
-
-void dp_teardown(int port)
-{
-	CPRINTS("C%d: DP teardown", port);
-}
-
diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
index 75d7744597..aba8075a27 100644
--- a/common/usb_pd_alt_mode_dfp.c
+++ b/common/usb_pd_alt_mode_dfp.c
@@ -259,6 +259,7 @@ uint32_t pd_dfp_enter_mode(int port, enum tcpm_transmit_type type,
 	return VDO(modep->fx->svid, 1, CMD_ENTER_MODE | VDO_OPOS(modep->opos));
 }
 
+/* TODO(b/170372521) : Incorporate exit mode specific changes to DPM SM */
 int pd_dfp_exit_mode(int port, enum tcpm_transmit_type type, uint16_t svid,
 		int opos)
 {
@@ -1180,7 +1181,6 @@ __overridable int svdm_dp_attention(int port, uint32_t *payload)
 
 __overridable void svdm_exit_dp_mode(int port)
 {
-	svdm_safe_dp_mode(port);
 #ifdef CONFIG_USB_PD_DP_HPD_GPIO
 	svdm_set_hpd_gpio(port, 0);
 #endif /* CONFIG_USB_PD_DP_HPD_GPIO */
@@ -1189,9 +1189,6 @@ __overridable void svdm_exit_dp_mode(int port)
 	if (port == USB_PD_PORT_TCPC_MST)
 		baseboard_mst_enable_control(port, 0);
 #endif
-#ifdef CONFIG_USB_PD_TCPMV2
-	dp_teardown(port);
-#endif
 }
 
 __overridable int svdm_enter_gfu_mode(int port, uint32_t mode_caps)
@@ -1232,10 +1229,6 @@ __overridable int svdm_tbt_compat_enter_mode(int port, uint32_t mode_caps)
 
 __overridable void svdm_tbt_compat_exit_mode(int port)
 {
-	if (IS_ENABLED(CONFIG_USB_PD_TCPMV2)) {
-		usb_mux_set_safe_mode(port);
-		tbt_teardown(port);
-	}
 }
 
 __overridable int svdm_tbt_compat_status(int port, uint32_t *payload)
@@ -1254,6 +1247,10 @@ __overridable int svdm_tbt_compat_attention(int port, uint32_t *payload)
 }
 #endif /* CONFIG_USB_PD_TBT_COMPAT_MODE */
 
+/*
+ * TODO: b:169262276: For TCPMv2, move alternate mode specific entry, exit and
+ * configuration to Device Policy Manager.
+ */
 const struct svdm_amode_fx supported_modes[] = {
 	{
 		.svid = USB_SID_DISPLAYPORT,
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index 5b2b344e4d..fe125aa5db 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -822,7 +822,8 @@ static inline void set_state(int port, enum pd_states next_state)
 		charge_manager_update_dualrole(port, CAP_UNKNOWN);
 #endif
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
-		pd_dfp_exit_mode(port, TCPC_TX_SOP, 0, 0);
+		if (pd_dfp_exit_mode(port, TCPC_TX_SOP, 0, 0))
+			usb_mux_set_safe_mode(port);
 #endif
 		/*
 		 * Indicate that the port is disconnected by setting role to
@@ -1357,7 +1358,8 @@ void pd_execute_hard_reset(int port)
 	invalidate_last_message_id(port);
 	tcpm_set_rx_enable(port, 0);
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
-	pd_dfp_exit_mode(port, TCPC_TX_SOP, 0, 0);
+	if (pd_dfp_exit_mode(port, TCPC_TX_SOP, 0, 0))
+		usb_mux_set_safe_mode(port);
 #endif
 
 #ifdef CONFIG_USB_PD_REV30
@@ -2173,7 +2175,9 @@ static void exit_tbt_mode_sop_prime(int port)
 	 * type. TCPMv1 only uses one discovery structure, so all accesses
 	 * specify TCPC_TX_SOP.
 	 */
-	if (!pd_dfp_exit_mode(port, TCPC_TX_SOP, USB_VID_INTEL, opos))
+	if (pd_dfp_exit_mode(port, TCPC_TX_SOP, USB_VID_INTEL, opos))
+		usb_mux_set_safe_mode(port);
+	else
 		return;
 
 	header = PD_HEADER(PD_DATA_VENDOR_DEF, pd[port].power_role,
@@ -2381,6 +2385,7 @@ __maybe_unused static void exit_supported_alt_mode(int port)
 			    port, TCPC_TX_SOP, supported_modes[i].svid, opos)) {
 			CPRINTS("C%d Exiting ALT mode with SVID = 0x%x", port,
 				supported_modes[i].svid);
+			usb_mux_set_safe_mode(port);
 			pd_send_vdm(port, supported_modes[i].svid,
 				    CMD_EXIT_MODE | VDO_OPOS(opos), NULL, 0);
 			/* Wait for an ACK from port-partner */
diff --git a/common/usbc/dp_alt_mode.c b/common/usbc/dp_alt_mode.c
index cc5305c0c3..cd5eec1650 100644
--- a/common/usbc/dp_alt_mode.c
+++ b/common/usbc/dp_alt_mode.c
@@ -12,8 +12,9 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include "assert.h"
-#include "usb_pd.h"
+#include "usb_common.h"
 #include "usb_dp_alt_mode.h"
+#include "usb_pd.h"
 #include "usb_pd_dpm.h"
 #include "usb_pd_tcpm.h"
 
@@ -61,12 +62,6 @@ void dp_init(int port)
 	dp_state[port] = DP_START;
 }
 
-void dp_teardown(int port)
-{
-	CPRINTS("C%d: DP teardown", port);
-	dp_state[port] = DP_INACTIVE;
-}
-
 static void dp_entry_failed(int port)
 {
 	CPRINTS("C%d: DP alt mode protocol failed!", port);
@@ -101,6 +96,7 @@ void dp_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
 	const struct svdm_amode_data *modep =
 		pd_get_amode_data(port, type, USB_SID_DISPLAYPORT);
 	const uint8_t vdm_cmd = PD_VDO_CMD(vdm[0]);
+	int opos;
 
 	if (!dp_response_valid(port, type, "ACK", vdm_cmd))
 		return;
@@ -131,6 +127,12 @@ void dp_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
 		 */
 		CPRINTS("C%d: Exited DP mode", port);
 		dp_state[port] = DP_INACTIVE;
+		opos = pd_alt_mode(port, TCPC_TX_SOP, USB_SID_DISPLAYPORT);
+
+		/* Clear DisplayPort related signals */
+		pd_dfp_exit_mode(port, TCPC_TX_SOP, USB_SID_DISPLAYPORT,
+				     opos);
+		set_usb_mux_with_current_data_role(port);
 		break;
 	case DP_ENTER_NAKED:
 		/*
@@ -247,6 +249,7 @@ int dp_setup_next_vdm(int port, int vdo_count, uint32_t *vdm)
 		if (!(modep && modep->opos))
 			return -1;
 
+		svdm_safe_dp_mode(port);
 		vdm[0] = VDO(USB_SID_DISPLAYPORT,
 			     1, /* structured */
 			     CMD_EXIT_MODE);
diff --git a/common/usbc/tbt_alt_mode.c b/common/usbc/tbt_alt_mode.c
index 619d8a5b2e..4e65e7eff7 100644
--- a/common/usbc/tbt_alt_mode.c
+++ b/common/usbc/tbt_alt_mode.c
@@ -13,6 +13,7 @@
 #include "compile_time_macros.h"
 #include "console.h"
 #include "tcpm.h"
+#include "usb_common.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
 #include "usb_pd_dpm.h"
@@ -68,7 +69,9 @@
  * with a partner. It may be fixed in b/159495742, in which case this
  * logic is unneeded.
  */
+/* TODO:b/169169804 :Add entry, exit and retry flags for individual port */
 static bool retry_done;
+static bool exit_request;
 
 static int tbt_prints(const char *string, int port)
 {
@@ -110,24 +113,30 @@ void tbt_init(int port)
 
 bool tbt_is_active(int port)
 {
-	return tbt_state[port] == TBT_ACTIVE;
+	return tbt_state[port] != TBT_INACTIVE &&
+	       tbt_state[port] != TBT_START;
 }
 
-void tbt_teardown(int port)
+static void tbt_exit_done(int port)
 {
-	 tbt_prints("teardown", port);
-	 tbt_state[port] = TBT_INACTIVE;
-	 retry_done = false;
-}
-
-static void tbt_entry_failed(int port)
-{
-	tbt_prints("alt mode protocol failed!", port);
 	tbt_state[port] = TBT_INACTIVE;
+
+	if (exit_request) {
+		tbt_prints("Exited alternate mode", port);
+		return;
+	}
+
 	retry_done = false;
+	tbt_prints("alt mode protocol failed!", port);
 	dpm_set_mode_entry_done(port);
 }
 
+void tbt_exit_mode_request(void)
+{
+	retry_done = true;
+	exit_request = true;
+}
+
 static bool tbt_response_valid(int port, enum tcpm_transmit_type type,
 				char *cmdt, int vdm_cmd)
 {
@@ -142,7 +151,7 @@ static bool tbt_response_valid(int port, enum tcpm_transmit_type type,
 	if ((st != TBT_INACTIVE && state_vdm_cmd[st] != vdm_cmd) ||
 	    (get_usb_pd_cable_type(port) == IDH_PTYPE_PCABLE &&
 	     type != TCPC_TX_SOP)) {
-		tbt_entry_failed(port);
+		tbt_exit_done(port);
 		return false;
 	}
 	return true;
@@ -173,6 +182,7 @@ void intel_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
 {
 	struct pd_discovery *disc;
 	const uint8_t vdm_cmd = PD_VDO_CMD(vdm[0]);
+	int opos_sop, opos_sop_prime;
 
 	if (!tbt_response_valid(port, type, "ACK", vdm_cmd))
 		return;
@@ -197,38 +207,53 @@ void intel_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
 		tbt_prints("enter mode SOP", port);
 		break;
 	case TBT_ACTIVE:
-		if (get_usb_pd_cable_type(port) == IDH_PTYPE_ACABLE)
+		tbt_prints("exit mode SOP", port);
+		opos_sop = pd_alt_mode(port, TCPC_TX_SOP, USB_VID_INTEL);
+
+		/* Clear Thunderbolt related signals */
+		pd_dfp_exit_mode(port, TCPC_TX_SOP, USB_VID_INTEL, opos_sop);
+		set_usb_mux_with_current_data_role(port);
+		if (get_usb_pd_cable_type(port) == IDH_PTYPE_ACABLE) {
 			tbt_active_cable_exit_mode(port);
-		else {
+		} else {
 			/*
 			 * Exit Mode process is complete; go to inactive state.
 			 */
-			tbt_prints("exit mode SOP", port);
-			tbt_state[port] = TBT_INACTIVE;
 			retry_done = false;
 		}
 		break;
 	case TBT_EXIT_SOP:
+		set_usb_mux_with_current_data_role(port);
 		if (get_usb_pd_cable_type(port) == IDH_PTYPE_ACABLE)
 			tbt_active_cable_exit_mode(port);
 		else {
 			if (retry_done)
 				/* retried enter mode, still failed, give up */
-				tbt_entry_failed(port);
+				tbt_exit_done(port);
 			else
 				tbt_retry_enter_mode(port);
 		}
 		break;
 	case TBT_EXIT_SOP_PRIME_PRIME:
+		tbt_prints("exit mode SOP''", port);
 		tbt_state[port] = TBT_EXIT_SOP_PRIME;
+		set_usb_mux_with_current_data_role(port);
 		break;
 	case TBT_EXIT_SOP_PRIME:
+		tbt_prints("exit mode SOP'", port);
 		if (retry_done) {
 			/*
 			 * Exit mode process is complete; go to inactive state.
 			 */
-			tbt_prints("exit mode SOP'", port);
-			tbt_entry_failed(port);
+			tbt_exit_done(port);
+			opos_sop_prime =
+				pd_alt_mode(port, TCPC_TX_SOP_PRIME,
+					    USB_VID_INTEL);
+
+			/* Clear Thunderbolt related signals */
+			pd_dfp_exit_mode(port, TCPC_TX_SOP_PRIME, USB_VID_INTEL,
+				     opos_sop_prime);
+			set_usb_mux_with_current_data_role(port);
 		} else {
 			tbt_retry_enter_mode(port);
 		}
@@ -244,7 +269,7 @@ void intel_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
 		/* Invalid or unexpected negotiation state */
 		 CPRINTF("%s called with invalid state %d\n",
 				__func__, tbt_state[port]);
-		tbt_entry_failed(port);
+		tbt_exit_done(port);
 		break;
 	}
 }
@@ -268,6 +293,7 @@ void intel_vdm_naked(int port, enum tcpm_transmit_type type, uint8_t vdm_cmd)
 		break;
 	case TBT_ACTIVE:
 		/* Exit SOP got NAK'ed */
+		set_usb_mux_with_current_data_role(port);
 		if (get_usb_pd_cable_type(port) == IDH_PTYPE_ACABLE)
 			tbt_active_cable_exit_mode(port);
 		else {
@@ -278,27 +304,30 @@ void intel_vdm_naked(int port, enum tcpm_transmit_type type, uint8_t vdm_cmd)
 		break;
 	case TBT_EXIT_SOP:
 		/* Exit SOP got NAK'ed */
+		set_usb_mux_with_current_data_role(port);
 		if (get_usb_pd_cable_type(port) == IDH_PTYPE_ACABLE)
 			tbt_active_cable_exit_mode(port);
 		else {
 			if (retry_done)
 				/* Retried enter mode, still failed, give up */
-				tbt_entry_failed(port);
+				tbt_exit_done(port);
 			else
 				tbt_retry_enter_mode(port);
 		}
 		break;
 	case TBT_EXIT_SOP_PRIME_PRIME:
+		set_usb_mux_with_current_data_role(port);
 		tbt_prints("exit mode SOP'' failed", port);
 		tbt_state[port] = TBT_EXIT_SOP_PRIME;
 		break;
 	case TBT_EXIT_SOP_PRIME:
+		set_usb_mux_with_current_data_role(port);
 		if (retry_done) {
 			/*
 			 * Exit mode process is complete; go to inactive state.
 			 */
 			tbt_prints("exit mode SOP' failed", port);
-			tbt_entry_failed(port);
+			tbt_exit_done(port);
 		} else {
 			tbt_retry_enter_mode(port);
 		}
@@ -306,7 +335,7 @@ void intel_vdm_naked(int port, enum tcpm_transmit_type type, uint8_t vdm_cmd)
 	default:
 		CPRINTS("C%d: NAK for cmd %d in state %d", port,
 			vdm_cmd, tbt_state[port]);
-		tbt_entry_failed(port);
+		tbt_exit_done(port);
 		break;
 	}
 }
@@ -398,6 +427,7 @@ int tbt_setup_next_vdm(int port, int vdo_count, uint32_t *vdm,
 		if (!(modep && modep->opos))
 			return -1;
 
+		usb_mux_set_safe_mode(port);
 		vdm[0] = VDO(USB_VID_INTEL, 1, CMD_EXIT_MODE) |
 			VDO_OPOS(modep->opos) |
 			VDO_CMDT(CMDT_INIT) |
@@ -411,6 +441,7 @@ int tbt_setup_next_vdm(int port, int vdo_count, uint32_t *vdm,
 		if (!(modep && modep->opos))
 			return -1;
 
+		usb_mux_set_safe_mode(port);
 		vdm[0] = VDO(USB_VID_INTEL, 1, CMD_EXIT_MODE) |
 			VDO_OPOS(modep->opos) |
 			VDO_CMDT(CMDT_INIT) |
@@ -425,6 +456,7 @@ int tbt_setup_next_vdm(int port, int vdo_count, uint32_t *vdm,
 		if (!(modep && modep->opos))
 			return -1;
 
+		usb_mux_set_safe_mode(port);
 		vdm[0] = VDO(USB_VID_INTEL, 1, CMD_EXIT_MODE) |
 			VDO_OPOS(modep->opos) |
 			VDO_CMDT(CMDT_INIT) |
diff --git a/common/usbc/usb_pd_dpm.c b/common/usbc/usb_pd_dpm.c
index e9c7782512..5e65873e31 100644
--- a/common/usbc/usb_pd_dpm.c
+++ b/common/usbc/usb_pd_dpm.c
@@ -170,45 +170,32 @@ static void dpm_attempt_mode_entry(int port)
 
 static void dpm_attempt_mode_exit(int port)
 {
-	int opos;
-	uint16_t svid;
-	uint32_t vdm;
+	uint32_t vdm = 0;
+	int vdo_count = 0;
+	enum tcpm_transmit_type tx_type = TCPC_TX_SOP;
 
 	/* TODO(b/156749387): Support Data Reset for exiting USB4. */
 	if (IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE) &&
-	    tbt_is_active(port))
-		svid = USB_VID_INTEL;
-	else if (dp_is_active(port))
-		svid = USB_SID_DISPLAYPORT;
-	else {
+	    tbt_is_active(port)) {
+		CPRINTS("C%d: TBT teardown", port);
+		tbt_exit_mode_request();
+		vdo_count = tbt_setup_next_vdm(port, VDO_MAX_SIZE, &vdm,
+					&tx_type);
+	} else if (dp_is_active(port)) {
+		CPRINTS("C%d: DP teardown", port);
+		vdo_count = dp_setup_next_vdm(port, VDO_MAX_SIZE, &vdm);
+	} else {
 		/* Clear exit mode request */
 		dpm_clear_mode_exit_request(port);
 		return;
 	}
 
-	/*
-	 * TODO(b/148528713): Support cable plug Exit Mode (probably outsource
-	 * VDM construction to alt mode modules).
-	 */
-	opos = pd_alt_mode(port, TCPC_TX_SOP, svid);
-	if (opos > 0 && pd_dfp_exit_mode(port, TCPC_TX_SOP, svid, opos)) {
-		/*
-		 * TODO b/159717794: Delay deleting the data until after the
-		 * EXIT_MODE message is has ACKed. Unfortunately the callers
-		 * of this function expect the mode to be cleaned up before
-		 * return.
-		 */
-		vdm = VDO(svid, 1, /* Structured */
-			  VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPC_TX_SOP)) |
-			  VDO_OPOS(opos) | VDO_CMDT(CMDT_INIT) | CMD_EXIT_MODE);
-
-		if (!pd_setup_vdm_request(port, TCPC_TX_SOP, &vdm, 1)) {
-			dpm_clear_mode_exit_request(port);
-			return;
-		}
-
-		pd_dpm_request(port, DPM_REQUEST_VDM);
+	if (!pd_setup_vdm_request(port, tx_type, &vdm, vdo_count)) {
+		dpm_clear_mode_exit_request(port);
+		return;
 	}
+
+	pd_dpm_request(port, DPM_REQUEST_VDM);
 }
 
 void dpm_run(int port)
diff --git a/include/usb_dp_alt_mode.h b/include/usb_dp_alt_mode.h
index 4aceecd4ec..5a063e63a2 100644
--- a/include/usb_dp_alt_mode.h
+++ b/include/usb_dp_alt_mode.h
@@ -54,14 +54,6 @@ void dp_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
 void dp_vdm_naked(int port, enum tcpm_transmit_type type, uint8_t vdm_cmd);
 
 /*
- * Reset the DisplayPort VDM state for the specified port, as when exiting
- * DisplayPort mode.
- *
- * @param port USB-C port number
- */
-void dp_teardown(int port);
-
-/*
  * Construct the next DisplayPort VDM that should be sent.
  *
  * @param port      USB-C port number
diff --git a/include/usb_tbt_alt_mode.h b/include/usb_tbt_alt_mode.h
index fb3aadb481..11ad95f673 100644
--- a/include/usb_tbt_alt_mode.h
+++ b/include/usb_tbt_alt_mode.h
@@ -23,10 +23,15 @@
 void tbt_init(int port);
 
 /*
- * Returns True if Thunderbolt mode is in active state
+ * Sets exit_mode and retry_done bits to true
+ */
+void tbt_exit_mode_request(void);
+
+/*
+ * Returns True if Thunderbolt mode is not in inactive state
  *
  * @param port      USB-C port number
- * @return          True if Thunderbolt mode is in active state
+ * @return          True if Thunderbolt mode is not in inactive state
  *                  False otherwise
  */
 bool tbt_is_active(int port);
@@ -53,14 +58,6 @@ void intel_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
 void intel_vdm_naked(int port, enum tcpm_transmit_type type, uint8_t vdm_cmd);
 
 /*
- * Reset the Thunderbolt mode VDM state for the specified port, as when exiting
- * Thunderbolt mode.
- *
- * @param port USB-C port number
- */
-void tbt_teardown(int port);
-
-/*
  * Construct the next Thunderbolt VDM that should be sent.
  *
  * @param port      USB-C port number
diff --git a/test/fake_usbc.c b/test/fake_usbc.c
index 0f92379b38..dc513b5be9 100644
--- a/test/fake_usbc.c
+++ b/test/fake_usbc.c
@@ -253,10 +253,6 @@ void dpm_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
 {
 }
 
-void dp_teardown(int port)
-{
-}
-
 void dpm_vdm_naked(int port, enum tcpm_transmit_type type, uint16_t svid,
 		uint8_t vdm_cmd)
 {