TCPMv1: Add support for USB4.0

USB4 is based on the Thunderbolt 3 protocol specification. It
supports 40 Gbit/s throughput, is compatible with Thunderbolt 3,
and backwards compatible with USB 3.2, USB 2.0.

USB4.0 PD Flow:
Ref: USB Type-C Cable and Connector Specification 2.0
Figure 5-1 USB4 Discovery and Entry Flow Model

                  USB PD Explicit Contract
Discover ID SOP -------- USB4 compatible?
				|
    -------------yes------------|------No----- Exit USB4 Discovery
   |
Discover ID SOP' --------- Product type
                               |
        Passive cable----------|----Active Cable---USB4?
            |                                        |
            |                          (Not implemented in this CL)
       USB Signaling                    -----------------------
            |                           |                     |
            |             USB4 with USB4 active cable   Exit USB4
            |                                           Discovery
  ---------------------------------------------
 |                |              |             |
USB4 Gen3      USB3.2 Gen2   USB3.2 Gen1      USB2.0
 |                |              |             |
Enter USB4 with   |         Enter USB4 with   Exit USB4 Discovery
USB4 Gen3         |         USB4 Gen1
Passive cable     |         Passive cable
                  |
              DFP Gen3 capable?
                  |
    ------yes---- |---------No--------
    |                                |
Discover SVID SOP      Enter USB4 with USB3.2 Gen2 Passive Cable
    |
Discover SVID SOP'
    |
Discover Mode SOP
    |
Discover Mode SOP' --------Is TBT3?
                               |
                   -----yes----|-----No----
                   |                       |
        Enter USB4 with TBT3       Enter USB4 with TBT
        Gen3 passive cable         Gen2 passive cable

BUG=b:140819518
BRANCH=None
TEST=With Gatkex creek 3 device, TGLRVP can enter to USB4.0 mode

Change-Id: Id861661c66c53a0a32679388bb7e2e81aae3ceb5
Signed-off-by: Vijay Hiremath <vijay.p.hiremath@intel.com>
Signed-off-by: Ayushee <ayushee.shah@intel.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1926382
Reviewed-by: Diana Z <dzigterman@chromium.org>

9 files changed, 580 insertions, 28 deletions

diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index feb022d911..1b582b75d4 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -236,6 +236,86 @@ enum tbt_compat_rounded_support get_tbt_rounded_support(int port)
 	return cable[port].cable_mode_resp.tbt_rounded;
 }
 
+static enum usb_rev30_ss get_usb4_cable_speed(int port)
+{
+	if ((cable[port].rev == PD_REV30) &&
+	    (get_usb_pd_cable_type(port) == IDH_PTYPE_PCABLE) &&
+	   ((cable[port].attr.p_rev30.ss != USB_R30_SS_U32_U40_GEN2) ||
+	    !IS_ENABLED(CONFIG_USB_PD_TBT_GEN3_CAPABLE))) {
+		return cable[port].attr.p_rev30.ss;
+	}
+
+	/*
+	 * Converting Thunderolt-Compatible cable speed to equivalent USB4 cable
+	 * speed.
+	 */
+	return cable[port].cable_mode_resp.tbt_cable_speed == TBT_SS_TBT_GEN3 ?
+	       USB_R30_SS_U40_GEN3 : USB_R30_SS_U32_U40_GEN2;
+}
+
+uint32_t get_enter_usb_msg_payload(int port)
+{
+	/*
+	 * Ref: USB Power Delivery Specification Revision 3.0, Version 2.0
+	 * Table 6-47 Enter_USB Data Object
+	 */
+	union enter_usb_data_obj eudo;
+
+	if (!IS_ENABLED(CONFIG_USB_PD_USB4))
+		return 0;
+
+	eudo.mode = USB_PD_40;
+	eudo.usb4_drd_cap = IS_ENABLED(CONFIG_USB_PD_USB4);
+	eudo.usb3_drd_cap = IS_ENABLED(CONFIG_USB_PD_USB32);
+	eudo.cable_speed = get_usb4_cable_speed(port);
+
+	if ((cable[port].rev == PD_REV30) &&
+	    (get_usb_pd_cable_type(port) == IDH_PTYPE_ACABLE)) {
+		eudo.cable_type = (cable[port].attr2.a2_rev30.active_elem ==
+			ACTIVE_RETIMER) ? CABLE_TYPE_ACTIVE_RETIMER :
+			CABLE_TYPE_ACTIVE_REDRIVER;
+	/* TODO: Add eudo.cable_type for Revisiosn 2 active cables */
+	} else {
+		eudo.cable_type = CABLE_TYPE_PASSIVE;
+	}
+
+	switch (cable[port].attr.p_rev20.vbus_cur) {
+	case USB_VBUS_CUR_3A:
+		eudo.cable_current = USB4_CABLE_CURRENT_3A;
+		break;
+	case USB_VBUS_CUR_5A:
+		eudo.cable_current = USB4_CABLE_CURRENT_5A;
+		break;
+	default:
+		eudo.cable_current = USB4_CABLE_CURRENT_INVALID;
+		break;
+	}
+	eudo.pcie_supported = IS_ENABLED(CONFIG_USB_PD_PCIE_TUNNELING);
+	eudo.dp_supported = IS_ENABLED(CONFIG_USB_PD_ALT_MODE_DFP);
+	eudo.tbt_supported = IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE);
+	eudo.host_present = 1;
+
+	return eudo.raw_value;
+}
+
+bool should_enter_usb4_mode(int port)
+{
+	return IS_ENABLED(CONFIG_USB_PD_USB4) &&
+		cable[port].flags & CABLE_FLAGS_ENTER_USB_MODE;
+}
+
+void enable_enter_usb4_mode(int port)
+{
+	if (IS_ENABLED(CONFIG_USB_PD_USB4))
+		cable[port].flags |= CABLE_FLAGS_ENTER_USB_MODE;
+}
+
+void disable_enter_usb4_mode(int port)
+{
+	if (IS_ENABLED(CONFIG_USB_PD_USB4))
+		cable[port].flags &= ~CABLE_FLAGS_ENTER_USB_MODE;
+}
+
 #ifdef CONFIG_USB_PD_ALT_MODE
 
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
@@ -376,6 +456,118 @@ static inline bool is_limit_tbt_cable_speed(int port)
 	return !!(cable[port].flags & CABLE_FLAGS_TBT_COMPAT_LIMIT_SPEED);
 }
 
+static inline bool is_usb4_mode_enabled(int port)
+{
+	return (IS_ENABLED(CONFIG_USB_PD_USB4) &&
+	       (cable[port].flags & CABLE_FLAGS_USB4_CAPABLE));
+}
+
+static inline void enable_usb4_mode(int port)
+{
+	if (IS_ENABLED(CONFIG_USB_PD_USB4))
+		cable[port].flags |= CABLE_FLAGS_USB4_CAPABLE;
+}
+
+static inline void disable_usb4_mode(int port)
+{
+	if (IS_ENABLED(CONFIG_USB_PD_USB4))
+		cable[port].flags &= ~CABLE_FLAGS_USB4_CAPABLE;
+}
+
+/*
+ * Ref: USB Type-C Cable and Connector Specification
+ * Figure 5-1 USB4 Discovery and Entry Flow Model.
+ *
+ * Note: USB Type-C Cable and Connector Specification
+ * doesn't include details for Revision 2 cables.
+ *
+ *                         Passive Cable
+ *                                |
+ *                -----------------------------------
+ *                |                                 |
+ *           Revision 2                        Revision 3
+ *          USB Signalling                   USB Signalling
+ *             |                                     |
+ *     ------------------            -------------------------
+ *     |       |        |            |       |       |       |
+ * USB2.0   USB3.1    USB3.1       USB3.2   USB4   USB3.2   USB2
+ *   |      Gen1      Gen1 Gen2    Gen2     Gen3   Gen1       |
+ *   |       |          |           |        |       |       Exit
+ *   --------           ------------         --------        USB4
+ *      |                    |                  |          Discovery.
+ *    Exit          Is DFP Gen3 Capable?     Enter USB4
+ *    USB4                  |                with respective
+ *   Discovery.   --- No ---|--- Yes ---     cable speed.
+ *                |                    |
+ *    Enter USB4 with             Is Cable TBT3
+ *    respective cable                 |
+ *    speed.                 --- No ---|--- Yes ---
+ *                           |                    |
+ *                   Enter USB4 with        Enter USB4 with
+ *                   TBT Gen2 passive       TBT Gen3 passive
+ *                   cable.                 cable.
+ *
+ */
+static bool is_cable_ready_to_enter_usb4(int port, int cnt)
+{
+	/* TODO: USB4 enter mode for Active cables */
+	if (IS_ENABLED(CONFIG_USB_PD_USB4) &&
+	   (get_usb_pd_cable_type(port) == IDH_PTYPE_PCABLE) &&
+	    is_vdo_present(cnt, VDO_INDEX_PTYPE_CABLE1)) {
+		switch (cable[port].rev) {
+		case PD_REV30:
+			switch (cable[port].attr.p_rev30.ss) {
+			case USB_R30_SS_U40_GEN3:
+			case USB_R30_SS_U32_U40_GEN1:
+				return true;
+			case USB_R30_SS_U32_U40_GEN2:
+				/* Check if DFP is Gen 3 capable */
+				if (IS_ENABLED(CONFIG_USB_PD_TBT_GEN3_CAPABLE))
+					return false;
+				return true;
+			default:
+				disable_usb4_mode(port);
+				return false;
+			}
+		case PD_REV20:
+			switch (cable[port].attr.p_rev20.ss) {
+			case USB_R20_SS_U31_GEN1_GEN2:
+				/* Check if DFP is Gen 3 capable */
+				if (IS_ENABLED(CONFIG_USB_PD_TBT_GEN3_CAPABLE))
+					return false;
+				return true;
+			default:
+				disable_usb4_mode(port);
+				return false;
+		}
+		default:
+			disable_usb4_mode(port);
+		}
+	}
+	return false;
+}
+
+static bool is_usb4_vdo(int port, int cnt, uint32_t *payload)
+{
+	enum idh_ptype ptype = PD_IDH_PTYPE(payload[VDO_I(PRODUCT)]);
+
+	/*
+	 * Product types Hub and peripheral should use UFP product vdos
+	 * Reference Table 6-30 USB PD spec 3.2.
+	 */
+	if (ptype == IDH_PTYPE_HUB || ptype == IDH_PTYPE_PERIPH) {
+		/*
+		 * Ref: USB Type-C Cable and Connector Specification
+		 * Figure 5-1 USB4 Discovery and Entry Flow Model
+		 * Device USB4 VDO detection.
+		 */
+		return IS_ENABLED(CONFIG_USB_PD_USB4) &&
+			is_vdo_present(cnt, VDO_INDEX_PTYPE_UFP1_VDO) &&
+			PD_PRODUCT_IS_USB4(payload[VDO_INDEX_PTYPE_UFP1_VDO]);
+	}
+	return false;
+}
+
 void pd_dfp_pe_init(int port)
 {
 	memset(&pe[port], 0, sizeof(struct pd_policy));
@@ -508,6 +700,20 @@ static void dfp_consume_modes(int port, int cnt, uint32_t *payload)
 	pe[port].svid_idx++;
 }
 
+/*
+ * This function returns
+ * True - If the THunderbolt cable speed is TBT_SS_TBT_GEN3 or
+ *        TBT_SS_U32_GEN1_GEN2
+ * False - Otherwise
+ */
+static bool check_tbt_cable_speed(int port)
+{
+	return (cable[port].cable_mode_resp.tbt_cable_speed ==
+						TBT_SS_TBT_GEN3 ||
+		cable[port].cable_mode_resp.tbt_cable_speed ==
+						TBT_SS_U32_GEN1_GEN2);
+}
+
 static int get_mode_idx(int port, uint16_t svid)
 {
 	int i;
@@ -909,12 +1115,27 @@ static int process_tbt_compat_discover_modes(int port, uint32_t *payload)
 		}
 
 		/*
-		 * Enter Mode SOP' (Cable Enter Mode) is skipped for
-		 * passive cables.
+		 * Enter Mode SOP' (Cable Enter Mode) and Enter USB SOP' is
+		 * skipped for passive cables.
 		 */
 		if (get_usb_pd_cable_type(port) == IDH_PTYPE_PCABLE)
 			disable_transmit_sop_prime(port);
 
+		if (is_usb4_mode_enabled(port)) {
+			/*
+			 * If Cable is not Thunderbolt Gen 3
+			 * capable or Thunderbolt Gen1_Gen2
+			 * capable, disable USB4 mode and
+			 * continue flow for
+			 * Thunderbolt-compatible mode
+			 */
+			if (check_tbt_cable_speed(port)) {
+				enable_enter_usb4_mode(port);
+				usb_mux_set_safe_mode(port);
+				return 0;
+			}
+			disable_usb4_mode(port);
+		}
 		rsize = enter_tbt_compat_mode(port, payload);
 	} else {
 		/* Store Discover Mode SOP response */
@@ -1025,27 +1246,63 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload,
 				/* Store cable type */
 				dfp_consume_cable_response(port, cnt, payload,
 							head);
+
+				/*
+				 * Enter USB4 mode if the cable supports USB4
+				 * operation and has USB4 VDO.
+				 */
+				if (is_usb4_mode_enabled(port) &&
+				    is_cable_ready_to_enter_usb4(port, cnt)) {
+					enable_enter_usb4_mode(port);
+					usb_mux_set_safe_mode(port);
+					disable_transmit_sop_prime(port);
+					/*
+					 * To change the mode of operation from
+					 * USB4 the port needs to be
+					 * reconfigured.
+					 * Ref: USB Type-C Cable and Connectot
+					 * Specification section 5.4.4.
+					 *
+					 */
+					disable_tbt_compat_mode(port);
+					rsize = 0;
+					break;
+				}
+
 				/*
 				 * Disable Thunderbolt-compatible mode if the
 				 * cable does not support superspeed
 				 */
 				if (is_tbt_compat_enabled(port) &&
-					!is_tbt_cable_superspeed(port))
+					!is_tbt_cable_superspeed(port)) {
 					disable_tbt_compat_mode(port);
+				}
 
 				rsize = dfp_discover_svids(payload);
+
 				disable_transmit_sop_prime(port);
 			/* Received a SOP Discover Ident Message */
 			} else if (IS_ENABLED(CONFIG_USB_PD_DECODE_SOP) &&
 				board_is_tbt_usb4_port(port)) {
 				dfp_consume_identity(port, cnt, payload);
 
+				/* Enable USB4 mode if USB4 VDO present
+				 * and port partner supports USB Rev 3.0.
+				 */
+				if (is_usb4_vdo(port, cnt, payload) &&
+				    PD_HEADER_REV(head)	== PD_REV30) {
+					enable_usb4_mode(port);
+				}
+
 				/*
 				 * Enable Thunderbolt-compatible mode
 				 * if the modal operation is supported
 				 */
-				if (is_modal(port, cnt, payload)) {
+				if (is_modal(port, cnt, payload))
 					enable_tbt_compat_mode(port);
+
+				if (is_modal(port, cnt, payload) ||
+				    is_usb4_vdo(port, cnt, payload)) {
 					rsize = dfp_discover_ident(payload);
 					enable_transmit_sop_prime(port);
 				} else {
@@ -1092,6 +1349,7 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload,
 			}
 
 			rsize = dfp_discover_modes(port, payload);
+
 			disable_transmit_sop_prime(port);
 			}
 			break;
@@ -1104,10 +1362,15 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload,
 				break;
 			}
 
-			disable_tbt_compat_mode(port);
 			rsize = dfp_discover_modes(port, payload);
 			/* enter the default mode for DFP */
 			if (!rsize) {
+				/*
+				 * Disabling Thunderbolt-Compatible mode if
+				 * discover mode response doesn't include Intel
+				 * SVID.
+				 */
+				disable_tbt_compat_mode(port);
 				payload[0] = pd_dfp_enter_mode(port, 0, 0);
 				if (payload[0])
 					rsize = 1;
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index a73175a725..6e29bf06b8 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -295,6 +295,7 @@ static const char * const pd_state_names[] = {
 	"SOFT_RESET", "HARD_RESET_SEND", "HARD_RESET_EXECUTE", "BIST_RX",
 	"BIST_TX",
 	"DRP_AUTO_TOGGLE",
+	"ENTER_USB",
 };
 BUILD_ASSERT(ARRAY_SIZE(pd_state_names) == PD_STATE_COUNT);
 #endif
@@ -1689,6 +1690,12 @@ static void handle_data_request(int port, uint16_t head,
 #ifdef CONFIG_USB_PD_REV30
 	case PD_DATA_BATTERY_STATUS:
 		break;
+	/* TODO : Add case PD_DATA_RESET for exiting USB4 */
+
+	/*
+	 * TODO : Add case PD_DATA_ENTER_USB to accept or reject
+	 * Enter_USB request from port partner.
+	 */
 #endif
 	case PD_DATA_VENDOR_DEF:
 		handle_vdm_request(port, cnt, payload, head);
@@ -1856,6 +1863,21 @@ static void handle_ctrl_request(int port, uint16_t head,
 		break;
 #endif
 	case PD_CTRL_REJECT:
+		if (pd[port].task_state == PD_STATE_ENTER_USB) {
+			if (!IS_ENABLED(CONFIG_USBC_SS_MUX))
+				break;
+
+			/*
+			 * Since Enter USB sets the mux state to SAFE mode,
+			 * resetting the mux state back to USB mode on
+			 * recieveing a NACK.
+			 */
+			usb_mux_set(port, USB_PD_MUX_USB_ENABLED,
+				USB_SWITCH_CONNECT, pd[port].polarity);
+
+			set_state(port, READY_RETURN_STATE(port));
+			break;
+		}
 	case PD_CTRL_WAIT:
 		if (pd[port].task_state == PD_STATE_DR_SWAP) {
 			if (type == PD_CTRL_WAIT) /* try again ... */
@@ -1918,7 +1940,20 @@ static void handle_ctrl_request(int port, uint16_t head,
 #endif
 		break;
 	case PD_CTRL_ACCEPT:
-		if (pd[port].task_state == PD_STATE_SOFT_RESET) {
+		if (pd[port].task_state == PD_STATE_ENTER_USB) {
+			if (!IS_ENABLED(CONFIG_USBC_SS_MUX))
+				break;
+
+			/* Connect the SBU and USB lines to the connector */
+			if (IS_ENABLED(CONFIG_USBC_PPC_SBU))
+				ppc_set_sbu(port, 1);
+
+			/* Set usb mux to USB4 mode */
+			usb_mux_set(port, USB_PD_MUX_USB4_ENABLED,
+				USB_SWITCH_CONNECT, pd[port].polarity);
+
+			set_state(port, READY_RETURN_STATE(port));
+		} else if (pd[port].task_state == PD_STATE_SOFT_RESET) {
 			/*
 			 * For the case that we sent soft reset in SNK_DISCOVERY
 			 * on startup due to VBUS never low, clear the flag.
@@ -2836,6 +2871,46 @@ void pd_interrupt_handler_task(void *p)
 }
 #endif /* HAS_TASK_PD_INT_C0 || HAS_TASK_PD_INT_C1 || HAS_TASK_PD_INT_C2 */
 
+static void pd_send_enter_usb(int port, int *timeout)
+{
+	uint32_t usb4_payload = get_enter_usb_msg_payload(port);
+	uint16_t header;
+	int res;
+
+	/*
+	 * TODO: Enable Enter USB for cables (SOP').
+	 * This is needed for active cables
+	 */
+	if (!IS_ENABLED(CONFIG_USBC_SS_MUX) || !IS_ENABLED(CONFIG_USB_PD_USB4))
+		return;
+
+	header = PD_HEADER(PD_DATA_ENTER_USB,
+		pd[port].power_role,
+		pd[port].data_role,
+		pd[port].msg_id,
+		1,
+		PD_REV30,
+		0);
+
+	res = pd_transmit(port, TCPC_TX_SOP, header, &usb4_payload, AMS_START);
+	if (res < 0) {
+		*timeout = 10*MSEC;
+		/*
+		 * If failed to get goodCRC, send soft reset, otherwise ignore
+		 * failure.
+		 */
+		set_state(port, res == -1 ?
+			   PD_STATE_SOFT_RESET :
+			   READY_RETURN_STATE(port));
+		return;
+	}
+
+	/* Disable Enter USB4 mode prevent re-entry */
+	disable_enter_usb4_mode(port);
+
+	set_state(port, PD_STATE_ENTER_USB);
+}
+
 void pd_task(void *u)
 {
 	int head;
@@ -3699,6 +3774,15 @@ void pd_task(void *u)
 				break;
 			}
 
+			/*
+			 * Enter_USB if port partner and cable are
+			 * USB4 compatible.
+			 */
+			if (should_enter_usb4_mode(port)) {
+				pd_send_enter_usb(port, &timeout);
+				break;
+			}
+
 			if (!(pd[port].flags & PD_FLAGS_PING_ENABLED))
 				break;
 
@@ -4312,6 +4396,15 @@ void pd_task(void *u)
 				break;
 			}
 
+			/*
+			 * Enter_USB if port partner and cable are
+			 * USB4 compatible.
+			 */
+			if (should_enter_usb4_mode(port)) {
+				pd_send_enter_usb(port, &timeout);
+				break;
+			}
+
 			/* Sent all messages, don't need to wake very often */
 			timeout = 200*MSEC;
 			break;
@@ -4687,6 +4780,13 @@ void pd_task(void *u)
 			break;
 		}
 #endif
+		case PD_STATE_ENTER_USB:
+			if (pd[port].last_state != pd[port].task_state) {
+				set_state_timeout(port,
+					get_time().val + PD_T_SENDER_RESPONSE,
+					READY_RETURN_STATE(port));
+			}
+			break;
 		default:
 			break;
 		}
diff --git a/driver/usb_mux/usb_mux.c b/driver/usb_mux/usb_mux.c
index f38f153000..5bf057ed91 100644
--- a/driver/usb_mux/usb_mux.c
+++ b/driver/usb_mux/usb_mux.c
@@ -264,7 +264,7 @@ static int command_typec(int argc, char **argv)
 
 		mux_state = usb_mux_get(port);
 		ccprintf("Port %d: USB=%d DP=%d POLARITY=%s HPD_IRQ=%d "
-			"HPD_LVL=%d SAFE=%d TBT=%d\n", port,
+			"HPD_LVL=%d SAFE=%d TBT=%d USB4=%d\n", port,
 			!!(mux_state & USB_PD_MUX_USB_ENABLED),
 			!!(mux_state & USB_PD_MUX_DP_ENABLED),
 			mux_state & USB_PD_MUX_POLARITY_INVERTED ?
@@ -272,7 +272,8 @@ static int command_typec(int argc, char **argv)
 			!!(mux_state & USB_PD_MUX_HPD_IRQ),
 			!!(mux_state & USB_PD_MUX_HPD_LVL),
 			!!(mux_state & USB_PD_MUX_SAFE_MODE),
-			!!(mux_state & USB_PD_MUX_TBT_COMPAT_ENABLED));
+			!!(mux_state & USB_PD_MUX_TBT_COMPAT_ENABLED),
+			!!(mux_state & USB_PD_MUX_USB4_ENABLED));
 
 		return EC_SUCCESS;
 	}
diff --git a/driver/usb_mux/virtual.c b/driver/usb_mux/virtual.c
index cef3e89ca2..6f80157deb 100644
--- a/driver/usb_mux/virtual.c
+++ b/driver/usb_mux/virtual.c
@@ -19,7 +19,8 @@
 #define USB_PD_MUX_HPD_STATE	(USB_PD_MUX_HPD_LVL | USB_PD_MUX_HPD_IRQ)
 #define USB_PD_MUX_USB_DP_STATE	(USB_PD_MUX_USB_ENABLED | \
 			USB_PD_MUX_DP_ENABLED | USB_PD_MUX_POLARITY_INVERTED | \
-			USB_PD_MUX_SAFE_MODE | USB_PD_MUX_TBT_COMPAT_ENABLED)
+			USB_PD_MUX_SAFE_MODE | USB_PD_MUX_TBT_COMPAT_ENABLED | \
+			USB_PD_MUX_USB4_ENABLED)
 
 static mux_state_t virtual_mux_state[CONFIG_USB_PD_PORT_MAX_COUNT];
 
diff --git a/include/config.h b/include/config.h
index ad23405eb1..20c26cf231 100644
--- a/include/config.h
+++ b/include/config.h
@@ -3853,6 +3853,18 @@
  */
 #undef CONFIG_USB_PD_TBT_COMPAT_MODE
 
+/* Enable to enter into USB4 mode between two port partners */
+#undef CONFIG_USB_PD_USB4
+
+/* Enable if the board supports USB3.2 devices */
+#undef CONFIG_USB_PD_USB32
+
+/* Enable if the board is Thunderbolt Gen 3 capable */
+#undef CONFIG_USB_PD_TBT_GEN3_CAPABLE
+
+/* Enable PCIE tunneling if Thunderbolt-Compatible mode is enabled*/
+#undef CONFIG_USB_PD_PCIE_TUNNELING
+
 /*
  * Track VBUS level in TCPC module. This will only be needed if we're acting
  * as an external TCPC.
@@ -5032,6 +5044,22 @@
 #define CONFIG_SEVEN_SEG_DISPLAY
 #endif /* CONFIG_MAX695X_SEVEN_SEGMENT_DISPLAY */
 
+/*****************************************************************************/
+/* Enable PCIE tunneling if the board supports Thunderbolt-Compatible mode */
+#ifdef CONFIG_USB_PD_TBT_COMPAT_MODE
+#define CONFIG_USB_PD_PCIE_TUNNELING
+#define CONFIG_USB_PD_TBT_GEN3_CAPABLE
+#endif /* CONFIG_USB_PD_TBT_COMPAT_MODE */
+
+/*****************************************************************************/
+/*
+ * The board is Gen3 compatible and supports USB3.2 devices if it supports
+ * USB4 mode.
+ */
+#ifdef CONFIG_USB_PD_USB4
+#define CONFIG_USB_PD_USB32
+#endif /* CONFIG_USB_PD_USB4 */
+
 /*
  * Apply fuzzer and test config overrides last, since fuzzers and tests need to
  * override some of the config flags in non-standard ways to mock only parts of
diff --git a/include/ec_commands.h b/include/ec_commands.h
index ff537234cd..d172d8cf7e 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -5520,6 +5520,7 @@ struct ec_params_usb_pd_mux_info {
 #define USB_PD_MUX_HPD_LVL            BIT(4) /* HPD level is asserted */
 #define USB_PD_MUX_SAFE_MODE          BIT(5) /* DP is in safe mode */
 #define USB_PD_MUX_TBT_COMPAT_ENABLED BIT(6) /* TBT compat enabled */
+#define USB_PD_MUX_USB4_ENABLED       BIT(7) /* USB4 enabled */
 
 /* USB-C Dock connected */
 #define USB_PD_MUX_DOCK		(USB_PD_MUX_USB_ENABLED | USB_PD_MUX_DP_ENABLED)
diff --git a/include/usb_pd.h b/include/usb_pd.h
index 59b293fe15..eaff1212c9 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -436,18 +436,24 @@ struct pd_policy {
  * [2] :: Cert Stat VDO
  * [3] :: (Product | Cable) VDO
  * [4] :: AMA VDO
+ * [4] :: Product type UFP1 VDO
  * [4] :: Product type Cable VDO 1
+ * [5] :: Product type UFP2 VDO
  * [5] :: Product type Cable VDO 2
- *
- */
-#define VDO_INDEX_HDR          0
-#define VDO_INDEX_IDH          1
-#define VDO_INDEX_CSTAT        2
-#define VDO_INDEX_CABLE        3
-#define VDO_INDEX_PRODUCT      3
-#define VDO_INDEX_AMA          4
-#define VDO_INDEX_PTYPE_CABLE1 4
-#define VDO_INDEX_PTYPE_CABLE2 5
+ * [6] :: Product type DFP VDO
+ *
+ */
+#define VDO_INDEX_HDR            0
+#define VDO_INDEX_IDH            1
+#define VDO_INDEX_CSTAT          2
+#define VDO_INDEX_CABLE          3
+#define VDO_INDEX_PRODUCT        3
+#define VDO_INDEX_AMA            4
+#define VDO_INDEX_PTYPE_UFP1_VDO 4
+#define VDO_INDEX_PTYPE_CABLE1   4
+#define VDO_INDEX_PTYPE_UFP2_VDO 4
+#define VDO_INDEX_PTYPE_CABLE2   5
+#define VDO_INDEX_PTYPE_DFP_VDO  6
 #define VDO_I(name) VDO_INDEX_##name
 
 /*
@@ -514,6 +520,13 @@ union active_cable_vdo2 {
 	uint32_t raw_value;
 };
 
+/* Protocol revision */
+enum pd_rev_type {
+	PD_REV10,
+	PD_REV20,
+	PD_REV30,
+};
+
 /* Cable structure for storing cable attributes */
 struct pd_cable {
 	uint8_t is_identified;
@@ -526,7 +539,7 @@ struct pd_cable {
 	/* For USB PD REV3, active cable has 2 VDOs */
 	union active_cable_vdo2 attr2;
 	/* Cable revision */
-	uint8_t rev;
+	enum pd_rev_type rev;
 	/* For storing Discover mode response from device */
 	union tbt_mode_resp_device dev_mode_resp;
 	/* For storing Discover mode response from cable */
@@ -541,6 +554,10 @@ struct pd_cable {
 #define CABLE_FLAGS_TBT_COMPAT_ENABLE	   BIT(2)
 /* Flag to limit speed to TBT Gen 2 passive cable */
 #define CABLE_FLAGS_TBT_COMPAT_LIMIT_SPEED BIT(3)
+/* Flag for checking if device is USB4.0 capable */
+#define CABLE_FLAGS_USB4_CAPABLE           BIT(4)
+/* Flag for entering ENTER_USB mode */
+#define CABLE_FLAGS_ENTER_USB_MODE         BIT(5)
 
 /*
  * SVDM Discover SVIDs request -> response
@@ -773,6 +790,7 @@ enum pd_states {
 	PD_STATE_BIST_RX,			/* C36 */
 	PD_STATE_BIST_TX,			/* C37 */
 	PD_STATE_DRP_AUTO_TOGGLE,		/* C38 */
+	PD_STATE_ENTER_USB,			/* C39 */
 	/* Number of states. Not an actual state. */
 	PD_STATE_COUNT,
 };
@@ -1065,16 +1083,10 @@ enum pd_data_msg_type {
 	PD_DATA_ALERT = 6,
 	PD_DATA_GET_COUNTRY_INFO = 7,
 	/* 8-14 Reserved for REV 3.0 */
+	PD_DATA_ENTER_USB = 8,
 	PD_DATA_VENDOR_DEF = 15,
 };
 
-/* Protocol revision */
-enum pd_rev_type {
-	PD_REV10,
-	PD_REV20,
-	PD_REV30
-};
-
 /*
  * Power role. See 6.2.1.1.4 Port Power Role. Only applies to SOP packets.
  * Replaced by pd_cable_plug for SOP' and SOP" packets.
@@ -1700,6 +1712,34 @@ void reset_pd_cable(int port);
 enum idh_ptype get_usb_pd_cable_type(int port);
 
 /**
+ * Return enter USB message payload
+ *
+ * @param port	USB-C port number
+ */
+uint32_t get_enter_usb_msg_payload(int port);
+
+/**
+ * Enter USB4 mode
+ *
+ * @param port	USB-C port number
+ */
+void enter_usb4_mode(int port);
+
+/**
+ * Clear enter USB4 mode
+ *
+ * @param port	USB-C port number
+ */
+void disable_enter_usb4_mode(int port);
+
+/**
+ * Return if need to enter into USB4 mode
+ *
+ * @param port	USB-C port number
+ */
+bool should_enter_usb4_mode(int port);
+
+/**
  * Return the response of discover mode SOP prime, with SVID = 0x8087
  *
  * @param port	USB-C port number
diff --git a/include/usb_pd_vdo.h b/include/usb_pd_vdo.h
index b813bda933..d141d878f1 100644
--- a/include/usb_pd_vdo.h
+++ b/include/usb_pd_vdo.h
@@ -25,6 +25,35 @@
 
 /*****************************************************************************/
 /*
+ * Table 6-35 UFP VDO 1
+ * -------------------------------------------------------------
+ * <31:29> : UFP VDO version
+ * <28>    : Reserved
+ * <27:24> : Device Capability
+ *           0001b = USB2.0 Device capable
+ *           0010b = USB2.0 Device capable (Billboard only)
+ *           0100b = USB3.2 Device capable
+ *           1000b = USB4 Device Capable
+ * <23:6>  : Reserved
+ * <5:3>   : Alternate Modes
+ *           001b = Supports TBT3 alternate mode
+ *           010b = Supports Alternate Modes that reconfigure
+ *                  the signals on the [USB Type-C 2.0] connector
+ *                  – except for [TBT3]
+ *           100b = Supports Alternate Modes that do not
+ *                  reconfigure the signals on the [USB Type-C 2.0]
+ *                  connector
+ * <2:0>   : USB Highest Speed
+ *           000b = USB 2.0 only, no SuperSpeed support
+ *           001b = USB 3.2 Gen1
+ *           010b = USB 3.2/USB4 Gen2
+ *           011b = USB4 Gen3
+ *           100b…111b = Reserved, Shall Not be used
+ */
+#define PD_PRODUCT_IS_USB4(vdo) ((vdo) >> 27 & 0x1)
+
+/*****************************************************************************/
+/*
  * Table 6-38 Passive Cable VDO
  * -------------------------------------------------------------
  * <31:28> : HW Version
@@ -265,6 +294,11 @@ union active_cable_vdo1_rev30 {
  *           1b = Gen 2 or higher
  *           Note: see VDO1 USB Highest Speed for details of Gen supported.
  */
+enum retimer_active_element {
+	ACTIVE_REDRIVER,
+	ACTIVE_RETIMER,
+};
+
 union active_cable_vdo2_rev30 {
 	struct {
 		uint8_t usb_gen : 1;
@@ -275,7 +309,7 @@ union active_cable_vdo2_rev30 {
 		uint8_t usb_20_support : 1;
 		uint8_t usb_20_hub_hop : 2;
 		uint8_t usb_40_support : 1;
-		uint8_t active_elem : 1;
+		enum retimer_active_element active_elem : 1;
 		uint8_t physical_conn : 1;
 		uint8_t u3_to_u0 : 1;
 		uint8_t u3_power : 3;
@@ -640,4 +674,87 @@ enum ama_usb_ss {
 	AMA_USBSS_BBONLY,
 };
 
+/*
+ * Enter USB Data Object (Ref: USB PD 3.2 Version 2.0 Table 6-47)
+ * -----------------------
+ * <31>    : Reserved
+ * <30:28> : USB Mode
+ *           000b - USB2.0
+ *           001b - USB3.2
+ *           010b - USB4
+ * <27>    : Reserved
+ * <26>    : USB4 DRD
+ *           0b: Not capable of operating as a [USB4] Device
+ *           1b: Capable of operating as a [USB4] Device
+ * <25>    : USB3 DRD
+ *           0b: Not capable of operating as a [USB 3.2] Device
+ *           1b: Capable of operating as a [USB 3.2] Device
+ * <24>    : Reserved
+ * <23:21> : Cable Speed
+ *           000b - [USB 2.0] only, no SuperSpeed support
+ *           001b - [USB 3.2] Gen1
+ *           010b - [USB 3.2] Gen2 and [USB4] Gen2
+ *           011b - [USB4] Gen3
+ *           111b..100b: Reserved, Shall not be used
+ * <20:19> : Cable Type
+ *           00b - Passive
+ *           01b - Active Re-timer
+ *           10b - Active Re-driver
+ *           11b - Optically Isolated
+ * <18:17> : Cable Current
+ *           00b = VBUS is not supported
+ *           01b = Reserved
+ *           10b = 3A
+ *           11b = 5A
+ * <16>    : PCIe Supported ? (1b == Yes, 0b == No)
+ * <15     : DP Supported ? (1b == Yes, 0b == No)
+ * <14>    : TBT Supported ? (1b == Yes, 0b == No)
+ * <13>    : Host present ? (1b == Yes, 0b == No)
+ * <12:0>  : Reserved
+ */
+enum usb_mode {
+	USB_PD_20,
+	USB_PD_32,
+	USB_PD_40,
+	USB_PD_INVALID_3,
+	USB_PD_INVALID_4,
+	USB_PD_INVALID_5,
+	USB_PD_INVALID_6,
+	USB_PD_INVALID_7,
+};
+
+enum usb4_cable_current {
+	USB4_CABLE_CURRENT_INVALID,
+	USB4_CABLE_CURRENT_RESERVED,
+	USB4_CABLE_CURRENT_3A,
+	USB4_CABLE_CURRENT_5A,
+};
+
+enum usb4_cable_type {
+	CABLE_TYPE_PASSIVE,
+	CABLE_TYPE_ACTIVE_RETIMER,
+	CABLE_TYPE_ACTIVE_REDRIVER,
+	CABLE_TYPE_ISOLATED,
+};
+
+union enter_usb_data_obj {
+	struct {
+		uint16_t reserved3 : 13;
+		uint8_t host_present : 1;
+		uint8_t tbt_supported : 1;
+		uint8_t dp_supported : 1;
+		uint8_t pcie_supported : 1;
+		enum usb4_cable_current cable_current : 2;
+		enum usb4_cable_type cable_type : 2;
+		enum usb_rev30_ss cable_speed : 3;
+		uint8_t reserved2 : 1;
+		uint8_t usb3_drd_cap : 1;
+		uint8_t usb4_drd_cap : 1;
+		uint8_t reserved1 : 1;
+		enum usb_mode mode : 3;
+		uint8_t reserved0 : 1;
+	};
+	uint32_t raw_value;
+};
+
 #endif /* __CROS_EC_USB_PD_VDO_H */
diff --git a/util/ectool.c b/util/ectool.c
index fe580083b8..392621c887 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -5723,6 +5723,7 @@ int cmd_usb_pd_mux_info(int argc, char *argv[])
 		printf("HPD_LVL=%d ", !!(r.flags & USB_PD_MUX_HPD_LVL));
 		printf("SAFE=%d ", !!(r.flags & USB_PD_MUX_SAFE_MODE));
 		printf("TBT=%d ", !!(r.flags & USB_PD_MUX_TBT_COMPAT_ENABLED));
+		printf("USB4=%d ", !!(r.flags & USB_PD_MUX_USB4_ENABLED));
 		printf("\n");
 	}