TCPM: Separate Discovery into its own module

Discovery can be a separate feature from DFP mode entry, as any data
role can run discovery in PD 3.0. Additionally, boards may want to
enable discovery separate from full EC mode entry so put this feature
behind a new CONFIG indicating its scope.

BRANCH=None
BUG=b:266714542
LOW_COVERAGE_REASON=only moving functions to a new file, b/267964449
filed to track expanding the testing of these functions later
TEST=./twister -T ./zephyr/test, run on both nipperkin (ECOS) and skyrim
(zephyr) to confirm cable and device identities are being reported
correctly

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

9 files changed, 491 insertions, 432 deletions

diff --git a/common/build.mk b/common/build.mk
index 6b3e7db25c..80e039e3c7 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -188,6 +188,7 @@ common-$(CONFIG_USB_PD_HOST_CMD)+=usb_pd_host_cmd.o
 common-$(CONFIG_USB_PD_CONSOLE_CMD)+=usb_pd_console_cmd.o
 endif
 common-$(CONFIG_USB_PD_ALT_MODE_DFP)+=usb_pd_alt_mode_dfp.o
+common-$(CONFIG_USB_PD_DISCOVERY)+=usb_pd_discovery.o
 common-$(CONFIG_USB_PD_ALT_MODE_UFP)+=usb_pd_alt_mode_ufp.o
 common-$(CONFIG_USB_PD_DPS)+=dps.o
 common-$(CONFIG_USB_PD_LOGGING)+=event_log.o pd_log.o
diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
index 5300bf1334..caba71bb31 100644
--- a/common/usb_pd_alt_mode_dfp.c
+++ b/common/usb_pd_alt_mode_dfp.c
@@ -326,23 +326,6 @@ int pd_dfp_exit_mode(int port, enum tcpci_msg_type type, uint16_t svid,
 	return 1;
 }
 
-/*
- * Check if the SVID has been recorded previously. Some peripherals provide
- * duplicated SVID.
- */
-static bool is_svid_duplicated(const struct pd_discovery *disc, uint16_t svid)
-{
-	int i;
-
-	for (i = 0; i < disc->svid_cnt; ++i)
-		if (disc->svids[i].svid == svid) {
-			CPRINTF("ERR:SVIDDUP\n");
-			return true;
-		}
-
-	return false;
-}
-
 void dfp_consume_attention(int port, uint32_t *payload)
 {
 	uint16_t svid = PD_VDO_VID(payload[0]);
@@ -357,143 +340,6 @@ void dfp_consume_attention(int port, uint32_t *payload)
 		modep->fx->attention(port, payload);
 }
 
-void dfp_consume_identity(int port, enum tcpci_msg_type type, int cnt,
-			  uint32_t *payload)
-{
-	int ptype;
-	struct pd_discovery *disc;
-	size_t identity_size;
-
-	if (type == TCPCI_MSG_SOP_PRIME &&
-	    !IS_ENABLED(CONFIG_USB_PD_DECODE_SOP)) {
-		CPRINTF("ERR:Unexpected cable response\n");
-		return;
-	}
-
-	ptype = PD_IDH_PTYPE(payload[VDO_I(IDH)]);
-	disc = pd_get_am_discovery_and_notify_access(port, type);
-	identity_size =
-		MIN(sizeof(union disc_ident_ack), (cnt - 1) * sizeof(uint32_t));
-
-	/* Note: only store VDOs, not the VDM header */
-	memcpy(disc->identity.raw_value, payload + 1, identity_size);
-	disc->identity_cnt = identity_size / sizeof(uint32_t);
-
-	switch (ptype) {
-	case IDH_PTYPE_AMA:
-		/* Leave vbus ON if the following macro is false */
-		if (IS_ENABLED(CONFIG_USB_PD_DUAL_ROLE) &&
-		    IS_ENABLED(CONFIG_USBC_VCONN_SWAP)) {
-			/* Adapter is requesting vconn, try to supply it */
-			if (PD_VDO_AMA_VCONN_REQ(payload[VDO_I(AMA)]))
-				pd_try_vconn_src(port);
-
-			/* Only disable vbus if vconn was requested */
-			if (PD_VDO_AMA_VCONN_REQ(payload[VDO_I(AMA)]) &&
-			    !PD_VDO_AMA_VBUS_REQ(payload[VDO_I(AMA)]))
-				pd_power_supply_reset(port);
-		}
-		break;
-	default:
-		break;
-	}
-	pd_set_identity_discovery(port, type, PD_DISC_COMPLETE);
-}
-
-void dfp_consume_svids(int port, enum tcpci_msg_type type, int cnt,
-		       uint32_t *payload)
-{
-	int i;
-	uint32_t *ptr = payload + 1;
-	int vdo = 1;
-	uint16_t svid0, svid1;
-	struct pd_discovery *disc =
-		pd_get_am_discovery_and_notify_access(port, type);
-
-	for (i = disc->svid_cnt; i < disc->svid_cnt + 12; i += 2) {
-		if (i >= SVID_DISCOVERY_MAX) {
-			CPRINTF("ERR:SVIDCNT\n");
-			break;
-		}
-		/*
-		 * Verify we're still within the valid packet (count will be one
-		 * for the VDM header + xVDOs)
-		 */
-		if (vdo >= cnt)
-			break;
-
-		svid0 = PD_VDO_SVID_SVID0(*ptr);
-		if (!svid0)
-			break;
-
-		if (!is_svid_duplicated(disc, svid0))
-			disc->svids[disc->svid_cnt++].svid = svid0;
-
-		svid1 = PD_VDO_SVID_SVID1(*ptr);
-		if (!svid1)
-			break;
-
-		if (!is_svid_duplicated(disc, svid1))
-			disc->svids[disc->svid_cnt++].svid = svid1;
-
-		ptr++;
-		vdo++;
-	}
-	/* TODO(tbroch) need to re-issue discover svids if > 12 */
-	if (i && ((i % 12) == 0))
-		CPRINTF("ERR:SVID+12\n");
-
-	pd_set_svids_discovery(port, type, PD_DISC_COMPLETE);
-}
-
-void dfp_consume_modes(int port, enum tcpci_msg_type type, int cnt,
-		       uint32_t *payload)
-{
-	int svid_idx;
-	struct svid_mode_data *mode_discovery = NULL;
-	struct pd_discovery *disc =
-		pd_get_am_discovery_and_notify_access(port, type);
-	uint16_t response_svid = (uint16_t)PD_VDO_VID(payload[0]);
-
-	for (svid_idx = 0; svid_idx < disc->svid_cnt; ++svid_idx) {
-		uint16_t svid = disc->svids[svid_idx].svid;
-
-		if (svid == response_svid) {
-			mode_discovery = &disc->svids[svid_idx];
-			break;
-		}
-	}
-	if (!mode_discovery) {
-		const struct svid_mode_data *requested_mode_data =
-			pd_get_next_mode(port, type);
-		CPRINTF("C%d: Mode response for undiscovered SVID %x, but TCPM "
-			"requested SVID %x\n",
-			port, response_svid, requested_mode_data->svid);
-		/*
-		 * Although SVIDs discovery seemed like it succeeded before, the
-		 * partner is now responding with undiscovered SVIDs. Discovery
-		 * cannot reasonably continue under these circumstances.
-		 */
-		pd_set_modes_discovery(port, type, requested_mode_data->svid,
-				       PD_DISC_FAIL);
-		return;
-	}
-
-	mode_discovery->mode_cnt = cnt - 1;
-	if (mode_discovery->mode_cnt < 1) {
-		CPRINTF("ERR:NOMODE\n");
-		pd_set_modes_discovery(port, type, mode_discovery->svid,
-				       PD_DISC_FAIL);
-		return;
-	}
-
-	memcpy(mode_discovery->mode_vdo, &payload[1],
-	       sizeof(uint32_t) * mode_discovery->mode_cnt);
-	disc->svid_idx++;
-	pd_set_modes_discovery(port, type, mode_discovery->svid,
-			       PD_DISC_COMPLETE);
-}
-
 int pd_alt_mode(int port, enum tcpci_msg_type type, uint16_t svid)
 {
 	struct svdm_amode_data *modep = pd_get_amode_data(port, type, svid);
@@ -501,200 +347,6 @@ int pd_alt_mode(int port, enum tcpci_msg_type type, uint16_t svid)
 	return (modep) ? modep->opos : -1;
 }
 
-void pd_set_identity_discovery(int port, enum tcpci_msg_type type,
-			       enum pd_discovery_state disc)
-{
-	struct pd_discovery *pd =
-		pd_get_am_discovery_and_notify_access(port, type);
-
-	pd->identity_discovery = disc;
-}
-
-enum pd_discovery_state pd_get_identity_discovery(int port,
-						  enum tcpci_msg_type type)
-{
-	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
-
-	return disc->identity_discovery;
-}
-
-const union disc_ident_ack *pd_get_identity_response(int port,
-						     enum tcpci_msg_type type)
-{
-	if (type >= DISCOVERY_TYPE_COUNT)
-		return NULL;
-
-	return &pd_get_am_discovery(port, type)->identity;
-}
-
-uint16_t pd_get_identity_vid(int port)
-{
-	const union disc_ident_ack *resp =
-		pd_get_identity_response(port, TCPCI_MSG_SOP);
-
-	return resp->idh.usb_vendor_id;
-}
-
-uint16_t pd_get_identity_pid(int port)
-{
-	const union disc_ident_ack *resp =
-		pd_get_identity_response(port, TCPCI_MSG_SOP);
-
-	return resp->product.product_id;
-}
-
-uint8_t pd_get_product_type(int port)
-{
-	const union disc_ident_ack *resp =
-		pd_get_identity_response(port, TCPCI_MSG_SOP);
-
-	return resp->idh.product_type;
-}
-
-void pd_set_svids_discovery(int port, enum tcpci_msg_type type,
-			    enum pd_discovery_state disc)
-{
-	struct pd_discovery *pd =
-		pd_get_am_discovery_and_notify_access(port, type);
-
-	pd->svids_discovery = disc;
-}
-
-enum pd_discovery_state pd_get_svids_discovery(int port,
-					       enum tcpci_msg_type type)
-{
-	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
-
-	return disc->svids_discovery;
-}
-
-int pd_get_svid_count(int port, enum tcpci_msg_type type)
-{
-	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
-
-	return disc->svid_cnt;
-}
-
-uint16_t pd_get_svid(int port, uint16_t svid_idx, enum tcpci_msg_type type)
-{
-	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
-
-	return disc->svids[svid_idx].svid;
-}
-
-void pd_set_modes_discovery(int port, enum tcpci_msg_type type, uint16_t svid,
-			    enum pd_discovery_state disc)
-{
-	struct pd_discovery *pd =
-		pd_get_am_discovery_and_notify_access(port, type);
-	int svid_idx;
-
-	for (svid_idx = 0; svid_idx < pd->svid_cnt; ++svid_idx) {
-		struct svid_mode_data *mode_data = &pd->svids[svid_idx];
-
-		if (mode_data->svid != svid)
-			continue;
-
-		mode_data->discovery = disc;
-		return;
-	}
-}
-
-enum pd_discovery_state pd_get_modes_discovery(int port,
-					       enum tcpci_msg_type type)
-{
-	const struct svid_mode_data *mode_data = pd_get_next_mode(port, type);
-
-	/*
-	 * If there are no SVIDs for which to discover modes, mode discovery is
-	 * trivially complete.
-	 */
-	if (!mode_data)
-		return PD_DISC_COMPLETE;
-
-	return mode_data->discovery;
-}
-
-int pd_get_mode_vdo_for_svid(int port, enum tcpci_msg_type type, uint16_t svid,
-			     uint32_t *vdo_out)
-{
-	int idx;
-	const struct pd_discovery *disc;
-
-	if (type >= DISCOVERY_TYPE_COUNT)
-		return 0;
-
-	disc = pd_get_am_discovery(port, type);
-
-	for (idx = 0; idx < disc->svid_cnt; ++idx) {
-		if (pd_get_svid(port, idx, type) == svid) {
-			memcpy(vdo_out, disc->svids[idx].mode_vdo,
-			       sizeof(uint32_t) * disc->svids[idx].mode_cnt);
-			return disc->svids[idx].mode_cnt;
-		}
-	}
-	return 0;
-}
-
-const struct svid_mode_data *pd_get_next_mode(int port,
-					      enum tcpci_msg_type type)
-{
-	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
-	const struct svid_mode_data *failed_mode_data = NULL;
-	bool svid_good_discovery = false;
-	int svid_idx;
-
-	/* Walk through all of the discovery mode entries */
-	for (svid_idx = 0; svid_idx < disc->svid_cnt; ++svid_idx) {
-		const struct svid_mode_data *mode_data = &disc->svids[svid_idx];
-
-		/* Discovery is needed, so send this one back now */
-		if (mode_data->discovery == PD_DISC_NEEDED)
-			return mode_data;
-
-		/* Discovery already succeeded, save that it was seen */
-		if (mode_data->discovery == PD_DISC_COMPLETE)
-			svid_good_discovery = true;
-		/* Discovery already failed, save first failure */
-		else if (!failed_mode_data)
-			failed_mode_data = mode_data;
-	}
-
-	/* If no good entries were located, then return last failed */
-	if (!svid_good_discovery)
-		return failed_mode_data;
-
-	/*
-	 * Mode discovery has been attempted for every discovered SVID (if
-	 * any exist)
-	 */
-	return NULL;
-}
-
-const uint32_t *pd_get_mode_vdo(int port, uint16_t svid_idx,
-				enum tcpci_msg_type type)
-{
-	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
-
-	return disc->svids[svid_idx].mode_vdo;
-}
-
-bool pd_is_mode_discovered_for_svid(int port, enum tcpci_msg_type type,
-				    uint16_t svid)
-{
-	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
-	const struct svid_mode_data *mode_data;
-
-	for (mode_data = disc->svids; mode_data < disc->svids + disc->svid_cnt;
-	     ++mode_data) {
-		if (mode_data->svid == svid &&
-		    mode_data->discovery == PD_DISC_COMPLETE)
-			return true;
-	}
-
-	return false;
-}
-
 void notify_sysjump_ready(void)
 {
 	/*
@@ -705,86 +357,6 @@ void notify_sysjump_ready(void)
 		task_set_event(sysjump_task_waiting, TASK_EVENT_SYSJUMP_READY);
 }
 
-static inline bool is_pd_rev3(int port, enum tcpci_msg_type type)
-{
-	return pd_get_rev(port, type) == PD_REV30;
-}
-
-/*
- * ############################################################################
- *
- * (Charge Through) Vconn Powered Device functions
- *
- * ############################################################################
- */
-bool is_vpd_ct_supported(int port)
-{
-	const struct pd_discovery *disc =
-		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
-	union vpd_vdo vpd = disc->identity.product_t1.vpd;
-
-	return vpd.ct_support;
-}
-
-/*
- * ############################################################################
- *
- * Cable communication functions
- *
- * ############################################################################
- */
-enum idh_ptype get_usb_pd_cable_type(int port)
-{
-	const struct pd_discovery *disc =
-		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
-
-	return disc->identity.idh.product_type;
-}
-
-bool is_usb2_cable_support(int port)
-{
-	const struct pd_discovery *disc =
-		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
-
-	return disc->identity.idh.product_type == IDH_PTYPE_PCABLE ||
-	       pd_get_vdo_ver(port, TCPCI_MSG_SOP_PRIME) < VDM_VER20 ||
-	       disc->identity.product_t2.a2_rev30.usb_20_support ==
-		       USB2_SUPPORTED;
-}
-
-bool is_cable_speed_gen2_capable(int port)
-{
-	const struct pd_discovery *disc =
-		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
-
-	switch (pd_get_rev(port, TCPCI_MSG_SOP_PRIME)) {
-	case PD_REV20:
-		return disc->identity.product_t1.p_rev20.ss ==
-		       USB_R20_SS_U31_GEN1_GEN2;
-
-	case PD_REV30:
-		return disc->identity.product_t1.p_rev30.ss ==
-			       USB_R30_SS_U32_U40_GEN2 ||
-		       disc->identity.product_t1.p_rev30.ss ==
-			       USB_R30_SS_U40_GEN3;
-	default:
-		return false;
-	}
-}
-
-bool is_active_cable_element_retimer(int port)
-{
-	const struct pd_discovery *disc =
-		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
-
-	/* Ref: USB PD Spec 2.0 Table 6-29 Active Cable VDO
-	 * Revision 2 Active cables do not have Active element support.
-	 */
-	return is_pd_rev3(port, TCPCI_MSG_SOP_PRIME) &&
-	       disc->identity.idh.product_type == IDH_PTYPE_ACABLE &&
-	       disc->identity.product_t2.a2_rev30.active_elem == ACTIVE_RETIMER;
-}
-
 #ifdef CONFIG_USB_PD_DP_MODE
 __overridable void svdm_safe_dp_mode(int port)
 {
diff --git a/common/usb_pd_discovery.c b/common/usb_pd_discovery.c
new file mode 100644
index 0000000000..9cadd197be
--- /dev/null
+++ b/common/usb_pd_discovery.c
@@ -0,0 +1,460 @@
+/* Copyright 2023 The ChromiumOS Authors
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ *
+ * Alternate Mode Discovery storage, access, and helpers
+ */
+
+#include "builtin/assert.h"
+#include "chipset.h"
+#include "console.h"
+#include "gpio.h"
+#include "task.h"
+#include "task_id.h"
+#include "timer.h"
+#include "typec_control.h"
+#include "usb_charge.h"
+#include "usb_common.h"
+#include "usb_dp_alt_mode.h"
+#include "usb_mux.h"
+#include "usb_pd.h"
+#include "usb_pd_tcpm.h"
+#include "usb_tbt_alt_mode.h"
+#include "usbc_ppc.h"
+#include "util.h"
+
+#ifdef CONFIG_COMMON_RUNTIME
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ##args)
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ##args)
+#else
+#define CPRINTS(format, args...)
+#define CPRINTF(format, args...)
+#endif
+
+/*
+ * Check if the SVID has been recorded previously. Some peripherals provide
+ * duplicated SVID.
+ */
+static bool is_svid_duplicated(const struct pd_discovery *disc, uint16_t svid)
+{
+	int i;
+
+	for (i = 0; i < disc->svid_cnt; ++i)
+		if (disc->svids[i].svid == svid) {
+			CPRINTF("ERR:SVIDDUP\n");
+			return true;
+		}
+
+	return false;
+}
+
+void dfp_consume_identity(int port, enum tcpci_msg_type type, int cnt,
+			  uint32_t *payload)
+{
+	int ptype;
+	struct pd_discovery *disc;
+	size_t identity_size;
+
+	if (type == TCPCI_MSG_SOP_PRIME &&
+	    !IS_ENABLED(CONFIG_USB_PD_DECODE_SOP)) {
+		CPRINTF("ERR:Unexpected cable response\n");
+		return;
+	}
+
+	ptype = PD_IDH_PTYPE(payload[VDO_I(IDH)]);
+	disc = pd_get_am_discovery_and_notify_access(port, type);
+	identity_size =
+		MIN(sizeof(union disc_ident_ack), (cnt - 1) * sizeof(uint32_t));
+
+	/* Note: only store VDOs, not the VDM header */
+	memcpy(disc->identity.raw_value, payload + 1, identity_size);
+	disc->identity_cnt = identity_size / sizeof(uint32_t);
+
+	switch (ptype) {
+	case IDH_PTYPE_AMA:
+		/* Leave vbus ON if the following macro is false */
+		if (IS_ENABLED(CONFIG_USB_PD_DUAL_ROLE) &&
+		    IS_ENABLED(CONFIG_USBC_VCONN_SWAP)) {
+			/* Adapter is requesting vconn, try to supply it */
+			if (PD_VDO_AMA_VCONN_REQ(payload[VDO_I(AMA)]))
+				pd_try_vconn_src(port);
+
+			/* Only disable vbus if vconn was requested */
+			if (PD_VDO_AMA_VCONN_REQ(payload[VDO_I(AMA)]) &&
+			    !PD_VDO_AMA_VBUS_REQ(payload[VDO_I(AMA)]))
+				pd_power_supply_reset(port);
+		}
+		break;
+	default:
+		break;
+	}
+	pd_set_identity_discovery(port, type, PD_DISC_COMPLETE);
+}
+
+void dfp_consume_svids(int port, enum tcpci_msg_type type, int cnt,
+		       uint32_t *payload)
+{
+	int i;
+	uint32_t *ptr = payload + 1;
+	int vdo = 1;
+	uint16_t svid0, svid1;
+	struct pd_discovery *disc =
+		pd_get_am_discovery_and_notify_access(port, type);
+
+	for (i = disc->svid_cnt; i < disc->svid_cnt + 12; i += 2) {
+		if (i >= SVID_DISCOVERY_MAX) {
+			CPRINTF("ERR:SVIDCNT\n");
+			break;
+		}
+		/*
+		 * Verify we're still within the valid packet (count will be one
+		 * for the VDM header + xVDOs)
+		 */
+		if (vdo >= cnt)
+			break;
+
+		svid0 = PD_VDO_SVID_SVID0(*ptr);
+		if (!svid0)
+			break;
+
+		if (!is_svid_duplicated(disc, svid0))
+			disc->svids[disc->svid_cnt++].svid = svid0;
+
+		svid1 = PD_VDO_SVID_SVID1(*ptr);
+		if (!svid1)
+			break;
+
+		if (!is_svid_duplicated(disc, svid1))
+			disc->svids[disc->svid_cnt++].svid = svid1;
+
+		ptr++;
+		vdo++;
+	}
+	/* TODO(tbroch) need to re-issue discover svids if > 12 */
+	if (i && ((i % 12) == 0))
+		CPRINTF("ERR:SVID+12\n");
+
+	pd_set_svids_discovery(port, type, PD_DISC_COMPLETE);
+}
+
+void dfp_consume_modes(int port, enum tcpci_msg_type type, int cnt,
+		       uint32_t *payload)
+{
+	int svid_idx;
+	struct svid_mode_data *mode_discovery = NULL;
+	struct pd_discovery *disc =
+		pd_get_am_discovery_and_notify_access(port, type);
+	uint16_t response_svid = (uint16_t)PD_VDO_VID(payload[0]);
+
+	for (svid_idx = 0; svid_idx < disc->svid_cnt; ++svid_idx) {
+		uint16_t svid = disc->svids[svid_idx].svid;
+
+		if (svid == response_svid) {
+			mode_discovery = &disc->svids[svid_idx];
+			break;
+		}
+	}
+	if (!mode_discovery) {
+		const struct svid_mode_data *requested_mode_data =
+			pd_get_next_mode(port, type);
+		CPRINTF("C%d: Mode response for undiscovered SVID %x, but TCPM "
+			"requested SVID %x\n",
+			port, response_svid, requested_mode_data->svid);
+		/*
+		 * Although SVIDs discovery seemed like it succeeded before, the
+		 * partner is now responding with undiscovered SVIDs. Discovery
+		 * cannot reasonably continue under these circumstances.
+		 */
+		pd_set_modes_discovery(port, type, requested_mode_data->svid,
+				       PD_DISC_FAIL);
+		return;
+	}
+
+	mode_discovery->mode_cnt = cnt - 1;
+	if (mode_discovery->mode_cnt < 1) {
+		CPRINTF("ERR:NOMODE\n");
+		pd_set_modes_discovery(port, type, mode_discovery->svid,
+				       PD_DISC_FAIL);
+		return;
+	}
+
+	memcpy(mode_discovery->mode_vdo, &payload[1],
+	       sizeof(uint32_t) * mode_discovery->mode_cnt);
+	disc->svid_idx++;
+	pd_set_modes_discovery(port, type, mode_discovery->svid,
+			       PD_DISC_COMPLETE);
+}
+
+void pd_set_identity_discovery(int port, enum tcpci_msg_type type,
+			       enum pd_discovery_state disc)
+{
+	struct pd_discovery *pd =
+		pd_get_am_discovery_and_notify_access(port, type);
+
+	pd->identity_discovery = disc;
+}
+
+enum pd_discovery_state pd_get_identity_discovery(int port,
+						  enum tcpci_msg_type type)
+{
+	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
+
+	return disc->identity_discovery;
+}
+
+const union disc_ident_ack *pd_get_identity_response(int port,
+						     enum tcpci_msg_type type)
+{
+	if (type >= DISCOVERY_TYPE_COUNT)
+		return NULL;
+
+	return &pd_get_am_discovery(port, type)->identity;
+}
+
+uint16_t pd_get_identity_vid(int port)
+{
+	const union disc_ident_ack *resp =
+		pd_get_identity_response(port, TCPCI_MSG_SOP);
+
+	return resp->idh.usb_vendor_id;
+}
+
+uint16_t pd_get_identity_pid(int port)
+{
+	const union disc_ident_ack *resp =
+		pd_get_identity_response(port, TCPCI_MSG_SOP);
+
+	return resp->product.product_id;
+}
+
+uint8_t pd_get_product_type(int port)
+{
+	const union disc_ident_ack *resp =
+		pd_get_identity_response(port, TCPCI_MSG_SOP);
+
+	return resp->idh.product_type;
+}
+
+void pd_set_svids_discovery(int port, enum tcpci_msg_type type,
+			    enum pd_discovery_state disc)
+{
+	struct pd_discovery *pd =
+		pd_get_am_discovery_and_notify_access(port, type);
+
+	pd->svids_discovery = disc;
+}
+
+enum pd_discovery_state pd_get_svids_discovery(int port,
+					       enum tcpci_msg_type type)
+{
+	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
+
+	return disc->svids_discovery;
+}
+
+int pd_get_svid_count(int port, enum tcpci_msg_type type)
+{
+	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
+
+	return disc->svid_cnt;
+}
+
+uint16_t pd_get_svid(int port, uint16_t svid_idx, enum tcpci_msg_type type)
+{
+	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
+
+	return disc->svids[svid_idx].svid;
+}
+
+void pd_set_modes_discovery(int port, enum tcpci_msg_type type, uint16_t svid,
+			    enum pd_discovery_state disc)
+{
+	struct pd_discovery *pd =
+		pd_get_am_discovery_and_notify_access(port, type);
+	int svid_idx;
+
+	for (svid_idx = 0; svid_idx < pd->svid_cnt; ++svid_idx) {
+		struct svid_mode_data *mode_data = &pd->svids[svid_idx];
+
+		if (mode_data->svid != svid)
+			continue;
+
+		mode_data->discovery = disc;
+		return;
+	}
+}
+
+enum pd_discovery_state pd_get_modes_discovery(int port,
+					       enum tcpci_msg_type type)
+{
+	const struct svid_mode_data *mode_data = pd_get_next_mode(port, type);
+
+	/*
+	 * If there are no SVIDs for which to discover modes, mode discovery is
+	 * trivially complete.
+	 */
+	if (!mode_data)
+		return PD_DISC_COMPLETE;
+
+	return mode_data->discovery;
+}
+
+int pd_get_mode_vdo_for_svid(int port, enum tcpci_msg_type type, uint16_t svid,
+			     uint32_t *vdo_out)
+{
+	int idx;
+	const struct pd_discovery *disc;
+
+	if (type >= DISCOVERY_TYPE_COUNT)
+		return 0;
+
+	disc = pd_get_am_discovery(port, type);
+
+	for (idx = 0; idx < disc->svid_cnt; ++idx) {
+		if (pd_get_svid(port, idx, type) == svid) {
+			memcpy(vdo_out, disc->svids[idx].mode_vdo,
+			       sizeof(uint32_t) * disc->svids[idx].mode_cnt);
+			return disc->svids[idx].mode_cnt;
+		}
+	}
+	return 0;
+}
+
+const struct svid_mode_data *pd_get_next_mode(int port,
+					      enum tcpci_msg_type type)
+{
+	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
+	const struct svid_mode_data *failed_mode_data = NULL;
+	bool svid_good_discovery = false;
+	int svid_idx;
+
+	/* Walk through all of the discovery mode entries */
+	for (svid_idx = 0; svid_idx < disc->svid_cnt; ++svid_idx) {
+		const struct svid_mode_data *mode_data = &disc->svids[svid_idx];
+
+		/* Discovery is needed, so send this one back now */
+		if (mode_data->discovery == PD_DISC_NEEDED)
+			return mode_data;
+
+		/* Discovery already succeeded, save that it was seen */
+		if (mode_data->discovery == PD_DISC_COMPLETE)
+			svid_good_discovery = true;
+		/* Discovery already failed, save first failure */
+		else if (!failed_mode_data)
+			failed_mode_data = mode_data;
+	}
+
+	/* If no good entries were located, then return last failed */
+	if (!svid_good_discovery)
+		return failed_mode_data;
+
+	/*
+	 * Mode discovery has been attempted for every discovered SVID (if
+	 * any exist)
+	 */
+	return NULL;
+}
+
+const uint32_t *pd_get_mode_vdo(int port, uint16_t svid_idx,
+				enum tcpci_msg_type type)
+{
+	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
+
+	return disc->svids[svid_idx].mode_vdo;
+}
+
+bool pd_is_mode_discovered_for_svid(int port, enum tcpci_msg_type type,
+				    uint16_t svid)
+{
+	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
+	const struct svid_mode_data *mode_data;
+
+	for (mode_data = disc->svids; mode_data < disc->svids + disc->svid_cnt;
+	     ++mode_data) {
+		if (mode_data->svid == svid &&
+		    mode_data->discovery == PD_DISC_COMPLETE)
+			return true;
+	}
+
+	return false;
+}
+
+static inline bool is_pd_rev3(int port, enum tcpci_msg_type type)
+{
+	return pd_get_rev(port, type) == PD_REV30;
+}
+
+/*
+ * ############################################################################
+ *
+ * (Charge Through) Vconn Powered Device functions
+ *
+ * ############################################################################
+ */
+bool is_vpd_ct_supported(int port)
+{
+	const struct pd_discovery *disc =
+		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
+	union vpd_vdo vpd = disc->identity.product_t1.vpd;
+
+	return vpd.ct_support;
+}
+
+/*
+ * ############################################################################
+ *
+ * Cable communication functions
+ *
+ * ############################################################################
+ */
+enum idh_ptype get_usb_pd_cable_type(int port)
+{
+	const struct pd_discovery *disc =
+		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
+
+	return disc->identity.idh.product_type;
+}
+
+bool is_usb2_cable_support(int port)
+{
+	const struct pd_discovery *disc =
+		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
+
+	return disc->identity.idh.product_type == IDH_PTYPE_PCABLE ||
+	       pd_get_vdo_ver(port, TCPCI_MSG_SOP_PRIME) < VDM_VER20 ||
+	       disc->identity.product_t2.a2_rev30.usb_20_support ==
+		       USB2_SUPPORTED;
+}
+
+bool is_cable_speed_gen2_capable(int port)
+{
+	const struct pd_discovery *disc =
+		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
+
+	switch (pd_get_rev(port, TCPCI_MSG_SOP_PRIME)) {
+	case PD_REV20:
+		return disc->identity.product_t1.p_rev20.ss ==
+		       USB_R20_SS_U31_GEN1_GEN2;
+
+	case PD_REV30:
+		return disc->identity.product_t1.p_rev30.ss ==
+			       USB_R30_SS_U32_U40_GEN2 ||
+		       disc->identity.product_t1.p_rev30.ss ==
+			       USB_R30_SS_U40_GEN3;
+	default:
+		return false;
+	}
+}
+
+bool is_active_cable_element_retimer(int port)
+{
+	const struct pd_discovery *disc =
+		pd_get_am_discovery(port, TCPCI_MSG_SOP_PRIME);
+
+	/* Ref: USB PD Spec 2.0 Table 6-29 Active Cable VDO
+	 * Revision 2 Active cables do not have Active element support.
+	 */
+	return is_pd_rev3(port, TCPCI_MSG_SOP_PRIME) &&
+	       disc->identity.idh.product_type == IDH_PTYPE_ACABLE &&
+	       disc->identity.product_t2.a2_rev30.active_elem == ACTIVE_RETIMER;
+}
diff --git a/fuzz/fuzz_config.h b/fuzz/fuzz_config.h
index edfe5b4c24..d642513b24 100644
--- a/fuzz/fuzz_config.h
+++ b/fuzz/fuzz_config.h
@@ -55,6 +55,7 @@
 #define CONFIG_USB_DRP_ACC_TRYSRC
 #define CONFIG_USB_PD_ALT_MODE_DFP
 #define CONFIG_USB_PD_DP_MODE
+#define CONFIG_USB_PD_DISCOVERY
 #define CONFIG_USBC_SS_MUX
 #define CONFIG_USBC_VCONN
 #define CONFIG_USBC_VCONN_SWAP
@@ -77,6 +78,7 @@
 #define CONFIG_USB_DRP_ACC_TRYSRC
 #define CONFIG_USB_PD_ALT_MODE_DFP
 #define CONFIG_USB_PD_DP_MODE
+#define CONFIG_USB_PD_DISCOVERY
 #define CONFIG_USBC_SS_MUX
 #define CONFIG_USBC_VCONN
 #define CONFIG_USBC_VCONN_SWAP
diff --git a/include/config.h b/include/config.h
index f534f39df6..4084c357f2 100644
--- a/include/config.h
+++ b/include/config.h
@@ -4566,15 +4566,18 @@
 #define CONFIG_USB_PD_HOST_CMD
 #endif
 
-/* Support for USB PD alternate mode */
+/* Support for USB PD alternate mode entry */
 #undef CONFIG_USB_PD_ALT_MODE
 
-/* Support for USB PD alternate mode of Downward Facing Port */
+/* Support for USB PD alternate mode entry by a Downward Facing Port */
 #undef CONFIG_USB_PD_ALT_MODE_DFP
 
-/* Support for USB PD alternate mode of Upward Facing Port */
+/* Support for USB PD alternate mode entry from an Upward Facing Port */
 #undef CONFIG_USB_PD_ALT_MODE_UFP
 
+/* Support for automatic USB PD Discovery VDM probing and storage */
+#undef CONFIG_USB_PD_DISCOVERY
+
 /*
  * Do not enter USB PD alternate modes or USB4 automatically. Wait for the AP to
  * direct the EC to enter a mode. This requires AP software support.
@@ -5929,10 +5932,12 @@
 /******************************************************************************/
 /*
  * If CONFIG_USB_PD_ALT_MODE_DFP is set and this isn't a zephyr build (which
- * already did its preprocessing earlier), then enable DP Mode by default
+ * already did its preprocessing earlier), then enable DP Mode by default and
+ * also enable discovery by default.
  */
 #if defined(CONFIG_USB_PD_ALT_MODE_DFP) && !defined(CONFIG_ZEPHYR)
 #define CONFIG_USB_PD_DP_MODE
+#define CONFIG_USB_PD_DISCOVERY
 #endif
 
 /******************************************************************************/
diff --git a/test/test_config.h b/test/test_config.h
index bc5b2ae133..926ebba090 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -452,6 +452,7 @@ int ncp15wb_calculate_temp(uint16_t adc);
 #undef CONFIG_USB_PD_HOST_CMD
 #define CONFIG_USB_PD_ALT_MODE_DFP
 #define CONFIG_USB_PD_DP_MODE
+#define CONFIG_USB_PD_DISCOVERY
 #define CONFIG_USBC_SS_MUX
 #define CONFIG_USB_PD_3A_PORTS 0 /* Host does not define a 3.0 A PDO */
 #endif
@@ -477,6 +478,7 @@ int ncp15wb_calculate_temp(uint16_t adc);
 #undef CONFIG_USB_PD_HOST_CMD
 #define CONFIG_USB_PD_ALT_MODE_DFP
 #define CONFIG_USB_PD_DP_MODE
+#define CONFIG_USB_PD_DISCOVERY
 #define CONFIG_USBC_SS_MUX
 #define I2C_PORT_HOST_TCPC 0
 #define CONFIG_CHARGE_MANAGER
@@ -554,6 +556,7 @@ int ncp15wb_calculate_temp(uint16_t adc);
 #define CONFIG_TEST_USB_PE_SM
 #define CONFIG_USB_PD_ALT_MODE_DFP
 #define CONFIG_USB_PD_DP_MODE
+#define CONFIG_USB_PD_DISCOVERY
 #define CONFIG_USBC_VCONN
 #define CONFIG_USBC_VCONN_SWAP
 #define CONFIG_USB_PID 0x5036
diff --git a/zephyr/CMakeLists.txt b/zephyr/CMakeLists.txt
index dc63d1f3cc..34ba6715a4 100644
--- a/zephyr/CMakeLists.txt
+++ b/zephyr/CMakeLists.txt
@@ -470,6 +470,8 @@ zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_USBC_OCP
 
 zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_USB_PD_ALT_MODE_DFP
                                                 "${PLATFORM_EC}/common/usb_pd_alt_mode_dfp.c")
+zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_USB_PD_DISCOVERY
+                                                "${PLATFORM_EC}/common/usb_pd_discovery.c")
 zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_USB_PD_ALT_MODE_UFP
                                                 "${PLATFORM_EC}/common/usb_pd_alt_mode_ufp.c")
 
diff --git a/zephyr/Kconfig.pd b/zephyr/Kconfig.pd
index 49db2fb864..68ecfe78e6 100644
--- a/zephyr/Kconfig.pd
+++ b/zephyr/Kconfig.pd
@@ -225,6 +225,7 @@ config PLATFORM_EC_USB_PD_VDM_AP_CONTROL
 config PLATFORM_EC_USB_PD_ALT_MODE_DFP
 	bool "Downward Facing Port support"
 	default y
+	depends on PLATFORM_EC_USB_PD_DISCOVERY
 	help
 	  Enable support for USB Power Delivery alternate mode of Downward
 	  Facing Port.
@@ -243,6 +244,14 @@ config PLATFORM_EC_USB_PD_ALT_MODE_UFP
 	  USB4 and ThunderBolt operation when the Chromium OS data role
 	  resolves to the UFP role.
 
+config PLATFORM_EC_USB_PD_DISCOVERY
+	bool "Enable EC to direct Discover VDMs"
+	default y
+	help
+	  This enables support for the EC probing and storing the various
+	  partner discovery messages (DiscoverIdentity, DiscoverModes,
+	  DiscoverSVIDs).
+
 config PLATFORM_EC_USB_PD_USB32_DRD
 	bool "Port is capable of operating as a USB3.2 device"
 	default n
diff --git a/zephyr/shim/include/config_chip.h b/zephyr/shim/include/config_chip.h
index 14254363b0..fd97a9d72a 100644
--- a/zephyr/shim/include/config_chip.h
+++ b/zephyr/shim/include/config_chip.h
@@ -1737,6 +1737,11 @@ extern char mock_jump_data[CONFIG_PLATFORM_EC_PRESERVED_END_OF_RAM_SIZE];
 #define CONFIG_USB_PD_ALT_MODE_UFP
 #endif
 
+#undef CONFIG_USB_PD_DISCOVERY
+#ifdef CONFIG_PLATFORM_EC_USB_PD_DISCOVERY
+#define CONFIG_USB_PD_DISCOVERY
+#endif
+
 #undef CONFIG_USB_PD_DPS
 #ifdef CONFIG_PLATFORM_EC_USB_PD_DPS
 #define CONFIG_USB_PD_DPS