TCPMv2: Prepare for boards using USB mux task

For boards using USB mux tasks, mux sets will be running in a task
parallel to the PD task.  This means we need to restructure any mux sets
which have required PD sequencing, such as setting USB safe state before
mode entry and DP configure.  Restructure the DPM and associated code to
account for the mux set taking some amount of time before VDMs may
proceed.

For the TBT module, align the module states to always indicate the
message the state is sending (ex. enter SOP will only send from
TBT_ENTER_SOP now).

This includes a couple of functional changes for boards:
- All boards will now set safe state before configuring DP pins, even if
their board functions previously did not
- TBT mode exit will now set safe state once before SOP exit, rather
than bouncing between safe state and USB mode after each exit ACK, which
matches more closely with how mode entry is currently working.
- USB4 exit will leave safe state set for TBT cable exit if active

BRANCH=None
BUG=b:172222942,b:186777984
TEST=on voxel with no mux task, pass tast typec.Mode*.manual

Signed-off-by: Diana Z <dzigterman@chromium.org>
Change-Id: I9d9fb6087199fba04084f57cefb31f3cb45c28e5
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3078417
Reviewed-by: Abe Levkoy <alevkoy@chromium.org>

8 files changed, 147 insertions, 84 deletions

diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
index d7048f4c8e..c7ad4fb4e7 100644
--- a/common/usb_pd_alt_mode_dfp.c
+++ b/common/usb_pd_alt_mode_dfp.c
@@ -965,6 +965,7 @@ enum tbt_compat_cable_speed get_tbt_cable_speed(int port)
 		max_tbt_speed : cable_tbt_speed;
 }
 
+/* Note: Assumes that pins have already been set in safe state */
 int enter_tbt_compat_mode(int port, enum tcpci_msg_type sop,
 			uint32_t *payload)
 {
@@ -985,13 +986,6 @@ int enter_tbt_compat_mode(int port, enum tcpci_msg_type sop,
 		     VDO_CMDT(CMDT_INIT) |
 		     VDO_SVDM_VERS(pd_get_vdo_ver(port, enter_mode_sop));
 
-	/*
-	 * Enter safe mode before sending Enter mode SOP/SOP'/SOP''
-	 * Ref: Tiger Lake Platform PD Controller Interface Requirements for
-	 * Integrated USB C, section A.1.2 TBT as DFP.
-	 */
-	usb_mux_set_safe_mode(port);
-
 	/* For TBT3 Cable Enter Mode Command, number of Objects is 1 */
 	if ((sop == TCPCI_MSG_SOP_PRIME) ||
 	    (sop == TCPCI_MSG_SOP_PRIME_PRIME))
@@ -1237,7 +1231,7 @@ __overridable uint8_t get_dp_pin_mode(int port)
 	return pd_dfp_dp_get_pin_mode(port, dp_status[port]);
 }
 
-static mux_state_t svdm_dp_get_mux_mode(int port)
+mux_state_t svdm_dp_get_mux_mode(int port)
 {
 	int pin_mode = get_dp_pin_mode(port);
 	/* Default dp_port_mf_allow is true */
@@ -1259,6 +1253,7 @@ static mux_state_t svdm_dp_get_mux_mode(int port)
 		return USB_PD_MUX_DP_ENABLED;
 }
 
+/* Note: Assumes that pins have already been set in safe state if necessary */
 __overridable int svdm_dp_config(int port, uint32_t *payload)
 {
 	int opos = pd_alt_mode(port, TCPCI_MSG_SOP, USB_SID_DISPLAYPORT);
@@ -1278,17 +1273,6 @@ __overridable int svdm_dp_config(int port, uint32_t *payload)
 
 	CPRINTS("pin_mode: %x, mf: %d, mux: %d", pin_mode, mf_pref, mux_mode);
 
-	/*
-	 * Place the USB Type-C pins that are to be re-configured to DisplayPort
-	 * Configuration into the Safe state. For USB_PD_MUX_DOCK, the
-	 * superspeed signals can remain connected. For USB_PD_MUX_DP_ENABLED,
-	 * disconnect the superspeed signals here, before the pins are
-	 * re-configured to DisplayPort (in svdm_dp_post_config, when we receive
-	 * the config ack).
-	 */
-	if (mux_mode == USB_PD_MUX_DP_ENABLED)
-		usb_mux_set_safe_mode(port);
-
 	payload[0] = VDO(USB_SID_DISPLAYPORT, 1,
 			 CMD_DP_CONFIG | VDO_OPOS(opos));
 	payload[1] = VDO_DP_CFG(pin_mode,      /* pin mode */
diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index de6fc63a60..30aa936f28 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -786,13 +786,31 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload,
 			}
 			break;
 		case CMD_DP_STATUS:
-			/* DP status response & UFP's DP attention have same
-			   payload */
+			/*
+			 * Note: DP status response & UFP's DP attention have
+			 * the same payload
+			 */
 			dfp_consume_attention(port, payload);
-			if (modep && modep->opos)
+
+			if (modep && modep->opos) {
+				/*
+				 * Place the USB Type-C pins that are to be
+				 * re-configured to DisplayPort Configuration
+				 * into the Safe state. For USB_PD_MUX_DOCK,
+				 * the superspeed signals can remain connected.
+				 * For USB_PD_MUX_DP_ENABLED, disconnect the
+				 * superspeed signals here, before the pins are
+				 * re-configured to DisplayPort (in
+				 * svdm_dp_post_config, when we receive the
+				 * config ack).
+				 */
+				if (svdm_dp_get_mux_mode(port) ==
+							USB_PD_MUX_DP_ENABLED)
+					usb_mux_set_safe_mode(port);
 				rsize = modep->fx->config(port, payload);
-			else
+			} else {
 				rsize = 0;
+			}
 			break;
 		case CMD_DP_CONFIG:
 			if (modep && modep->opos && modep->fx->post_config)
diff --git a/common/usbc/dp_alt_mode.c b/common/usbc/dp_alt_mode.c
index ecade62773..2a532466ac 100644
--- a/common/usbc/dp_alt_mode.c
+++ b/common/usbc/dp_alt_mode.c
@@ -12,6 +12,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include "assert.h"
+#include "atomic.h"
 #include "usb_common.h"
 #include "usb_dp_alt_mode.h"
 #include "usb_pd.h"
@@ -31,8 +32,10 @@ enum dp_states {
 	DP_ENTER_ACKED,
 	DP_ENTER_NAKED,
 	DP_STATUS_ACKED,
+	DP_PREPARE_CONFIG,
 	DP_ACTIVE,
 	DP_ENTER_RETRY,
+	DP_PREPARE_EXIT,
 	DP_INACTIVE,
 	DP_STATE_COUNT
 };
@@ -45,20 +48,31 @@ static enum dp_states dp_state[CONFIG_USB_PD_PORT_MAX_COUNT];
 static const uint8_t state_vdm_cmd[DP_STATE_COUNT] = {
 	[DP_START] = CMD_ENTER_MODE,
 	[DP_ENTER_ACKED] = CMD_DP_STATUS,
-	[DP_STATUS_ACKED] = CMD_DP_CONFIG,
-	[DP_ACTIVE] = CMD_EXIT_MODE,
-	[DP_ENTER_NAKED] = CMD_EXIT_MODE,
+	[DP_PREPARE_CONFIG] = CMD_DP_CONFIG,
+	[DP_PREPARE_EXIT] = CMD_EXIT_MODE,
 	[DP_ENTER_RETRY] = CMD_ENTER_MODE,
 };
 
+/*
+ * Track if we're retrying due to an Enter Mode NAK
+ */
+#define DP_FLAG_RETRY	BIT(0)
+
+static uint32_t dpm_dp_flags[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+#define DP_SET_FLAG(port, flag) atomic_or(&dpm_dp_flags[port], (flag))
+#define DP_CLR_FLAG(port, flag) atomic_clear_bits(&dpm_dp_flags[port], (flag))
+#define DP_CHK_FLAG(port, flag) (dpm_dp_flags[port] & (flag))
+
 bool dp_is_active(int port)
 {
-	return dp_state[port] == DP_ACTIVE;
+	return dp_state[port] == DP_ACTIVE || dp_state[port] == DP_PREPARE_EXIT;
 }
 
 void dp_init(int port)
 {
 	dp_state[port] = DP_START;
+	dpm_dp_flags[port] = 0;
 }
 
 bool dp_entry_is_done(int port)
@@ -71,6 +85,7 @@ static void dp_entry_failed(int port)
 {
 	CPRINTS("C%d: DP alt mode protocol failed!", port);
 	dp_state[port] = DP_INACTIVE;
+	dpm_dp_flags[port] = 0;
 }
 
 static bool dp_response_valid(int port, enum tcpci_msg_type type,
@@ -135,25 +150,23 @@ void dp_vdm_acked(int port, enum tcpci_msg_type type, int vdo_count,
 		dfp_consume_attention(port, vdm);
 		dp_state[port] = DP_STATUS_ACKED;
 		break;
-	case DP_STATUS_ACKED:
+	case DP_PREPARE_CONFIG:
 		if (modep && modep->opos && modep->fx->post_config)
 			modep->fx->post_config(port);
 		dp_state[port] = DP_ACTIVE;
 		CPRINTS("C%d: Entered DP mode", port);
 		break;
-	case DP_ACTIVE:
+	case DP_PREPARE_EXIT:
 		/*
 		 * Request to exit mode successful, so put the module in an
-		 * inactive state.
+		 * inactive state or give entry another shot.
 		 */
-		dp_exit_to_usb_mode(port);
-		break;
-	case DP_ENTER_NAKED:
-		/*
-		 * The request to exit the mode was successful,
-		 * so try to enter the mode again.
-		 */
-		dp_state[port] = DP_ENTER_RETRY;
+		if (DP_CHK_FLAG(port, DP_FLAG_RETRY)) {
+			dp_state[port] = DP_ENTER_RETRY;
+			DP_CLR_FLAG(port, DP_FLAG_RETRY);
+		} else {
+			dp_exit_to_usb_mode(port);
+		}
 		break;
 	case DP_INACTIVE:
 		/*
@@ -195,7 +208,7 @@ void dp_vdm_naked(int port, enum tcpci_msg_type type, uint8_t vdm_cmd)
 		 */
 		dp_entry_failed(port);
 		break;
-	case DP_ACTIVE:
+	case DP_PREPARE_EXIT:
 		/* Treat an Exit Mode NAK the same as an Exit Mode ACK. */
 		dp_exit_to_usb_mode(port);
 		break;
@@ -247,6 +260,26 @@ enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 		if (!(modep && modep->opos))
 			return MSG_SETUP_ERROR;
 
+		if (!get_dp_pin_mode(port))
+			return MSG_SETUP_ERROR;
+
+		dp_state[port] = DP_PREPARE_CONFIG;
+
+		/*
+		 * Place the USB Type-C pins that are to be re-configured to
+		 * DisplayPort Configuration into the Safe state. For
+		 * USB_PD_MUX_DOCK, the superspeed signals can remain
+		 * connected. For USB_PD_MUX_DP_ENABLED, disconnect the
+		 * superspeed signals here, before the pins are re-configured
+		 * to DisplayPort (in svdm_dp_post_config, when we receive
+		 * the config ack).
+		 */
+		if (svdm_dp_get_mux_mode(port) == USB_PD_MUX_DP_ENABLED) {
+			usb_mux_set_safe_mode(port);
+			return MSG_SETUP_MUX_WAIT;
+		}
+		/* Fall through if no mux set is needed */
+	case DP_PREPARE_CONFIG:
 		vdo_count_ret = modep->fx->config(port, vdm);
 		if (vdo_count_ret == 0)
 			return MSG_SETUP_ERROR;
@@ -254,6 +287,8 @@ enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPCI_MSG_SOP));
 		break;
 	case DP_ENTER_NAKED:
+		DP_SET_FLAG(port, DP_FLAG_RETRY);
+		/* Fall through to send exit mode */
 	case DP_ACTIVE:
 		/*
 		 * Called to exit DP alt mode, either when the mode
@@ -269,7 +304,10 @@ enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 			return MSG_SETUP_ERROR;
 
 		usb_mux_set_safe_mode_exit(port);
-
+		dp_state[port] = DP_PREPARE_EXIT;
+		return MSG_SETUP_MUX_WAIT;
+	case DP_PREPARE_EXIT:
+		/* DPM should call setup only after safe state is set */
 		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 eb64519c1e..5baf9d1a73 100644
--- a/common/usbc/tbt_alt_mode.c
+++ b/common/usbc/tbt_alt_mode.c
@@ -90,6 +90,8 @@ enum tbt_states {
 	TBT_START = 0,
 	TBT_ENTER_SOP,
 	TBT_ACTIVE,
+	/* Set to force Exit mode from non-Active states */
+	TBT_PREPARE_EXIT_MODE,
 	TBT_EXIT_SOP,
 	TBT_INACTIVE,
 	/* Active cable only */
@@ -103,7 +105,6 @@ static enum tbt_states tbt_state[CONFIG_USB_PD_PORT_MAX_COUNT];
 
 static const uint8_t state_vdm_cmd[TBT_STATE_COUNT] = {
 	[TBT_ENTER_SOP] = CMD_ENTER_MODE,
-	[TBT_ACTIVE] = CMD_EXIT_MODE,
 	[TBT_EXIT_SOP] = CMD_EXIT_MODE,
 	/* Active cable only */
 	[TBT_ENTER_SOP_PRIME] = CMD_ENTER_MODE,
@@ -299,27 +300,18 @@ void intel_vdm_acked(int port, enum tcpci_msg_type type, int vdo_count,
 		/* Indicate to PE layer that alt mode is active */
 		pd_set_dfp_enter_mode_flag(port, true);
 		break;
-	case TBT_ACTIVE:
+	case TBT_EXIT_SOP:
 		tbt_prints("exit mode SOP", port);
 		opos_sop = pd_alt_mode(port, TCPCI_MSG_SOP, USB_VID_INTEL);
 
 		/* Clear Thunderbolt related signals */
-		pd_dfp_exit_mode(port, TCPCI_MSG_SOP, USB_VID_INTEL, opos_sop);
-		set_usb_mux_with_current_data_role(port);
+		if (opos_sop > 0)
+			pd_dfp_exit_mode(port, TCPCI_MSG_SOP, USB_VID_INTEL,
+					 opos_sop);
 		if (get_usb_pd_cable_type(port) == IDH_PTYPE_ACABLE) {
 			tbt_active_cable_exit_mode(port);
 		} else {
-			/*
-			 * Exit Mode process is complete; go to inactive state.
-			 */
-			tbt_exit_done(port);
-		}
-		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 {
+			set_usb_mux_with_current_data_role(port);
 			if (TBT_CHK_FLAG(port, TBT_FLAG_RETRY_DONE))
 				/* retried enter mode, still failed, give up */
 				tbt_exit_done(port);
@@ -330,7 +322,6 @@ void intel_vdm_acked(int port, enum tcpci_msg_type type, int vdo_count,
 	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);
@@ -382,26 +373,15 @@ void intel_vdm_naked(int port, enum tcpci_msg_type type, uint8_t vdm_cmd)
 		 * so request to exit the mode first before retrying the enter
 		 * command. This can happen if the EC is restarted
 		 */
-		tbt_state[port] = TBT_EXIT_SOP;
-		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 {
-			tbt_prints("exit mode SOP failed", port);
-			tbt_state[port] = TBT_INACTIVE;
-			TBT_CLR_FLAG(port, TBT_FLAG_RETRY_DONE);
-		}
+		tbt_state[port] = TBT_PREPARE_EXIT_MODE;
 		break;
 	case TBT_EXIT_SOP:
 		/* Exit SOP got NAK'ed */
 		tbt_prints("exit mode SOP failed", port);
-		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 {
+			set_usb_mux_with_current_data_role(port);
 			if (TBT_CHK_FLAG(port, TBT_FLAG_RETRY_DONE))
 				/* Retried enter mode, still failed, give up */
 				tbt_exit_done(port);
@@ -410,7 +390,6 @@ void intel_vdm_naked(int port, enum tcpci_msg_type type, uint8_t vdm_cmd)
 		}
 		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;
@@ -481,23 +460,26 @@ enum dpm_msg_setup_status tbt_setup_next_vdm(int port, int *vdo_count,
 		else
 			tbt_prints("retry to enter mode", port);
 
+		/*
+		 * Enter safe mode before sending Enter mode SOP/SOP'/SOP''
+		 * Ref: Tiger Lake Platform PD Controller Interface
+		 * Requirements for Integrated USB C, section A.1.2 TBT as DFP.
+		 */
+		usb_mux_set_safe_mode(port);
+
 		cable_mode_resp.raw_value =
 			pd_get_tbt_mode_vdo(port, TCPCI_MSG_SOP_PRIME);
 
 		/* Active cable and LRD cables send Enter Mode SOP' first */
 		if (get_usb_pd_cable_type(port) == IDH_PTYPE_ACABLE ||
 		    cable_mode_resp.tbt_active_passive == TBT_CABLE_ACTIVE) {
-			vdo_count_ret = enter_tbt_compat_mode(port,
-						TCPCI_MSG_SOP_PRIME, vdm);
-			*tx_type = TCPCI_MSG_SOP_PRIME;
 			tbt_state[port] = TBT_ENTER_SOP_PRIME;
 		} else {
 			/* Passive cable send Enter Mode SOP */
-			vdo_count_ret =
-				enter_tbt_compat_mode(port, TCPCI_MSG_SOP, vdm);
 			tbt_state[port] = TBT_ENTER_SOP;
 		}
-		break;
+
+		return MSG_SETUP_MUX_WAIT;
 	case TBT_ENTER_SOP_PRIME:
 		vdo_count_ret =
 			enter_tbt_compat_mode(port, TCPCI_MSG_SOP_PRIME, vdm);
@@ -513,21 +495,31 @@ enum dpm_msg_setup_status tbt_setup_next_vdm(int port, int *vdo_count,
 		vdo_count_ret =
 			enter_tbt_compat_mode(port, TCPCI_MSG_SOP, vdm);
 		break;
-	case TBT_EXIT_SOP:
 	case TBT_ACTIVE:
 		/*
+		 * Since we had successfully entered mode, consider ourselves
+		 * done with any retires.
+		 */
+		TBT_SET_FLAG(port, TBT_FLAG_RETRY_DONE);
+		/* Fall through */
+	case TBT_PREPARE_EXIT_MODE:
+		/*
 		 * Called to exit Thunderbolt alt mode, either when the mode is
 		 * active and the system is shutting down, or when an initial
 		 * request to enter the mode is NAK'ed. This can happen if EC
 		 * is restarted while Thunderbolt mode is active.
 		 */
+		usb_mux_set_safe_mode_exit(port);
+
+		tbt_state[port] = TBT_EXIT_SOP;
+		return MSG_SETUP_MUX_WAIT;
+	case TBT_EXIT_SOP:
+		/* DPM will only call this after safe state set is done */
 		modep = pd_get_amode_data(port,
 					  TCPCI_MSG_SOP, USB_VID_INTEL);
 		if (!(modep && modep->opos))
 			return MSG_SETUP_ERROR;
 
-		usb_mux_set_safe_mode_exit(port);
-
 		vdm[0] = VDO(USB_VID_INTEL, 1, CMD_EXIT_MODE) |
 			VDO_OPOS(modep->opos) |
 			VDO_CMDT(CMDT_INIT) |
@@ -541,8 +533,6 @@ enum dpm_msg_setup_status tbt_setup_next_vdm(int port, int *vdo_count,
 		if (!(modep && modep->opos))
 			return MSG_SETUP_ERROR;
 
-		usb_mux_set_safe_mode_exit(port);
-
 		vdm[0] = VDO(USB_VID_INTEL, 1, CMD_EXIT_MODE) |
 			VDO_OPOS(modep->opos) |
 			VDO_CMDT(CMDT_INIT) |
@@ -557,8 +547,6 @@ enum dpm_msg_setup_status tbt_setup_next_vdm(int port, int *vdo_count,
 		if (!(modep && modep->opos))
 			return MSG_SETUP_ERROR;
 
-		usb_mux_set_safe_mode_exit(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_mode.c b/common/usbc/usb_mode.c
index b9dc4973bc..7a79ec7a7b 100644
--- a/common/usbc/usb_mode.c
+++ b/common/usbc/usb_mode.c
@@ -21,6 +21,7 @@
 #include "usb_pd_dpm.h"
 #include "usb_pd_tcpm.h"
 #include "usb_pe_sm.h"
+#include "usb_tbt_alt_mode.h"
 #include "usbc_ppc.h"
 
 #ifdef CONFIG_COMMON_RUNTIME
@@ -122,7 +123,9 @@ void usb4_exit_mode_request(int port)
 {
 	usb4_state[port] = USB4_START;
 	usb_mux_set_safe_mode_exit(port);
-	set_usb_mux_with_current_data_role(port);
+	/* If TBT mode is active, leave safe state for mode exit VDMs */
+	if (!tbt_is_active(port))
+		set_usb_mux_with_current_data_role(port);
 }
 
 void enter_usb_init(int port)
diff --git a/common/usbc/usb_pd_dpm.c b/common/usbc/usb_pd_dpm.c
index 3e399489b2..36cfdf0f75 100644
--- a/common/usbc/usb_pd_dpm.c
+++ b/common/usbc/usb_pd_dpm.c
@@ -18,6 +18,7 @@
 #include "tcpm/tcpm.h"
 #include "usb_dp_alt_mode.h"
 #include "usb_mode.h"
+#include "usb_mux.h"
 #include "usb_pd.h"
 #include "usb_pd_dpm.h"
 #include "usb_pd_tcpm.h"
@@ -278,6 +279,14 @@ static void dpm_attempt_mode_entry(int port)
 		return;
 	}
 
+	/*
+	 * If muxes are still settling, then wait on our next VDM.  We must
+	 * ensure we correctly sequence actions such as USB safe state with TBT
+	 * entry or DP configuration.
+	 */
+	if (IS_ENABLED(CONFIG_USBC_SS_MUX) && !usb_mux_set_completed(port))
+		return;
+
 	/* Check if port, port partner and cable support USB4. */
 	if (IS_ENABLED(CONFIG_USB_PD_USB4) &&
 	    board_is_tbt_usb4_port(port) &&
@@ -321,6 +330,10 @@ static void dpm_attempt_mode_entry(int port)
 		status = dp_setup_next_vdm(port, &vdo_count, vdm);
 	}
 
+	/* Not ready to send a VDM, check again next cycle */
+	if (status == MSG_SETUP_MUX_WAIT)
+		return;
+
 	/*
 	 * If the PE didn't discover any supported (requested) alternate mode,
 	 * just mark setup done and get out of here.
@@ -372,6 +385,15 @@ static void dpm_attempt_mode_exit(int port)
 		CPRINTS("C%d: USB4 teardown", port);
 		usb4_exit_mode_request(port);
 	}
+
+	/*
+	 * If muxes are still settling, then wait on our next VDM.  We must
+	 * ensure we correctly sequence actions such as USB safe state with TBT
+	 * or DP mode exit.
+	 */
+	if (IS_ENABLED(CONFIG_USBC_SS_MUX) && !usb_mux_set_completed(port))
+		return;
+
 	if (IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE) &&
 	    tbt_is_active(port)) {
 		/*
@@ -391,6 +413,7 @@ static void dpm_attempt_mode_exit(int port)
 		return;
 	}
 
+	/* This covers error, wait mux, and unsupported cases */
 	if (status != MSG_SETUP_SUCCESS)
 		return;
 
diff --git a/include/usb_pd.h b/include/usb_pd.h
index c9ead8ba33..ba6f3d99ff 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -3269,6 +3269,14 @@ __override_proto int svdm_dp_attention(int port, uint32_t *payload);
  */
 __override_proto void svdm_exit_dp_mode(int port);
 
+/**
+ * Get the DP mode that's desired on this port
+ *
+ * @param  port The PD port number
+ * @return USB_PD_MUX_DOCK or USB_PD_MUX_DP_ENABLED
+ */
+uint8_t svdm_dp_get_mux_mode(int port);
+
 /* Google Firmware Update Alternate Mode */
 /**
  * Enter Google Firmware Update (GFU) Mode.
diff --git a/include/usb_pd_dpm.h b/include/usb_pd_dpm.h
index 4a389b2116..18b73fca84 100644
--- a/include/usb_pd_dpm.h
+++ b/include/usb_pd_dpm.h
@@ -113,6 +113,7 @@ enum dpm_msg_setup_status {
 	MSG_SETUP_SUCCESS,
 	MSG_SETUP_ERROR,
 	MSG_SETUP_UNSUPPORTED,
+	MSG_SETUP_MUX_WAIT,
 };
 
 #endif  /* __CROS_EC_USB_DPM_H */