TCPM: Divorce v1 and v2 alternate mode handling

Retire the common DFP mode support to the TCPMv1 specific module from
whence it came.  Since TCPMv1 does not support TBT, all TBT functions
can be removed from its copy.

Move the DFP support for TCPMv2 to the DPM and its modules.  Note that
the reduced redundancy in tracking active alternate modes means that we
can remove the majority of the "generic" mode handling.  This results in
approximately 1k additional free flash space for boards.

For now, leave the svdm_dp_* interfaces in place as boards are using
them as entry points to modify the DP mode sequencing.  Also leave a
stripped down version of pd_dfp_exit_mode() in place, though this would
be worth consolidating to a new API in the future.

BRANCH=None
BUG=b:170372521,b:159856063
TEST=all unit tests passing, confirm display works on nipperkin (ECOS)
and skyrim (zephyr) boards, USB4 and TBT regression tested on skolas
LOW_COVERAGE_REASON=b/243151272 filed for improving TBT coverage,
common/mock files no longer relevant

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

10 files changed, 1196 insertions, 819 deletions

diff --git a/common/build.mk b/common/build.mk
index 591ea9fd6b..2da6a89b38 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -187,7 +187,6 @@ common-$(CONFIG_USB_PD_DUAL_ROLE)+=usb_pd_dual_role.o
 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
diff --git a/common/mock/usb_pd_dpm_mock.c b/common/mock/usb_pd_dpm_mock.c
index 22df6ed9a7..fee51b3e15 100644
--- a/common/mock/usb_pd_dpm_mock.c
+++ b/common/mock/usb_pd_dpm_mock.c
@@ -17,6 +17,12 @@
 #error "Mocks should only be in the test build."
 #endif
 
+__overridable const struct svdm_response svdm_rsp = {
+	.identity = NULL,
+	.svids = NULL,
+	.modes = NULL,
+};
+
 struct mock_dpm_port_t dpm[CONFIG_USB_PD_PORT_MAX_COUNT];
 
 void mock_dpm_reset(void)
@@ -25,6 +31,14 @@ void mock_dpm_reset(void)
 	memset(dpm, 0, sizeof(dpm));
 }
 
+void dfp_consume_attention(int port, uint32_t *payload)
+{
+}
+
+void pd_prepare_sysjump(void)
+{
+}
+
 void dpm_init(int port)
 {
 	dpm[port].mode_entry_done = false;
diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
deleted file mode 100644
index caba71bb31..0000000000
--- a/common/usb_pd_alt_mode_dfp.c
+++ /dev/null
@@ -1,743 +0,0 @@
-/* Copyright 2020 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 Downstream Facing Port (DFP) USB-PD module.
- */
-
-#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
-
-#ifndef PORT_TO_HPD
-#define PORT_TO_HPD(port) ((port) ? GPIO_USB_C1_DP_HPD : GPIO_USB_C0_DP_HPD)
-#endif /* PORT_TO_HPD */
-
-/* Tracker for which task is waiting on sysjump prep to finish */
-static volatile task_id_t sysjump_task_waiting = TASK_ID_INVALID;
-
-/*
- * timestamp of the next possible toggle to ensure the 2-ms spacing
- * between IRQ_HPD.  Since this is used in overridable functions, this
- * has to be global.
- */
-uint64_t svdm_hpd_deadline[CONFIG_USB_PD_PORT_MAX_COUNT];
-
-int dp_flags[CONFIG_USB_PD_PORT_MAX_COUNT];
-
-uint32_t dp_status[CONFIG_USB_PD_PORT_MAX_COUNT];
-
-/* Console command multi-function preference set for a PD port. */
-
-__maybe_unused bool dp_port_mf_allow[CONFIG_USB_PD_PORT_MAX_COUNT] = {
-	[0 ... CONFIG_USB_PD_PORT_MAX_COUNT - 1] = true
-};
-
-__overridable const struct svdm_response svdm_rsp = {
-	.identity = NULL,
-	.svids = NULL,
-	.modes = NULL,
-};
-
-static int pd_get_mode_idx(int port, enum tcpci_msg_type type, uint16_t svid)
-{
-	int amode_idx;
-	struct partner_active_modes *active =
-		pd_get_partner_active_modes(port, type);
-
-	for (amode_idx = 0; amode_idx < PD_AMODE_COUNT; amode_idx++) {
-		if (active->amodes[amode_idx].fx &&
-		    (active->amodes[amode_idx].fx->svid == svid))
-			return amode_idx;
-	}
-	return -1;
-}
-
-static int pd_allocate_mode(int port, enum tcpci_msg_type type, uint16_t svid)
-{
-	int i, j;
-	struct svdm_amode_data *modep;
-	int mode_idx = pd_get_mode_idx(port, type, svid);
-	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
-	struct partner_active_modes *active =
-		pd_get_partner_active_modes(port, type);
-	assert(active);
-
-	if (mode_idx != -1)
-		return mode_idx;
-
-	/* There's no space to enter another mode */
-	if (active->amode_idx == PD_AMODE_COUNT) {
-		CPRINTF("ERR:NO AMODE SPACE\n");
-		return -1;
-	}
-
-	/* Allocate ...  if SVID == 0 enter default supported policy */
-	for (i = 0; i < supported_modes_cnt; i++) {
-		for (j = 0; j < disc->svid_cnt; j++) {
-			const struct svid_mode_data *svidp = &disc->svids[j];
-
-			/*
-			 * Looking for a match between supported_modes and
-			 * discovered SVIDs; must also match the passed-in SVID
-			 * if that was non-zero. Otherwise, go to the next
-			 * discovered SVID.
-			 * TODO(b/155890173): Support AP-directed mode entry
-			 * where the mode is unknown to the TCPM.
-			 */
-			if ((svidp->svid != supported_modes[i].svid) ||
-			    (svid && (svidp->svid != svid)))
-				continue;
-
-			modep = &active->amodes[active->amode_idx];
-			modep->fx = &supported_modes[i];
-			modep->data = &disc->svids[j];
-			active->amode_idx++;
-			return active->amode_idx - 1;
-		}
-	}
-	return -1;
-}
-
-static int validate_mode_request(struct svdm_amode_data *modep, uint16_t svid,
-				 int opos)
-{
-	if (!modep->fx)
-		return 0;
-
-	if (svid != modep->fx->svid) {
-		CPRINTF("ERR:svid r:0x%04x != c:0x%04x\n", svid,
-			modep->fx->svid);
-		return 0;
-	}
-
-	if (opos != modep->opos) {
-		CPRINTF("ERR:opos r:%d != c:%d\n", opos, modep->opos);
-		return 0;
-	}
-
-	return 1;
-}
-
-void pd_prepare_sysjump(void)
-{
-#ifndef CONFIG_ZEPHYR
-	int i;
-
-	/* Exit modes before sysjump so we can cleanly enter again later */
-	for (i = 0; i < board_get_usb_pd_port_count(); i++) {
-		/*
-		 * If the port is not capable of Alternate mode no need to
-		 * send the event.
-		 */
-		if (!pd_alt_mode_capable(i))
-			continue;
-
-		sysjump_task_waiting = task_get_current();
-		task_set_event(PD_PORT_TO_TASK_ID(i), PD_EVENT_SYSJUMP);
-		task_wait_event_mask(TASK_EVENT_SYSJUMP_READY, -1);
-		sysjump_task_waiting = TASK_ID_INVALID;
-	}
-#endif /* CONFIG_ZEPHYR */
-}
-
-#ifdef CONFIG_USB_PD_DP_MODE
-/*
- * This algorithm defaults to choosing higher pin config over lower ones in
- * order to prefer multi-function if desired.
- *
- *  NAME | SIGNALING | OUTPUT TYPE | MULTI-FUNCTION | PIN CONFIG
- * -------------------------------------------------------------
- *  A    |  USB G2   |  ?          | no             | 00_0001
- *  B    |  USB G2   |  ?          | yes            | 00_0010
- *  C    |  DP       |  CONVERTED  | no             | 00_0100
- *  D    |  PD       |  CONVERTED  | yes            | 00_1000
- *  E    |  DP       |  DP         | no             | 01_0000
- *  F    |  PD       |  DP         | yes            | 10_0000
- *
- * if UFP has NOT asserted multi-function preferred code masks away B/D/F
- * leaving only A/C/E.  For single-output dongles that should leave only one
- * possible pin config depending on whether its a converter DP->(VGA|HDMI) or DP
- * output.  If UFP is a USB-C receptacle it may assert C/D/E/F.  The DFP USB-C
- * receptacle must always choose C/D in those cases.
- */
-int pd_dfp_dp_get_pin_mode(int port, uint32_t status)
-{
-	struct svdm_amode_data *modep =
-		pd_get_amode_data(port, TCPCI_MSG_SOP, USB_SID_DISPLAYPORT);
-	uint32_t mode_caps;
-	uint32_t pin_caps;
-	int mf_pref;
-
-	/*
-	 * Default dp_port_mf_allow is true, we allow mf operation
-	 * if UFP_D supports it.
-	 */
-
-	if (IS_ENABLED(CONFIG_CMD_MFALLOW))
-		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]) &&
-			  dp_port_mf_allow[port];
-	else
-		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
-
-	if (!modep)
-		return 0;
-
-	mode_caps = modep->data->mode_vdo[modep->opos - 1];
-
-	/* TODO(crosbug.com/p/39656) revisit with DFP that can be a sink */
-	pin_caps = PD_DP_PIN_CAPS(mode_caps);
-
-	/* if don't want multi-function then ignore those pin configs */
-	if (!mf_pref)
-		pin_caps &= ~MODE_DP_PIN_MF_MASK;
-
-	/* TODO(crosbug.com/p/39656) revisit if DFP drives USB Gen 2 signals */
-	pin_caps &= ~MODE_DP_PIN_BR2_MASK;
-
-	/* if C/D present they have precedence over E/F for USB-C->USB-C */
-	if (pin_caps & (MODE_DP_PIN_C | MODE_DP_PIN_D))
-		pin_caps &= ~(MODE_DP_PIN_E | MODE_DP_PIN_F);
-
-	/* get_next_bit returns undefined for zero */
-	if (!pin_caps)
-		return 0;
-
-	return 1 << get_next_bit(&pin_caps);
-}
-#endif /* CONFIG_USB_PD_DP_MODE */
-
-struct svdm_amode_data *pd_get_amode_data(int port, enum tcpci_msg_type type,
-					  uint16_t svid)
-{
-	int idx = pd_get_mode_idx(port, type, svid);
-	struct partner_active_modes *active =
-		pd_get_partner_active_modes(port, type);
-	assert(active);
-
-	return (idx == -1) ? NULL : &active->amodes[idx];
-}
-
-/*
- * Enter default mode ( payload[0] == 0 ) or attempt to enter mode via svid &
- * opos
- */
-uint32_t pd_dfp_enter_mode(int port, enum tcpci_msg_type type, uint16_t svid,
-			   int opos)
-{
-	int mode_idx = pd_allocate_mode(port, type, svid);
-	struct svdm_amode_data *modep;
-	uint32_t mode_caps;
-
-	if (mode_idx == -1)
-		return 0;
-	modep = &pd_get_partner_active_modes(port, type)->amodes[mode_idx];
-
-	if (!opos) {
-		/* choose the lowest as default */
-		modep->opos = 1;
-	} else if (opos <= modep->data->mode_cnt) {
-		modep->opos = opos;
-	} else {
-		CPRINTS("C%d: Invalid opos %d for SVID %x", port, opos, svid);
-		return 0;
-	}
-
-	mode_caps = modep->data->mode_vdo[modep->opos - 1];
-	if (modep->fx->enter(port, mode_caps) == -1)
-		return 0;
-
-	/*
-	 * Strictly speaking, this should only happen when the request
-	 * has been ACKed.
-	 * For TCPMV1, still set modal flag pre-emptively. For TCPMv2, the modal
-	 * flag is set when the ENTER command is ACK'd for each alt mode that is
-	 * supported.
-	 */
-	if (IS_ENABLED(CONFIG_USB_PD_TCPMV1))
-		pd_set_dfp_enter_mode_flag(port, true);
-
-	/* SVDM to send to UFP for mode entry */
-	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 tcpci_msg_type type, uint16_t svid,
-		     int opos)
-{
-	struct svdm_amode_data *modep;
-	struct partner_active_modes *active =
-		pd_get_partner_active_modes(port, type);
-	int idx;
-
-	/*
-	 * Empty svid signals we should reset DFP VDM state by exiting all
-	 * entered modes then clearing state.  This occurs when we've
-	 * disconnected or for hard reset.
-	 */
-	if (!svid) {
-		for (idx = 0; idx < PD_AMODE_COUNT; idx++)
-			if (active->amodes[idx].fx)
-				active->amodes[idx].fx->exit(port);
-
-		pd_dfp_mode_init(port);
-		return 0;
-	}
-
-	/*
-	 * TODO(crosbug.com/p/33946) : below needs revisited to allow multiple
-	 * mode exit.  Additionally it should honor OPOS == 7 as DFP's request
-	 * to exit all modes.  We currently don't have any UFPs that support
-	 * multiple modes on one SVID.
-	 */
-	modep = pd_get_amode_data(port, type, svid);
-	if (!modep || !validate_mode_request(modep, svid, opos))
-		return 0;
-
-	/* call DFPs exit function */
-	modep->fx->exit(port);
-
-	pd_set_dfp_enter_mode_flag(port, false);
-
-	/* exit the mode */
-	modep->opos = 0;
-	return 1;
-}
-
-void dfp_consume_attention(int port, uint32_t *payload)
-{
-	uint16_t svid = PD_VDO_VID(payload[0]);
-	int opos = PD_VDO_OPOS(payload[0]);
-	struct svdm_amode_data *modep =
-		pd_get_amode_data(port, TCPCI_MSG_SOP, svid);
-
-	if (!modep || !validate_mode_request(modep, svid, opos))
-		return;
-
-	if (modep->fx->attention)
-		modep->fx->attention(port, payload);
-}
-
-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);
-
-	return (modep) ? modep->opos : -1;
-}
-
-void notify_sysjump_ready(void)
-{
-	/*
-	 * If event was set from pd_prepare_sysjump, wake the
-	 * task waiting on us to complete.
-	 */
-	if (sysjump_task_waiting != TASK_ID_INVALID)
-		task_set_event(sysjump_task_waiting, TASK_EVENT_SYSJUMP_READY);
-}
-
-#ifdef CONFIG_USB_PD_DP_MODE
-__overridable void svdm_safe_dp_mode(int port)
-{
-	/* make DP interface safe until configure */
-	dp_flags[port] = 0;
-	dp_status[port] = 0;
-
-	usb_mux_set_safe_mode(port);
-}
-
-__overridable int svdm_enter_dp_mode(int port, uint32_t mode_caps)
-{
-	/*
-	 * Don't enter the mode if the SoC is off.
-	 *
-	 * There's no need to enter the mode while the SoC is off; we'll
-	 * actually enter the mode on the chipset resume hook.  Entering DP Alt
-	 * Mode twice will confuse some monitors and require and unplug/replug
-	 * to get them to work again.  The DP Alt Mode on USB-C spec says that
-	 * if we don't need to maintain HPD connectivity info in a low power
-	 * mode, then we shall exit DP Alt Mode.  (This is why we don't enter
-	 * when the SoC is off as opposed to suspend where adding a display
-	 * could cause a wake up.)  When in S5->S3 transition state, we
-	 * should treat it as a SoC off state.
-	 */
-#ifdef CONFIG_AP_POWER_CONTROL
-	if (!chipset_in_state(CHIPSET_STATE_ANY_SUSPEND | CHIPSET_STATE_ON))
-		return -1;
-#endif
-
-		/*
-		 * TCPMv2: Enable logging of CCD line state CCD_MODE_ODL.
-		 * DisplayPort Alternate mode requires that the SBU lines are
-		 * used for AUX communication. However, in Chromebooks SBU
-		 * signals are repurposed as USB2 signals for CCD. This
-		 * functionality is accomplished by override fets whose state is
-		 * controlled by CCD_MODE_ODL.
-		 *
-		 * This condition helps in debugging unexpected AUX timeout
-		 * issues by indicating the state of the CCD override fets.
-		 */
-#ifdef GPIO_CCD_MODE_ODL
-	if (!gpio_get_level(GPIO_CCD_MODE_ODL))
-		CPRINTS("WARNING: Tried to EnterMode DP with [CCD on AUX/SBU]");
-#endif
-
-	/* Only enter mode if device is DFP_D capable */
-	if (mode_caps & MODE_DP_SNK) {
-		svdm_safe_dp_mode(port);
-
-		if (IS_ENABLED(CONFIG_MKBP_EVENT) &&
-		    chipset_in_state(CHIPSET_STATE_ANY_SUSPEND))
-			/*
-			 * Wake the system up since we're entering DP AltMode.
-			 */
-			pd_notify_dp_alt_mode_entry(port);
-
-		return 0;
-	}
-
-	return -1;
-}
-
-__overridable int svdm_dp_status(int port, uint32_t *payload)
-{
-	int opos = pd_alt_mode(port, TCPCI_MSG_SOP, USB_SID_DISPLAYPORT);
-
-	payload[0] =
-		VDO(USB_SID_DISPLAYPORT, 1, CMD_DP_STATUS | VDO_OPOS(opos));
-	payload[1] = VDO_DP_STATUS(0, /* HPD IRQ  ... not applicable */
-				   0, /* HPD level ... not applicable */
-				   0, /* exit DP? ... no */
-				   0, /* usb mode? ... no */
-				   0, /* multi-function ... no */
-				   (!!(dp_flags[port] & DP_FLAGS_DP_ON)),
-				   0, /* power low? ... no */
-				   (!!DP_FLAGS_DP_ON));
-	return 2;
-};
-
-__overridable uint8_t get_dp_pin_mode(int port)
-{
-	return pd_dfp_dp_get_pin_mode(port, dp_status[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 */
-	int mf_pref;
-
-	if (IS_ENABLED(CONFIG_CMD_MFALLOW))
-		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]) &&
-			  dp_port_mf_allow[port];
-	else
-		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
-
-	/*
-	 * Multi-function operation is only allowed if that pin config is
-	 * supported.
-	 */
-	if ((pin_mode & MODE_DP_PIN_MF_MASK) && mf_pref)
-		return USB_PD_MUX_DOCK;
-	else
-		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);
-	uint8_t pin_mode = get_dp_pin_mode(port);
-	mux_state_t mux_mode = svdm_dp_get_mux_mode(port);
-	/* Default dp_port_mf_allow is true */
-	int mf_pref;
-
-	if (IS_ENABLED(CONFIG_CMD_MFALLOW))
-		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]) &&
-			  dp_port_mf_allow[port];
-	else
-		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
-
-	if (!pin_mode)
-		return 0;
-
-	CPRINTS("pin_mode: %x, mf: %d, mux: %d", pin_mode, mf_pref, mux_mode);
-
-	payload[0] =
-		VDO(USB_SID_DISPLAYPORT, 1, CMD_DP_CONFIG | VDO_OPOS(opos));
-	payload[1] = VDO_DP_CFG(pin_mode, /* pin mode */
-				1, /* DPv1.3 signaling */
-				2); /* UFP connected */
-	return 2;
-};
-
-#if defined(CONFIG_USB_PD_DP_HPD_GPIO) && \
-	!defined(CONFIG_USB_PD_DP_HPD_GPIO_CUSTOM)
-void svdm_set_hpd_gpio(int port, int en)
-{
-	gpio_set_level(PORT_TO_HPD(port), en);
-}
-
-int svdm_get_hpd_gpio(int port)
-{
-	return gpio_get_level(PORT_TO_HPD(port));
-}
-#endif
-
-__overridable void svdm_dp_post_config(int port)
-{
-	mux_state_t mux_mode = svdm_dp_get_mux_mode(port);
-	/* Connect the SBU and USB lines to the connector. */
-	typec_set_sbu(port, true);
-
-	usb_mux_set(port, mux_mode, USB_SWITCH_CONNECT,
-		    polarity_rm_dts(pd_get_polarity(port)));
-
-	dp_flags[port] |= DP_FLAGS_DP_ON;
-	if (!(dp_flags[port] & DP_FLAGS_HPD_HI_PENDING))
-		return;
-
-#ifdef CONFIG_USB_PD_DP_HPD_GPIO
-	svdm_set_hpd_gpio(port, 1);
-
-	/* set the minimum time delay (2ms) for the next HPD IRQ */
-	svdm_hpd_deadline[port] = get_time().val + HPD_USTREAM_DEBOUNCE_LVL;
-#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
-
-	usb_mux_hpd_update(port,
-			   USB_PD_MUX_HPD_LVL | USB_PD_MUX_HPD_IRQ_DEASSERTED);
-
-#ifdef USB_PD_PORT_TCPC_MST
-	if (port == USB_PD_PORT_TCPC_MST)
-		baseboard_mst_enable_control(port, 1);
-#endif
-}
-
-__overridable int svdm_dp_attention(int port, uint32_t *payload)
-{
-	int lvl = PD_VDO_DPSTS_HPD_LVL(payload[1]);
-	int irq = PD_VDO_DPSTS_HPD_IRQ(payload[1]);
-#ifdef CONFIG_USB_PD_DP_HPD_GPIO
-	int cur_lvl = svdm_get_hpd_gpio(port);
-#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
-	mux_state_t mux_state;
-
-	dp_status[port] = payload[1];
-
-	if (chipset_in_state(CHIPSET_STATE_ANY_SUSPEND) && (irq || lvl))
-		/*
-		 * Wake up the AP.  IRQ or level high indicates a DP sink is now
-		 * present.
-		 */
-		if (IS_ENABLED(CONFIG_MKBP_EVENT))
-			pd_notify_dp_alt_mode_entry(port);
-
-	/* Its initial DP status message prior to config */
-	if (!(dp_flags[port] & DP_FLAGS_DP_ON)) {
-		if (lvl)
-			dp_flags[port] |= DP_FLAGS_HPD_HI_PENDING;
-		return 1;
-	}
-
-#ifdef CONFIG_USB_PD_DP_HPD_GPIO
-	if (irq && !lvl) {
-		/*
-		 * IRQ can only be generated when the level is high, because
-		 * the IRQ is signaled by a short low pulse from the high level.
-		 */
-		CPRINTF("ERR:HPD:IRQ&LOW\n");
-		return 0; /* nak */
-	}
-
-	if (irq && cur_lvl) {
-		uint64_t now = get_time().val;
-		/* wait for the minimum spacing between IRQ_HPD if needed */
-		if (now < svdm_hpd_deadline[port])
-			usleep(svdm_hpd_deadline[port] - now);
-
-		/* generate IRQ_HPD pulse */
-		svdm_set_hpd_gpio(port, 0);
-		usleep(HPD_DSTREAM_DEBOUNCE_IRQ);
-		svdm_set_hpd_gpio(port, 1);
-	} else {
-		svdm_set_hpd_gpio(port, lvl);
-	}
-
-	/* set the minimum time delay (2ms) for the next HPD IRQ */
-	svdm_hpd_deadline[port] = get_time().val + HPD_USTREAM_DEBOUNCE_LVL;
-#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
-
-	mux_state = (lvl ? USB_PD_MUX_HPD_LVL : USB_PD_MUX_HPD_LVL_DEASSERTED) |
-		    (irq ? USB_PD_MUX_HPD_IRQ : USB_PD_MUX_HPD_IRQ_DEASSERTED);
-	usb_mux_hpd_update(port, mux_state);
-
-#ifdef USB_PD_PORT_TCPC_MST
-	if (port == USB_PD_PORT_TCPC_MST)
-		baseboard_mst_enable_control(port, lvl);
-#endif
-
-	/* ack */
-	return 1;
-}
-
-__overridable void svdm_exit_dp_mode(int port)
-{
-	dp_flags[port] = 0;
-	dp_status[port] = 0;
-#ifdef CONFIG_USB_PD_DP_HPD_GPIO
-	svdm_set_hpd_gpio(port, 0);
-#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
-	usb_mux_hpd_update(port, USB_PD_MUX_HPD_LVL_DEASSERTED |
-					 USB_PD_MUX_HPD_IRQ_DEASSERTED);
-#ifdef USB_PD_PORT_TCPC_MST
-	if (port == USB_PD_PORT_TCPC_MST)
-		baseboard_mst_enable_control(port, 0);
-#endif
-}
-#endif /* CONFIG_USB_PD_DP_MODE */
-
-#ifdef CONFIG_USB_PD_TCPMV1
-__overridable int svdm_enter_gfu_mode(int port, uint32_t mode_caps)
-{
-	/* Always enter GFU mode */
-	return 0;
-}
-
-__overridable void svdm_exit_gfu_mode(int port)
-{
-}
-
-__overridable int svdm_gfu_status(int port, uint32_t *payload)
-{
-	/*
-	 * This is called after enter mode is successful, send unstructured
-	 * VDM to read info.
-	 */
-	pd_send_vdm(port, USB_VID_GOOGLE, VDO_CMD_READ_INFO, NULL, 0);
-	return 0;
-}
-
-__overridable int svdm_gfu_config(int port, uint32_t *payload)
-{
-	return 0;
-}
-
-__overridable int svdm_gfu_attention(int port, uint32_t *payload)
-{
-	return 0;
-}
-#endif /* CONFIG_USB_PD_TCPMV1 */
-
-#ifdef CONFIG_USB_PD_TBT_COMPAT_MODE
-__overridable int svdm_tbt_compat_enter_mode(int port, uint32_t mode_caps)
-{
-	return 0;
-}
-
-__overridable void svdm_tbt_compat_exit_mode(int port)
-{
-}
-
-__overridable int svdm_tbt_compat_status(int port, uint32_t *payload)
-{
-	return 0;
-}
-
-__overridable int svdm_tbt_compat_config(int port, uint32_t *payload)
-{
-	return 0;
-}
-
-__overridable int svdm_tbt_compat_attention(int port, uint32_t *payload)
-{
-	return 0;
-}
-#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[] = {
-#ifdef CONFIG_USB_PD_DP_MODE
-	{
-		.svid = USB_SID_DISPLAYPORT,
-		.enter = &svdm_enter_dp_mode,
-		.status = &svdm_dp_status,
-		.config = &svdm_dp_config,
-		.post_config = &svdm_dp_post_config,
-		.attention = &svdm_dp_attention,
-		.exit = &svdm_exit_dp_mode,
-	},
-#endif /* CONFIG_USB_PD_DP_MODE */
-#ifdef CONFIG_USB_PD_TCPMV1
-	{
-		.svid = USB_VID_GOOGLE,
-		.enter = &svdm_enter_gfu_mode,
-		.status = &svdm_gfu_status,
-		.config = &svdm_gfu_config,
-		.attention = &svdm_gfu_attention,
-		.exit = &svdm_exit_gfu_mode,
-	},
-#endif /* CONFIG_USB_PD_TCPMV1 */
-#ifdef CONFIG_USB_PD_TBT_COMPAT_MODE
-	{
-		.svid = USB_VID_INTEL,
-		.enter = &svdm_tbt_compat_enter_mode,
-		.status = &svdm_tbt_compat_status,
-		.config = &svdm_tbt_compat_config,
-		.attention = &svdm_tbt_compat_attention,
-		.exit = &svdm_tbt_compat_exit_mode,
-	},
-#endif /* CONFIG_USB_PD_TBT_COMPAT_MODE */
-};
-const int supported_modes_cnt = ARRAY_SIZE(supported_modes);
-
-#if defined(CONFIG_CMD_MFALLOW)
-static int command_mfallow(int argc, const char **argv)
-{
-	char *e;
-	int port;
-
-	if (argc < 3)
-		return EC_ERROR_PARAM_COUNT;
-
-	port = strtoi(argv[1], &e, 10);
-	if (*e || port >= board_get_usb_pd_port_count())
-		return EC_ERROR_PARAM2;
-
-	if (!strcasecmp(argv[2], "true"))
-		dp_port_mf_allow[port] = true;
-	else if (!strcasecmp(argv[2], "false"))
-		dp_port_mf_allow[port] = false;
-	else
-		return EC_ERROR_PARAM1;
-
-	ccprintf("Port: %d multi function allowed is %s ", port, argv[2]);
-	return EC_SUCCESS;
-}
-
-DECLARE_CONSOLE_COMMAND(mfallow, command_mfallow, "port [true | false]",
-			"Controls Multifunction choice during DP Altmode.");
-#endif
diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index 1678536d50..38cfd226d2 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -6,6 +6,7 @@
 #include "atomic.h"
 #include "builtin/assert.h"
 #include "charge_manager.h"
+#include "chipset.h"
 #include "common.h"
 #include "console.h"
 #include "cros_version.h"
@@ -22,6 +23,7 @@
 #include "task.h"
 #include "tcpm/tcpm.h"
 #include "timer.h"
+#include "typec_control.h"
 #include "usb_api.h"
 #include "usb_common.h"
 #include "usb_mux.h"
@@ -46,6 +48,38 @@
 #error This file must only be used with TCPMv1
 #endif
 
+#ifdef CONFIG_USB_PD_ALT_MODE_DFP
+#ifndef PORT_TO_HPD
+#define PORT_TO_HPD(port) ((port) ? GPIO_USB_C1_DP_HPD : GPIO_USB_C0_DP_HPD)
+#endif /* PORT_TO_HPD */
+
+/* Tracker for which task is waiting on sysjump prep to finish */
+static volatile task_id_t sysjump_task_waiting = TASK_ID_INVALID;
+
+/*
+ * timestamp of the next possible toggle to ensure the 2-ms spacing
+ * between IRQ_HPD.  Since this is used in overridable functions, this
+ * has to be global.
+ */
+uint64_t svdm_hpd_deadline[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+int dp_flags[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+uint32_t dp_status[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+/* Console command multi-function preference set for a PD port. */
+
+__maybe_unused bool dp_port_mf_allow[CONFIG_USB_PD_PORT_MAX_COUNT] = {
+	[0 ... CONFIG_USB_PD_PORT_MAX_COUNT - 1] = true
+};
+
+__overridable const struct svdm_response svdm_rsp = {
+	.identity = NULL,
+	.svids = NULL,
+	.modes = NULL,
+};
+#endif /* CONFIG_USB_PD_ALT_MODE_DFP */
+
 static int rw_flash_changed = 1;
 
 __overridable void pd_check_pr_role(int port, enum pd_power_role pr_role,
@@ -573,4 +607,637 @@ static enum ec_status hc_remote_pd_get_amode(struct host_cmd_handler_args *args)
 DECLARE_HOST_COMMAND(EC_CMD_USB_PD_GET_AMODE, hc_remote_pd_get_amode,
 		     EC_VER_MASK(0));
 
+static int pd_get_mode_idx(int port, enum tcpci_msg_type type, uint16_t svid)
+{
+	int amode_idx;
+	struct partner_active_modes *active =
+		pd_get_partner_active_modes(port, type);
+
+	for (amode_idx = 0; amode_idx < PD_AMODE_COUNT; amode_idx++) {
+		if (active->amodes[amode_idx].fx &&
+		    (active->amodes[amode_idx].fx->svid == svid))
+			return amode_idx;
+	}
+	return -1;
+}
+
+static int pd_allocate_mode(int port, enum tcpci_msg_type type, uint16_t svid)
+{
+	int i, j;
+	struct svdm_amode_data *modep;
+	int mode_idx = pd_get_mode_idx(port, type, svid);
+	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
+	struct partner_active_modes *active =
+		pd_get_partner_active_modes(port, type);
+	assert(active);
+
+	if (mode_idx != -1)
+		return mode_idx;
+
+	/* There's no space to enter another mode */
+	if (active->amode_idx == PD_AMODE_COUNT) {
+		CPRINTF("ERR:NO AMODE SPACE\n");
+		return -1;
+	}
+
+	/* Allocate ...  if SVID == 0 enter default supported policy */
+	for (i = 0; i < supported_modes_cnt; i++) {
+		for (j = 0; j < disc->svid_cnt; j++) {
+			const struct svid_mode_data *svidp = &disc->svids[j];
+
+			/*
+			 * Looking for a match between supported_modes and
+			 * discovered SVIDs; must also match the passed-in SVID
+			 * if that was non-zero. Otherwise, go to the next
+			 * discovered SVID.
+			 * TODO(b/155890173): Support AP-directed mode entry
+			 * where the mode is unknown to the TCPM.
+			 */
+			if ((svidp->svid != supported_modes[i].svid) ||
+			    (svid && (svidp->svid != svid)))
+				continue;
+
+			modep = &active->amodes[active->amode_idx];
+			modep->fx = &supported_modes[i];
+			modep->data = &disc->svids[j];
+			active->amode_idx++;
+			return active->amode_idx - 1;
+		}
+	}
+	return -1;
+}
+
+static int validate_mode_request(struct svdm_amode_data *modep, uint16_t svid,
+				 int opos)
+{
+	if (!modep->fx)
+		return 0;
+
+	if (svid != modep->fx->svid) {
+		CPRINTF("ERR:svid r:0x%04x != c:0x%04x\n", svid,
+			modep->fx->svid);
+		return 0;
+	}
+
+	if (opos != modep->opos) {
+		CPRINTF("ERR:opos r:%d != c:%d\n", opos, modep->opos);
+		return 0;
+	}
+
+	return 1;
+}
+
+void pd_prepare_sysjump(void)
+{
+	int i;
+
+	/* Exit modes before sysjump so we can cleanly enter again later */
+	for (i = 0; i < board_get_usb_pd_port_count(); i++) {
+		/*
+		 * If the port is not capable of Alternate mode no need to
+		 * send the event.
+		 */
+		if (!pd_alt_mode_capable(i))
+			continue;
+
+		sysjump_task_waiting = task_get_current();
+		task_set_event(PD_PORT_TO_TASK_ID(i), PD_EVENT_SYSJUMP);
+		task_wait_event_mask(TASK_EVENT_SYSJUMP_READY, -1);
+		sysjump_task_waiting = TASK_ID_INVALID;
+	}
+}
+
+#ifdef CONFIG_USB_PD_DP_MODE
+/*
+ * This algorithm defaults to choosing higher pin config over lower ones in
+ * order to prefer multi-function if desired.
+ *
+ *  NAME | SIGNALING | OUTPUT TYPE | MULTI-FUNCTION | PIN CONFIG
+ * -------------------------------------------------------------
+ *  A    |  USB G2   |  ?          | no             | 00_0001
+ *  B    |  USB G2   |  ?          | yes            | 00_0010
+ *  C    |  DP       |  CONVERTED  | no             | 00_0100
+ *  D    |  PD       |  CONVERTED  | yes            | 00_1000
+ *  E    |  DP       |  DP         | no             | 01_0000
+ *  F    |  PD       |  DP         | yes            | 10_0000
+ *
+ * if UFP has NOT asserted multi-function preferred code masks away B/D/F
+ * leaving only A/C/E.  For single-output dongles that should leave only one
+ * possible pin config depending on whether its a converter DP->(VGA|HDMI) or DP
+ * output.  If UFP is a USB-C receptacle it may assert C/D/E/F.  The DFP USB-C
+ * receptacle must always choose C/D in those cases.
+ */
+int pd_dfp_dp_get_pin_mode(int port, uint32_t status)
+{
+	struct svdm_amode_data *modep =
+		pd_get_amode_data(port, TCPCI_MSG_SOP, USB_SID_DISPLAYPORT);
+	uint32_t mode_caps;
+	uint32_t pin_caps;
+	int mf_pref;
+
+	/*
+	 * Default dp_port_mf_allow is true, we allow mf operation
+	 * if UFP_D supports it.
+	 */
+
+	if (IS_ENABLED(CONFIG_CMD_MFALLOW))
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]) &&
+			  dp_port_mf_allow[port];
+	else
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
+
+	if (!modep)
+		return 0;
+
+	mode_caps = modep->data->mode_vdo[modep->opos - 1];
+
+	/* TODO(crosbug.com/p/39656) revisit with DFP that can be a sink */
+	pin_caps = PD_DP_PIN_CAPS(mode_caps);
+
+	/* if don't want multi-function then ignore those pin configs */
+	if (!mf_pref)
+		pin_caps &= ~MODE_DP_PIN_MF_MASK;
+
+	/* TODO(crosbug.com/p/39656) revisit if DFP drives USB Gen 2 signals */
+	pin_caps &= ~MODE_DP_PIN_BR2_MASK;
+
+	/* if C/D present they have precedence over E/F for USB-C->USB-C */
+	if (pin_caps & (MODE_DP_PIN_C | MODE_DP_PIN_D))
+		pin_caps &= ~(MODE_DP_PIN_E | MODE_DP_PIN_F);
+
+	/* get_next_bit returns undefined for zero */
+	if (!pin_caps)
+		return 0;
+
+	return 1 << get_next_bit(&pin_caps);
+}
+#endif /* CONFIG_USB_PD_DP_MODE */
+
+struct svdm_amode_data *pd_get_amode_data(int port, enum tcpci_msg_type type,
+					  uint16_t svid)
+{
+	int idx = pd_get_mode_idx(port, type, svid);
+	struct partner_active_modes *active =
+		pd_get_partner_active_modes(port, type);
+	assert(active);
+
+	return (idx == -1) ? NULL : &active->amodes[idx];
+}
+
+/*
+ * Enter default mode ( payload[0] == 0 ) or attempt to enter mode via svid &
+ * opos
+ */
+uint32_t pd_dfp_enter_mode(int port, enum tcpci_msg_type type, uint16_t svid,
+			   int opos)
+{
+	int mode_idx = pd_allocate_mode(port, type, svid);
+	struct svdm_amode_data *modep;
+	uint32_t mode_caps;
+
+	if (mode_idx == -1)
+		return 0;
+	modep = &pd_get_partner_active_modes(port, type)->amodes[mode_idx];
+
+	if (!opos) {
+		/* choose the lowest as default */
+		modep->opos = 1;
+	} else if (opos <= modep->data->mode_cnt) {
+		modep->opos = opos;
+	} else {
+		CPRINTS("C%d: Invalid opos %d for SVID %x", port, opos, svid);
+		return 0;
+	}
+
+	mode_caps = modep->data->mode_vdo[modep->opos - 1];
+	if (modep->fx->enter(port, mode_caps) == -1)
+		return 0;
+
+	/*
+	 * Strictly speaking, this should only happen when the request
+	 * has been ACKed.
+	 * For TCPMV1, still set modal flag pre-emptively. For TCPMv2, the modal
+	 * flag is set when the ENTER command is ACK'd for each alt mode that is
+	 * supported.
+	 */
+	if (IS_ENABLED(CONFIG_USB_PD_TCPMV1))
+		pd_set_dfp_enter_mode_flag(port, true);
+
+	/* SVDM to send to UFP for mode entry */
+	return VDO(modep->fx->svid, 1, CMD_ENTER_MODE | VDO_OPOS(modep->opos));
+}
+
+int pd_dfp_exit_mode(int port, enum tcpci_msg_type type, uint16_t svid,
+		     int opos)
+{
+	struct svdm_amode_data *modep;
+	struct partner_active_modes *active =
+		pd_get_partner_active_modes(port, type);
+	int idx;
+
+	/*
+	 * Empty svid signals we should reset DFP VDM state by exiting all
+	 * entered modes then clearing state.  This occurs when we've
+	 * disconnected or for hard reset.
+	 */
+	if (!svid) {
+		for (idx = 0; idx < PD_AMODE_COUNT; idx++)
+			if (active->amodes[idx].fx)
+				active->amodes[idx].fx->exit(port);
+
+		pd_dfp_mode_init(port);
+		return 0;
+	}
+
+	/*
+	 * TODO(crosbug.com/p/33946) : below needs revisited to allow multiple
+	 * mode exit.  Additionally it should honor OPOS == 7 as DFP's request
+	 * to exit all modes.  We currently don't have any UFPs that support
+	 * multiple modes on one SVID.
+	 */
+	modep = pd_get_amode_data(port, type, svid);
+	if (!modep || !validate_mode_request(modep, svid, opos))
+		return 0;
+
+	/* call DFPs exit function */
+	modep->fx->exit(port);
+
+	pd_set_dfp_enter_mode_flag(port, false);
+
+	/* exit the mode */
+	modep->opos = 0;
+	return 1;
+}
+
+void dfp_consume_attention(int port, uint32_t *payload)
+{
+	uint16_t svid = PD_VDO_VID(payload[0]);
+	int opos = PD_VDO_OPOS(payload[0]);
+	struct svdm_amode_data *modep =
+		pd_get_amode_data(port, TCPCI_MSG_SOP, svid);
+
+	if (!modep || !validate_mode_request(modep, svid, opos))
+		return;
+
+	if (modep->fx->attention)
+		modep->fx->attention(port, payload);
+}
+
+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);
+
+	return (modep) ? modep->opos : -1;
+}
+
+void notify_sysjump_ready(void)
+{
+	/*
+	 * If event was set from pd_prepare_sysjump, wake the
+	 * task waiting on us to complete.
+	 */
+	if (sysjump_task_waiting != TASK_ID_INVALID)
+		task_set_event(sysjump_task_waiting, TASK_EVENT_SYSJUMP_READY);
+}
+
+#ifdef CONFIG_USB_PD_DP_MODE
+__overridable void svdm_safe_dp_mode(int port)
+{
+	/* make DP interface safe until configure */
+	dp_flags[port] = 0;
+	dp_status[port] = 0;
+
+	usb_mux_set_safe_mode(port);
+}
+
+__overridable int svdm_enter_dp_mode(int port, uint32_t mode_caps)
+{
+	/*
+	 * Don't enter the mode if the SoC is off.
+	 *
+	 * There's no need to enter the mode while the SoC is off; we'll
+	 * actually enter the mode on the chipset resume hook.  Entering DP Alt
+	 * Mode twice will confuse some monitors and require and unplug/replug
+	 * to get them to work again.  The DP Alt Mode on USB-C spec says that
+	 * if we don't need to maintain HPD connectivity info in a low power
+	 * mode, then we shall exit DP Alt Mode.  (This is why we don't enter
+	 * when the SoC is off as opposed to suspend where adding a display
+	 * could cause a wake up.)  When in S5->S3 transition state, we
+	 * should treat it as a SoC off state.
+	 */
+#ifdef CONFIG_AP_POWER_CONTROL
+	if (!chipset_in_state(CHIPSET_STATE_ANY_SUSPEND | CHIPSET_STATE_ON))
+		return -1;
+#endif
+
+		/*
+		 * TCPMv2: Enable logging of CCD line state CCD_MODE_ODL.
+		 * DisplayPort Alternate mode requires that the SBU lines are
+		 * used for AUX communication. However, in Chromebooks SBU
+		 * signals are repurposed as USB2 signals for CCD. This
+		 * functionality is accomplished by override fets whose state is
+		 * controlled by CCD_MODE_ODL.
+		 *
+		 * This condition helps in debugging unexpected AUX timeout
+		 * issues by indicating the state of the CCD override fets.
+		 */
+#ifdef GPIO_CCD_MODE_ODL
+	if (!gpio_get_level(GPIO_CCD_MODE_ODL))
+		CPRINTS("WARNING: Tried to EnterMode DP with [CCD on AUX/SBU]");
+#endif
+
+	/* Only enter mode if device is DFP_D capable */
+	if (mode_caps & MODE_DP_SNK) {
+		svdm_safe_dp_mode(port);
+
+		if (IS_ENABLED(CONFIG_MKBP_EVENT) &&
+		    chipset_in_state(CHIPSET_STATE_ANY_SUSPEND))
+			/*
+			 * Wake the system up since we're entering DP AltMode.
+			 */
+			pd_notify_dp_alt_mode_entry(port);
+
+		return 0;
+	}
+
+	return -1;
+}
+
+__overridable int svdm_dp_status(int port, uint32_t *payload)
+{
+	int opos = pd_alt_mode(port, TCPCI_MSG_SOP, USB_SID_DISPLAYPORT);
+
+	payload[0] =
+		VDO(USB_SID_DISPLAYPORT, 1, CMD_DP_STATUS | VDO_OPOS(opos));
+	payload[1] = VDO_DP_STATUS(0, /* HPD IRQ  ... not applicable */
+				   0, /* HPD level ... not applicable */
+				   0, /* exit DP? ... no */
+				   0, /* usb mode? ... no */
+				   0, /* multi-function ... no */
+				   (!!(dp_flags[port] & DP_FLAGS_DP_ON)),
+				   0, /* power low? ... no */
+				   (!!DP_FLAGS_DP_ON));
+	return 2;
+};
+
+__overridable uint8_t get_dp_pin_mode(int port)
+{
+	return pd_dfp_dp_get_pin_mode(port, dp_status[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 */
+	int mf_pref;
+
+	if (IS_ENABLED(CONFIG_CMD_MFALLOW))
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]) &&
+			  dp_port_mf_allow[port];
+	else
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
+
+	/*
+	 * Multi-function operation is only allowed if that pin config is
+	 * supported.
+	 */
+	if ((pin_mode & MODE_DP_PIN_MF_MASK) && mf_pref)
+		return USB_PD_MUX_DOCK;
+	else
+		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);
+	uint8_t pin_mode = get_dp_pin_mode(port);
+	mux_state_t mux_mode = svdm_dp_get_mux_mode(port);
+	/* Default dp_port_mf_allow is true */
+	int mf_pref;
+
+	if (IS_ENABLED(CONFIG_CMD_MFALLOW))
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]) &&
+			  dp_port_mf_allow[port];
+	else
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
+
+	if (!pin_mode)
+		return 0;
+
+	CPRINTS("pin_mode: %x, mf: %d, mux: %d", pin_mode, mf_pref, mux_mode);
+
+	payload[0] =
+		VDO(USB_SID_DISPLAYPORT, 1, CMD_DP_CONFIG | VDO_OPOS(opos));
+	payload[1] = VDO_DP_CFG(pin_mode, /* pin mode */
+				1, /* DPv1.3 signaling */
+				2); /* UFP connected */
+	return 2;
+};
+
+#if defined(CONFIG_USB_PD_DP_HPD_GPIO) && \
+	!defined(CONFIG_USB_PD_DP_HPD_GPIO_CUSTOM)
+void svdm_set_hpd_gpio(int port, int en)
+{
+	gpio_set_level(PORT_TO_HPD(port), en);
+}
+
+int svdm_get_hpd_gpio(int port)
+{
+	return gpio_get_level(PORT_TO_HPD(port));
+}
+#endif
+
+__overridable void svdm_dp_post_config(int port)
+{
+	mux_state_t mux_mode = svdm_dp_get_mux_mode(port);
+	/* Connect the SBU and USB lines to the connector. */
+	typec_set_sbu(port, true);
+
+	usb_mux_set(port, mux_mode, USB_SWITCH_CONNECT,
+		    polarity_rm_dts(pd_get_polarity(port)));
+
+	dp_flags[port] |= DP_FLAGS_DP_ON;
+	if (!(dp_flags[port] & DP_FLAGS_HPD_HI_PENDING))
+		return;
+
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	svdm_set_hpd_gpio(port, 1);
+
+	/* set the minimum time delay (2ms) for the next HPD IRQ */
+	svdm_hpd_deadline[port] = get_time().val + HPD_USTREAM_DEBOUNCE_LVL;
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+
+	usb_mux_hpd_update(port,
+			   USB_PD_MUX_HPD_LVL | USB_PD_MUX_HPD_IRQ_DEASSERTED);
+
+#ifdef USB_PD_PORT_TCPC_MST
+	if (port == USB_PD_PORT_TCPC_MST)
+		baseboard_mst_enable_control(port, 1);
+#endif
+}
+
+__overridable int svdm_dp_attention(int port, uint32_t *payload)
+{
+	int lvl = PD_VDO_DPSTS_HPD_LVL(payload[1]);
+	int irq = PD_VDO_DPSTS_HPD_IRQ(payload[1]);
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	int cur_lvl = svdm_get_hpd_gpio(port);
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+	mux_state_t mux_state;
+
+	dp_status[port] = payload[1];
+
+	if (chipset_in_state(CHIPSET_STATE_ANY_SUSPEND) && (irq || lvl))
+		/*
+		 * Wake up the AP.  IRQ or level high indicates a DP sink is now
+		 * present.
+		 */
+		if (IS_ENABLED(CONFIG_MKBP_EVENT))
+			pd_notify_dp_alt_mode_entry(port);
+
+	/* Its initial DP status message prior to config */
+	if (!(dp_flags[port] & DP_FLAGS_DP_ON)) {
+		if (lvl)
+			dp_flags[port] |= DP_FLAGS_HPD_HI_PENDING;
+		return 1;
+	}
+
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	if (irq && !lvl) {
+		/*
+		 * IRQ can only be generated when the level is high, because
+		 * the IRQ is signaled by a short low pulse from the high level.
+		 */
+		CPRINTF("ERR:HPD:IRQ&LOW\n");
+		return 0; /* nak */
+	}
+
+	if (irq && cur_lvl) {
+		uint64_t now = get_time().val;
+		/* wait for the minimum spacing between IRQ_HPD if needed */
+		if (now < svdm_hpd_deadline[port])
+			usleep(svdm_hpd_deadline[port] - now);
+
+		/* generate IRQ_HPD pulse */
+		svdm_set_hpd_gpio(port, 0);
+		usleep(HPD_DSTREAM_DEBOUNCE_IRQ);
+		svdm_set_hpd_gpio(port, 1);
+	} else {
+		svdm_set_hpd_gpio(port, lvl);
+	}
+
+	/* set the minimum time delay (2ms) for the next HPD IRQ */
+	svdm_hpd_deadline[port] = get_time().val + HPD_USTREAM_DEBOUNCE_LVL;
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+
+	mux_state = (lvl ? USB_PD_MUX_HPD_LVL : USB_PD_MUX_HPD_LVL_DEASSERTED) |
+		    (irq ? USB_PD_MUX_HPD_IRQ : USB_PD_MUX_HPD_IRQ_DEASSERTED);
+	usb_mux_hpd_update(port, mux_state);
+
+#ifdef USB_PD_PORT_TCPC_MST
+	if (port == USB_PD_PORT_TCPC_MST)
+		baseboard_mst_enable_control(port, lvl);
+#endif
+
+	/* ack */
+	return 1;
+}
+
+__overridable void svdm_exit_dp_mode(int port)
+{
+	dp_flags[port] = 0;
+	dp_status[port] = 0;
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	svdm_set_hpd_gpio(port, 0);
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+	usb_mux_hpd_update(port, USB_PD_MUX_HPD_LVL_DEASSERTED |
+					 USB_PD_MUX_HPD_IRQ_DEASSERTED);
+#ifdef USB_PD_PORT_TCPC_MST
+	if (port == USB_PD_PORT_TCPC_MST)
+		baseboard_mst_enable_control(port, 0);
+#endif
+}
+#endif /* CONFIG_USB_PD_DP_MODE */
+
+__overridable int svdm_enter_gfu_mode(int port, uint32_t mode_caps)
+{
+	/* Always enter GFU mode */
+	return 0;
+}
+
+__overridable void svdm_exit_gfu_mode(int port)
+{
+}
+
+__overridable int svdm_gfu_status(int port, uint32_t *payload)
+{
+	/*
+	 * This is called after enter mode is successful, send unstructured
+	 * VDM to read info.
+	 */
+	pd_send_vdm(port, USB_VID_GOOGLE, VDO_CMD_READ_INFO, NULL, 0);
+	return 0;
+}
+
+__overridable int svdm_gfu_config(int port, uint32_t *payload)
+{
+	return 0;
+}
+
+__overridable int svdm_gfu_attention(int port, uint32_t *payload)
+{
+	return 0;
+}
+
+const struct svdm_amode_fx supported_modes[] = {
+#ifdef CONFIG_USB_PD_DP_MODE
+	{
+		.svid = USB_SID_DISPLAYPORT,
+		.enter = &svdm_enter_dp_mode,
+		.status = &svdm_dp_status,
+		.config = &svdm_dp_config,
+		.post_config = &svdm_dp_post_config,
+		.attention = &svdm_dp_attention,
+		.exit = &svdm_exit_dp_mode,
+	},
+#endif /* CONFIG_USB_PD_DP_MODE */
+	{
+		.svid = USB_VID_GOOGLE,
+		.enter = &svdm_enter_gfu_mode,
+		.status = &svdm_gfu_status,
+		.config = &svdm_gfu_config,
+		.attention = &svdm_gfu_attention,
+		.exit = &svdm_exit_gfu_mode,
+	},
+};
+const int supported_modes_cnt = ARRAY_SIZE(supported_modes);
+
+#if defined(CONFIG_CMD_MFALLOW)
+static int command_mfallow(int argc, const char **argv)
+{
+	char *e;
+	int port;
+
+	if (argc < 3)
+		return EC_ERROR_PARAM_COUNT;
+
+	port = strtoi(argv[1], &e, 10);
+	if (*e || port >= board_get_usb_pd_port_count())
+		return EC_ERROR_PARAM2;
+
+	if (!strcasecmp(argv[2], "true"))
+		dp_port_mf_allow[port] = true;
+	else if (!strcasecmp(argv[2], "false"))
+		dp_port_mf_allow[port] = false;
+	else
+		return EC_ERROR_PARAM1;
+
+	ccprintf("Port: %d multi function allowed is %s ", port, argv[2]);
+	return EC_SUCCESS;
+}
+
+DECLARE_CONSOLE_COMMAND(mfallow, command_mfallow, "port [true | false]",
+			"Controls Multifunction choice during DP Altmode.");
+#endif /* CONFIG_CMD_MFALLOW */
 #endif /* CONFIG_USB_PD_ALT_MODE_DFP */
diff --git a/common/usbc/dp_alt_mode.c b/common/usbc/dp_alt_mode.c
index 22975aff5b..7ece3573a8 100644
--- a/common/usbc/dp_alt_mode.c
+++ b/common/usbc/dp_alt_mode.c
@@ -11,9 +11,14 @@
 
 #include "atomic.h"
 #include "builtin/assert.h"
+#include "chipset.h"
 #include "console.h"
+#include "gpio.h"
+#include "timer.h"
+#include "typec_control.h"
 #include "usb_common.h"
 #include "usb_dp_alt_mode.h"
+#include "usb_mux.h"
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 
@@ -28,6 +33,33 @@
 #define CPRINTS(format, args...)
 #endif
 
+/* TODO(b/270409742): Remove this macro system for determining the GPIO */
+#ifndef PORT_TO_HPD
+#define PORT_TO_HPD(port) ((port) ? GPIO_USB_C1_DP_HPD : GPIO_USB_C0_DP_HPD)
+#endif /* PORT_TO_HPD */
+
+/*
+ * Note: the following DP-related variables must be kept as-is since
+ * some boards are using them in their board-specific code.
+ * TODO(b/267545470): Fold board DP code into the DP module
+ */
+
+/*
+ * timestamp of the next possible toggle to ensure the 2-ms spacing
+ * between IRQ_HPD.  Since this is used in overridable functions, this
+ * has to be global.
+ */
+uint64_t svdm_hpd_deadline[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+int dp_flags[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+uint32_t dp_status[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+/* Console command multi-function preference set for a PD port. */
+__maybe_unused bool dp_port_mf_allow[CONFIG_USB_PD_PORT_MAX_COUNT] = {
+	[0 ... CONFIG_USB_PD_PORT_MAX_COUNT - 1] = true
+};
+
 /* The state of the DP negotiation */
 enum dp_states {
 	DP_START = 0,
@@ -64,6 +96,9 @@ static atomic_t dpm_dp_flags[CONFIG_USB_PD_PORT_MAX_COUNT];
 #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))
 
+/* Note: There is only one DP mode currently specified */
+static const int dp_opos = 1;
+
 bool dp_is_active(int port)
 {
 	return dp_state[port] == DP_ACTIVE || dp_state[port] == DP_PREPARE_EXIT;
@@ -116,9 +151,9 @@ static bool dp_response_valid(int port, enum tcpci_msg_type type, char *cmdt,
 
 static void dp_exit_to_usb_mode(int port)
 {
-	int opos = pd_alt_mode(port, TCPCI_MSG_SOP, USB_SID_DISPLAYPORT);
+	svdm_exit_dp_mode(port);
+	pd_set_dfp_enter_mode_flag(port, false);
 
-	pd_dfp_exit_mode(port, TCPCI_MSG_SOP, USB_SID_DISPLAYPORT, opos);
 	set_usb_mux_with_current_data_role(port);
 
 	CPRINTS("C%d: Exited DP mode", port);
@@ -134,8 +169,6 @@ static void dp_exit_to_usb_mode(int port)
 void dp_vdm_acked(int port, enum tcpci_msg_type type, int vdo_count,
 		  uint32_t *vdm)
 {
-	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]);
 
 	if (!dp_response_valid(port, type, "ACK", vdm_cmd))
@@ -156,8 +189,7 @@ void dp_vdm_acked(int port, enum tcpci_msg_type type, int vdo_count,
 		dp_state[port] = DP_STATUS_ACKED;
 		break;
 	case DP_PREPARE_CONFIG:
-		if (modep && modep->opos && modep->fx->post_config)
-			modep->fx->post_config(port);
+		svdm_dp_post_config(port);
 		dp_state[port] = DP_ACTIVE;
 		CPRINTS("C%d: Entered DP mode", port);
 		break;
@@ -228,8 +260,7 @@ void dp_vdm_naked(int port, enum tcpci_msg_type type, uint8_t vdm_cmd)
 enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 					    uint32_t *vdm)
 {
-	const struct svdm_amode_data *modep =
-		pd_get_amode_data(port, TCPCI_MSG_SOP, USB_SID_DISPLAYPORT);
+	uint32_t mode_vdos[PDO_MODES];
 	int vdo_count_ret;
 
 	if (*vdo_count < VDO_MAX_SIZE)
@@ -239,10 +270,14 @@ enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 	case DP_START:
 	case DP_ENTER_RETRY:
 		/* Enter the first supported mode for DisplayPort. */
-		vdm[0] = pd_dfp_enter_mode(port, TCPCI_MSG_SOP,
-					   USB_SID_DISPLAYPORT, 0);
-		if (vdm[0] == 0)
+		if (pd_get_mode_vdo_for_svid(port, TCPCI_MSG_SOP,
+					     USB_SID_DISPLAYPORT,
+					     mode_vdos) == 0)
 			return MSG_SETUP_ERROR;
+		if (svdm_enter_dp_mode(port, mode_vdos[dp_opos - 1]) < 0)
+			return MSG_SETUP_ERROR;
+		vdm[0] = VDO(USB_SID_DISPLAYPORT, 1,
+			     CMD_ENTER_MODE | VDO_OPOS(dp_opos));
 		/* CMDT_INIT is 0, so this is a no-op */
 		vdm[0] |= VDO_CMDT(CMDT_INIT);
 		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPCI_MSG_SOP));
@@ -251,20 +286,14 @@ enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 			CPRINTS("C%d: Attempting to enter DP mode", port);
 		break;
 	case DP_ENTER_ACKED:
-		if (!(modep && modep->opos))
-			return MSG_SETUP_ERROR;
-
-		vdo_count_ret = modep->fx->status(port, vdm);
+		vdo_count_ret = svdm_dp_status(port, vdm);
 		if (vdo_count_ret == 0)
 			return MSG_SETUP_ERROR;
-		vdm[0] |= PD_VDO_OPOS(modep->opos);
+		vdm[0] |= PD_VDO_OPOS(dp_opos);
 		vdm[0] |= VDO_CMDT(CMDT_INIT);
 		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPCI_MSG_SOP));
 		break;
 	case DP_STATUS_ACKED:
-		if (!(modep && modep->opos))
-			return MSG_SETUP_ERROR;
-
 		if (!get_dp_pin_mode(port))
 			return MSG_SETUP_ERROR;
 
@@ -286,7 +315,7 @@ enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 		/* Fall through if no mux set is needed */
 		__fallthrough;
 	case DP_PREPARE_CONFIG:
-		vdo_count_ret = modep->fx->config(port, vdm);
+		vdo_count_ret = svdm_dp_config(port, vdm);
 		if (vdo_count_ret == 0)
 			return MSG_SETUP_ERROR;
 		vdm[0] |= VDO_CMDT(CMDT_INIT);
@@ -303,22 +332,16 @@ enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 		 * when an initial request to enter the mode is NAK'ed.
 		 * This can happen if the EC is restarted (e.g to go
 		 * into recovery mode) while DP alt mode is active.
-		 * It would be good to invoke modep->fx->exit but
-		 * this doesn't set up the VDM, it clears state.
-		 * TODO(b/159856063): Clean up the API to the fx functions.
 		 */
 		usb_mux_set_safe_mode_exit(port);
 		dp_state[port] = DP_PREPARE_EXIT;
 		return MSG_SETUP_MUX_WAIT;
 	case DP_PREPARE_EXIT:
-		if (!(modep && modep->opos))
-			return MSG_SETUP_ERROR;
-
 		/* DPM should call setup only after safe state is set */
 		vdm[0] = VDO(USB_SID_DISPLAYPORT, 1, /* structured */
 			     CMD_EXIT_MODE);
 
-		vdm[0] |= VDO_OPOS(modep->opos);
+		vdm[0] |= VDO_OPOS(dp_opos);
 		vdm[0] |= VDO_CMDT(CMDT_INIT);
 		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPCI_MSG_SOP));
 		vdo_count_ret = 1;
@@ -341,3 +364,355 @@ enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 
 	return MSG_SETUP_UNSUPPORTED;
 }
+
+int svdm_dp_status(int port, uint32_t *payload)
+{
+	payload[0] =
+		VDO(USB_SID_DISPLAYPORT, 1, CMD_DP_STATUS | VDO_OPOS(dp_opos));
+	payload[1] = VDO_DP_STATUS(0, /* HPD IRQ  ... not applicable */
+				   0, /* HPD level ... not applicable */
+				   0, /* exit DP? ... no */
+				   0, /* usb mode? ... no */
+				   0, /* multi-function ... no */
+				   (!!(dp_flags[port] & DP_FLAGS_DP_ON)),
+				   0, /* power low? ... no */
+				   (!!DP_FLAGS_DP_ON));
+	return 2;
+};
+
+/*
+ * This algorithm defaults to choosing higher pin config over lower ones in
+ * order to prefer multi-function if desired.
+ *
+ *  NAME | SIGNALING | OUTPUT TYPE | MULTI-FUNCTION | PIN CONFIG
+ * -------------------------------------------------------------
+ *  A    |  USB G2   |  ?          | no             | 00_0001
+ *  B    |  USB G2   |  ?          | yes            | 00_0010
+ *  C    |  DP       |  CONVERTED  | no             | 00_0100
+ *  D    |  PD       |  CONVERTED  | yes            | 00_1000
+ *  E    |  DP       |  DP         | no             | 01_0000
+ *  F    |  PD       |  DP         | yes            | 10_0000
+ *
+ * if UFP has NOT asserted multi-function preferred code masks away B/D/F
+ * leaving only A/C/E.  For single-output dongles that should leave only one
+ * possible pin config depending on whether its a converter DP->(VGA|HDMI) or DP
+ * output.  If UFP is a USB-C receptacle it may assert C/D/E/F.  The DFP USB-C
+ * receptacle must always choose C/D in those cases.
+ */
+int pd_dfp_dp_get_pin_mode(int port, uint32_t status)
+{
+	uint32_t mode_vdos[PDO_MODES];
+	uint32_t mode_caps;
+	uint32_t pin_caps;
+	int mf_pref;
+
+	/*
+	 * Default dp_port_mf_allow is true, we allow mf operation
+	 * if UFP_D supports it.
+	 */
+
+	if (IS_ENABLED(CONFIG_CMD_MFALLOW))
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]) &&
+			  dp_port_mf_allow[port];
+	else
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
+
+	if (pd_get_mode_vdo_for_svid(port, TCPCI_MSG_SOP, USB_SID_DISPLAYPORT,
+				     mode_vdos) == 0)
+		return 0;
+
+	mode_caps = mode_vdos[dp_opos - 1];
+
+	/* TODO(crosbug.com/p/39656) revisit with DFP that can be a sink */
+	pin_caps = PD_DP_PIN_CAPS(mode_caps);
+
+	/* if don't want multi-function then ignore those pin configs */
+	if (!mf_pref)
+		pin_caps &= ~MODE_DP_PIN_MF_MASK;
+
+	/* TODO(crosbug.com/p/39656) revisit if DFP drives USB Gen 2 signals */
+	pin_caps &= ~MODE_DP_PIN_BR2_MASK;
+
+	/* if C/D present they have precedence over E/F for USB-C->USB-C */
+	if (pin_caps & (MODE_DP_PIN_C | MODE_DP_PIN_D))
+		pin_caps &= ~(MODE_DP_PIN_E | MODE_DP_PIN_F);
+
+	/* get_next_bit returns undefined for zero */
+	if (!pin_caps)
+		return 0;
+
+	return 1 << get_next_bit(&pin_caps);
+}
+
+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 */
+	int mf_pref;
+
+	if (IS_ENABLED(CONFIG_CMD_MFALLOW))
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]) &&
+			  dp_port_mf_allow[port];
+	else
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
+
+	/*
+	 * Multi-function operation is only allowed if that pin config is
+	 * supported.
+	 */
+	if ((pin_mode & MODE_DP_PIN_MF_MASK) && mf_pref)
+		return USB_PD_MUX_DOCK;
+	else
+		return USB_PD_MUX_DP_ENABLED;
+}
+
+/*
+ * Note: the following DP-related overridables must be kept as-is since
+ * some boards are using them in their board-specific code.
+ * TODO(b/267545470): Fold board DP code into the DP module
+ */
+__overridable void svdm_safe_dp_mode(int port)
+{
+	/* make DP interface safe until configure */
+	dp_flags[port] = 0;
+	dp_status[port] = 0;
+
+	usb_mux_set_safe_mode(port);
+}
+
+__overridable int svdm_enter_dp_mode(int port, uint32_t mode_caps)
+{
+	/*
+	 * Don't enter the mode if the SoC is off.
+	 *
+	 * There's no need to enter the mode while the SoC is off; we'll
+	 * actually enter the mode on the chipset resume hook.  Entering DP Alt
+	 * Mode twice will confuse some monitors and require and unplug/replug
+	 * to get them to work again.  The DP Alt Mode on USB-C spec says that
+	 * if we don't need to maintain HPD connectivity info in a low power
+	 * mode, then we shall exit DP Alt Mode.  (This is why we don't enter
+	 * when the SoC is off as opposed to suspend where adding a display
+	 * could cause a wake up.)  When in S5->S3 transition state, we
+	 * should treat it as a SoC off state.
+	 */
+#ifdef CONFIG_AP_POWER_CONTROL
+	if (!chipset_in_state(CHIPSET_STATE_ANY_SUSPEND | CHIPSET_STATE_ON))
+		return -1;
+#endif
+
+		/*
+		 * TCPMv2: Enable logging of CCD line state CCD_MODE_ODL.
+		 * DisplayPort Alternate mode requires that the SBU lines are
+		 * used for AUX communication. However, in Chromebooks SBU
+		 * signals are repurposed as USB2 signals for CCD. This
+		 * functionality is accomplished by override fets whose state is
+		 * controlled by CCD_MODE_ODL.
+		 *
+		 * This condition helps in debugging unexpected AUX timeout
+		 * issues by indicating the state of the CCD override fets.
+		 */
+#ifdef GPIO_CCD_MODE_ODL
+	if (!gpio_get_level(GPIO_CCD_MODE_ODL))
+		CPRINTS("WARNING: Tried to EnterMode DP with [CCD on AUX/SBU]");
+#endif
+
+	/* Only enter mode if device is DFP_D capable */
+	if (mode_caps & MODE_DP_SNK) {
+		svdm_safe_dp_mode(port);
+
+		if (IS_ENABLED(CONFIG_MKBP_EVENT) &&
+		    chipset_in_state(CHIPSET_STATE_ANY_SUSPEND))
+			/*
+			 * Wake the system up since we're entering DP AltMode.
+			 */
+			pd_notify_dp_alt_mode_entry(port);
+
+		return 0;
+	}
+
+	return -1;
+}
+
+__overridable uint8_t get_dp_pin_mode(int port)
+{
+	return pd_dfp_dp_get_pin_mode(port, dp_status[port]);
+}
+
+/* Note: Assumes that pins have already been set in safe state if necessary */
+__overridable int svdm_dp_config(int port, uint32_t *payload)
+{
+	uint8_t pin_mode = get_dp_pin_mode(port);
+	mux_state_t mux_mode = svdm_dp_get_mux_mode(port);
+	/* Default dp_port_mf_allow is true */
+	int mf_pref;
+
+	if (IS_ENABLED(CONFIG_CMD_MFALLOW))
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]) &&
+			  dp_port_mf_allow[port];
+	else
+		mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
+
+	if (!pin_mode)
+		return 0;
+
+	CPRINTS("pin_mode: %x, mf: %d, mux: %d", pin_mode, mf_pref, mux_mode);
+
+	payload[0] =
+		VDO(USB_SID_DISPLAYPORT, 1, CMD_DP_CONFIG | VDO_OPOS(dp_opos));
+	payload[1] = VDO_DP_CFG(pin_mode, /* pin mode */
+				1, /* DPv1.3 signaling */
+				2); /* UFP connected */
+	return 2;
+};
+
+#if defined(CONFIG_USB_PD_DP_HPD_GPIO) && \
+	!defined(CONFIG_USB_PD_DP_HPD_GPIO_CUSTOM)
+void svdm_set_hpd_gpio(int port, int en)
+{
+	gpio_set_level(PORT_TO_HPD(port), en);
+}
+
+int svdm_get_hpd_gpio(int port)
+{
+	return gpio_get_level(PORT_TO_HPD(port));
+}
+#endif
+
+__overridable void svdm_dp_post_config(int port)
+{
+	mux_state_t mux_mode = svdm_dp_get_mux_mode(port);
+	/* Connect the SBU and USB lines to the connector. */
+	typec_set_sbu(port, true);
+
+	usb_mux_set(port, mux_mode, USB_SWITCH_CONNECT,
+		    polarity_rm_dts(pd_get_polarity(port)));
+
+	dp_flags[port] |= DP_FLAGS_DP_ON;
+	if (!(dp_flags[port] & DP_FLAGS_HPD_HI_PENDING))
+		return;
+
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	svdm_set_hpd_gpio(port, 1);
+
+	/* set the minimum time delay (2ms) for the next HPD IRQ */
+	svdm_hpd_deadline[port] = get_time().val + HPD_USTREAM_DEBOUNCE_LVL;
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+
+	usb_mux_hpd_update(port,
+			   USB_PD_MUX_HPD_LVL | USB_PD_MUX_HPD_IRQ_DEASSERTED);
+
+#ifdef USB_PD_PORT_TCPC_MST
+	if (port == USB_PD_PORT_TCPC_MST)
+		baseboard_mst_enable_control(port, 1);
+#endif
+}
+
+__overridable int svdm_dp_attention(int port, uint32_t *payload)
+{
+	int lvl = PD_VDO_DPSTS_HPD_LVL(payload[1]);
+	int irq = PD_VDO_DPSTS_HPD_IRQ(payload[1]);
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	int cur_lvl = svdm_get_hpd_gpio(port);
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+	mux_state_t mux_state;
+
+	dp_status[port] = payload[1];
+
+	if (chipset_in_state(CHIPSET_STATE_ANY_SUSPEND) && (irq || lvl))
+		/*
+		 * Wake up the AP.  IRQ or level high indicates a DP sink is now
+		 * present.
+		 */
+		if (IS_ENABLED(CONFIG_MKBP_EVENT))
+			pd_notify_dp_alt_mode_entry(port);
+
+	/* Its initial DP status message prior to config */
+	if (!(dp_flags[port] & DP_FLAGS_DP_ON)) {
+		if (lvl)
+			dp_flags[port] |= DP_FLAGS_HPD_HI_PENDING;
+		return 1;
+	}
+
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	if (irq && !lvl) {
+		/*
+		 * IRQ can only be generated when the level is high, because
+		 * the IRQ is signaled by a short low pulse from the high level.
+		 */
+		CPRINTF("ERR:HPD:IRQ&LOW\n");
+		return 0; /* nak */
+	}
+
+	if (irq && cur_lvl) {
+		uint64_t now = get_time().val;
+		/* wait for the minimum spacing between IRQ_HPD if needed */
+		if (now < svdm_hpd_deadline[port])
+			usleep(svdm_hpd_deadline[port] - now);
+
+		/* generate IRQ_HPD pulse */
+		svdm_set_hpd_gpio(port, 0);
+		usleep(HPD_DSTREAM_DEBOUNCE_IRQ);
+		svdm_set_hpd_gpio(port, 1);
+	} else {
+		svdm_set_hpd_gpio(port, lvl);
+	}
+
+	/* set the minimum time delay (2ms) for the next HPD IRQ */
+	svdm_hpd_deadline[port] = get_time().val + HPD_USTREAM_DEBOUNCE_LVL;
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+
+	mux_state = (lvl ? USB_PD_MUX_HPD_LVL : USB_PD_MUX_HPD_LVL_DEASSERTED) |
+		    (irq ? USB_PD_MUX_HPD_IRQ : USB_PD_MUX_HPD_IRQ_DEASSERTED);
+	usb_mux_hpd_update(port, mux_state);
+
+#ifdef USB_PD_PORT_TCPC_MST
+	if (port == USB_PD_PORT_TCPC_MST)
+		baseboard_mst_enable_control(port, lvl);
+#endif
+
+	/* ack */
+	return 1;
+}
+
+__overridable void svdm_exit_dp_mode(int port)
+{
+	dp_flags[port] = 0;
+	dp_status[port] = 0;
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	svdm_set_hpd_gpio(port, 0);
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+	usb_mux_hpd_update(port, USB_PD_MUX_HPD_LVL_DEASSERTED |
+					 USB_PD_MUX_HPD_IRQ_DEASSERTED);
+#ifdef USB_PD_PORT_TCPC_MST
+	if (port == USB_PD_PORT_TCPC_MST)
+		baseboard_mst_enable_control(port, 0);
+#endif
+}
+
+#ifdef CONFIG_CMD_MFALLOW
+static int command_mfallow(int argc, const char **argv)
+{
+	char *e;
+	int port;
+
+	if (argc < 3)
+		return EC_ERROR_PARAM_COUNT;
+
+	port = strtoi(argv[1], &e, 10);
+	if (*e || port >= board_get_usb_pd_port_count())
+		return EC_ERROR_PARAM2;
+
+	if (!strcasecmp(argv[2], "true"))
+		dp_port_mf_allow[port] = true;
+	else if (!strcasecmp(argv[2], "false"))
+		dp_port_mf_allow[port] = false;
+	else
+		return EC_ERROR_PARAM1;
+
+	ccprintf("Port: %d multi function allowed is %s ", port, argv[2]);
+	return EC_SUCCESS;
+}
+
+DECLARE_CONSOLE_COMMAND(mfallow, command_mfallow, "port [true | false]",
+			"Controls Multifunction choice during DP Altmode.");
+#endif
diff --git a/common/usbc/tbt_alt_mode.c b/common/usbc/tbt_alt_mode.c
index fec1ee9de3..6320af4600 100644
--- a/common/usbc/tbt_alt_mode.c
+++ b/common/usbc/tbt_alt_mode.c
@@ -82,6 +82,9 @@ static uint8_t tbt_flags[CONFIG_USB_PD_PORT_MAX_COUNT];
 #define TBT_CLR_FLAG(port, flag) (tbt_flags[port] &= (~flag))
 #define TBT_CHK_FLAG(port, flag) (tbt_flags[port] & (flag))
 
+/* Note: there is currently only one defined TBT mode */
+static const int tbt_opos = 1;
+
 static int tbt_prints(const char *string, int port)
 {
 	return CPRINTS("C%d: TBT %s", port, string);
@@ -287,7 +290,6 @@ void intel_vdm_acked(int port, enum tcpci_msg_type type, int vdo_count,
 		     uint32_t *vdm)
 {
 	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;
@@ -318,12 +320,7 @@ void intel_vdm_acked(int port, enum tcpci_msg_type type, int vdo_count,
 		break;
 	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 */
-		if (opos_sop > 0)
-			pd_dfp_exit_mode(port, TCPCI_MSG_SOP, USB_VID_INTEL,
-					 opos_sop);
+		pd_set_dfp_enter_mode_flag(port, false);
 
 		if (tbt_sop_prime_prime_needed(port)) {
 			tbt_state[port] = TBT_EXIT_SOP_PRIME_PRIME;
@@ -349,12 +346,7 @@ void intel_vdm_acked(int port, enum tcpci_msg_type type, int vdo_count,
 			 * Exit mode process is complete; go to inactive state.
 			 */
 			tbt_exit_done(port);
-			opos_sop_prime = pd_alt_mode(port, TCPCI_MSG_SOP_PRIME,
-						     USB_VID_INTEL);
-
 			/* Clear Thunderbolt related signals */
-			pd_dfp_exit_mode(port, TCPCI_MSG_SOP_PRIME,
-					 USB_VID_INTEL, opos_sop_prime);
 			set_usb_mux_with_current_data_role(port);
 		} else {
 			tbt_retry_enter_mode(port);
@@ -462,7 +454,6 @@ enum dpm_msg_setup_status tbt_setup_next_vdm(int port, int *vdo_count,
 					     uint32_t *vdm,
 					     enum tcpci_msg_type *tx_type)
 {
-	struct svdm_amode_data *modep;
 	int vdo_count_ret = 0;
 
 	*tx_type = TCPCI_MSG_SOP;
@@ -529,36 +520,22 @@ enum dpm_msg_setup_status tbt_setup_next_vdm(int port, int *vdo_count,
 		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;
-
 		vdm[0] = VDO(USB_VID_INTEL, 1, CMD_EXIT_MODE) |
-			 VDO_OPOS(modep->opos) | VDO_CMDT(CMDT_INIT) |
+			 VDO_OPOS(tbt_opos) | VDO_CMDT(CMDT_INIT) |
 			 VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPCI_MSG_SOP));
 		vdo_count_ret = 1;
 		break;
 	case TBT_EXIT_SOP_PRIME_PRIME:
-		modep = pd_get_amode_data(port, TCPCI_MSG_SOP_PRIME,
-					  USB_VID_INTEL);
-		if (!(modep && modep->opos))
-			return MSG_SETUP_ERROR;
-
 		vdm[0] = VDO(USB_VID_INTEL, 1, CMD_EXIT_MODE) |
-			 VDO_OPOS(modep->opos) | VDO_CMDT(CMDT_INIT) |
+			 VDO_OPOS(tbt_opos) | VDO_CMDT(CMDT_INIT) |
 			 VDO_SVDM_VERS(pd_get_vdo_ver(
 				 port, TCPCI_MSG_SOP_PRIME_PRIME));
 		vdo_count_ret = 1;
 		*tx_type = TCPCI_MSG_SOP_PRIME_PRIME;
 		break;
 	case TBT_EXIT_SOP_PRIME:
-		modep = pd_get_amode_data(port, TCPCI_MSG_SOP_PRIME,
-					  USB_VID_INTEL);
-		if (!(modep && modep->opos))
-			return MSG_SETUP_ERROR;
-
 		vdm[0] = VDO(USB_VID_INTEL, 1, CMD_EXIT_MODE) |
-			 VDO_OPOS(modep->opos) | VDO_CMDT(CMDT_INIT) |
+			 VDO_OPOS(tbt_opos) | VDO_CMDT(CMDT_INIT) |
 			 VDO_SVDM_VERS(
 				 pd_get_vdo_ver(port, TCPCI_MSG_SOP_PRIME));
 		vdo_count_ret = 1;
@@ -706,9 +683,10 @@ int enter_tbt_compat_mode(int port, enum tcpci_msg_type sop, uint32_t *payload)
 	 * doesn't have opos for SOP''. Hence, send Enter Mode SOP'' with same
 	 * opos and revision as SOP'.
 	 */
-	payload[0] = pd_dfp_enter_mode(port, enter_mode_sop, USB_VID_INTEL, 0) |
-		     VDO_CMDT(CMDT_INIT) |
-		     VDO_SVDM_VERS(pd_get_vdo_ver(port, enter_mode_sop));
+	payload[0] =
+		VDO(USB_VID_INTEL, 1, CMD_ENTER_MODE | VDO_OPOS(tbt_opos)) |
+		VDO_CMDT(CMDT_INIT) |
+		VDO_SVDM_VERS(pd_get_vdo_ver(port, enter_mode_sop));
 
 	/* For TBT3 Cable Enter Mode Command, number of Objects is 1 */
 	if ((sop == TCPCI_MSG_SOP_PRIME) || (sop == TCPCI_MSG_SOP_PRIME_PRIME))
diff --git a/common/usbc/usb_pd_dpm.c b/common/usbc/usb_pd_dpm.c
index 397e5071e5..5146951cad 100644
--- a/common/usbc/usb_pd_dpm.c
+++ b/common/usbc/usb_pd_dpm.c
@@ -83,6 +83,15 @@ static struct {
 #endif
 } dpm[CONFIG_USB_PD_PORT_MAX_COUNT];
 
+__overridable const struct svdm_response svdm_rsp = {
+	.identity = NULL,
+	.svids = NULL,
+	.modes = NULL,
+};
+
+/* Tracker for which task is waiting on sysjump prep to finish */
+static volatile task_id_t sysjump_task_waiting = TASK_ID_INVALID;
+
 #define DPM_SET_FLAG(port, flag) atomic_or(&dpm[(port)].flags, (flag))
 #define DPM_CLR_FLAG(port, flag) atomic_clear_bits(&dpm[(port)].flags, (flag))
 #define DPM_CHK_FLAG(port, flag) (dpm[(port)].flags & (flag))
@@ -182,6 +191,91 @@ static void init_attention_queue_structs(void)
 DECLARE_HOOK(HOOK_INIT, init_attention_queue_structs, HOOK_PRIO_FIRST);
 #endif
 
+void pd_prepare_sysjump(void)
+{
+#ifndef CONFIG_ZEPHYR
+	int i;
+
+	/* Exit modes before sysjump so we can cleanly enter again later */
+	for (i = 0; i < board_get_usb_pd_port_count(); i++) {
+		/*
+		 * If the port is not capable of alternate mode, then there's no
+		 * need to send the event.
+		 */
+		if (!pd_alt_mode_capable(i))
+			continue;
+
+		sysjump_task_waiting = task_get_current();
+		task_set_event(PD_PORT_TO_TASK_ID(i), PD_EVENT_SYSJUMP);
+		task_wait_event_mask(TASK_EVENT_SYSJUMP_READY, -1);
+		sysjump_task_waiting = TASK_ID_INVALID;
+	}
+#endif /* CONFIG_ZEPHYR */
+}
+
+void notify_sysjump_ready(void)
+{
+	/*
+	 * If event was set from pd_prepare_sysjump, wake the
+	 * task waiting on us to complete.
+	 */
+	if (sysjump_task_waiting != TASK_ID_INVALID)
+		task_set_event(sysjump_task_waiting, TASK_EVENT_SYSJUMP_READY);
+}
+
+/*
+ * TODO(b/270409939): Refactor this function
+ */
+int pd_dfp_exit_mode(int port, enum tcpci_msg_type type, uint16_t svid,
+		     int opos)
+{
+	/*
+	 * Empty svid signals we should reset DFP VDM state by exiting all
+	 * entered modes then clearing state.  This occurs when we've
+	 * disconnected or for hard reset.
+	 */
+	if (!svid) {
+		if (IS_ENABLED(CONFIG_USB_PD_DP_MODE) && dp_is_active(port))
+			svdm_exit_dp_mode(port);
+		pd_dfp_mode_init(port);
+	}
+
+	/*
+	 * Return 0 indicating no message is needed.  All modules should be
+	 * handling their SVID-specific cases themselves.
+	 */
+	return 0;
+}
+
+/*
+ * Note: this interface is used in board code, but should be obsoleted
+ * TODO(b/267545470): Fold board DP code into the DP module
+ */
+int pd_alt_mode(int port, enum tcpci_msg_type type, uint16_t svid)
+{
+	if (svid == USB_SID_DISPLAYPORT && !dp_is_idle(port))
+		return 1;
+	else if (IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE) &&
+		 svid == USB_VID_INTEL && tbt_is_active(port))
+		return 1;
+
+	return -1;
+}
+
+void dfp_consume_attention(int port, uint32_t *payload)
+{
+	uint16_t svid = PD_VDO_VID(payload[0]);
+
+	if (IS_ENABLED(CONFIG_USB_PD_DP_MODE) && svid == USB_SID_DISPLAYPORT) {
+		/*
+		 * Attention is only valid after EnterMode, so drop if this is
+		 * out of sequence
+		 */
+		if (!dp_is_idle(port))
+			svdm_dp_attention(port, payload);
+	}
+}
+
 static void vdm_attention_enqueue(int port, int length, uint32_t *buf)
 {
 #ifdef CONFIG_USB_PD_VDM_AP_CONTROL
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index 7824a285d5..749407b773 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -591,8 +591,6 @@ static struct policy_engine {
 	int32_t vpd_vdo;
 	/* Alternate mode discovery results */
 	struct pd_discovery discovery[DISCOVERY_TYPE_COUNT];
-	/* Active alternate modes */
-	struct partner_active_modes partner_amodes[AMODE_TYPE_COUNT];
 
 	/* Partner type to send */
 	enum tcpci_msg_type tx_type;
@@ -7786,8 +7784,6 @@ void pd_dfp_mode_init(int port)
 {
 	PE_CLR_FLAG(port, PE_FLAGS_MODAL_OPERATION);
 
-	memset(pe[port].partner_amodes, 0, sizeof(pe[port].partner_amodes));
-
 	/* Reset the DPM and DP modules to enable alternate mode entry. */
 	dpm_mode_exit_complete(port);
 	dp_init(port);
@@ -7837,15 +7833,6 @@ pd_get_am_discovery(int port, enum tcpci_msg_type type)
 	return &pe[port].discovery[type];
 }
 
-__maybe_unused struct partner_active_modes *
-pd_get_partner_active_modes(int port, enum tcpci_msg_type type)
-{
-	if (!IS_ENABLED(CONFIG_USB_PD_ALT_MODE_DFP))
-		assert(0);
-	ASSERT(type < AMODE_TYPE_COUNT);
-	return &pe[port].partner_amodes[type];
-}
-
 __maybe_unused void pd_set_dfp_enter_mode_flag(int port, bool set)
 {
 	if (!IS_ENABLED(CONFIG_USB_PD_ALT_MODE_DFP))
diff --git a/test/fake_usbc.c b/test/fake_usbc.c
index 798cd1fc57..faaed19503 100644
--- a/test/fake_usbc.c
+++ b/test/fake_usbc.c
@@ -270,6 +270,14 @@ const char *pd_get_task_state_name(int port)
 }
 #endif /* CONFIG_USB_DRP_ACC_TRYSRC */
 
+void dfp_consume_attention(int port, uint32_t *payload)
+{
+}
+
+void pd_prepare_sysjump(void)
+{
+}
+
 void dpm_init(int port)
 {
 }
diff --git a/zephyr/CMakeLists.txt b/zephyr/CMakeLists.txt
index fb5d996756..85eeef852a 100644
--- a/zephyr/CMakeLists.txt
+++ b/zephyr/CMakeLists.txt
@@ -473,8 +473,6 @@ zephyr_library_sources_ifdef(CONFIG_SVDM_RSP_DFP_ONLY
 zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_USBC_OCP
                                                 "${PLATFORM_EC}/common/usbc_ocp.c")
 
-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