servo_v4: add per port dualrole setting

This adds support to configure dualrole setting
per port, so that servo v4 can adjust charge and
dut port separately.

servo will detect charge capability on CHG port
and choose source or sink as appropriate.

Fix null dereference bug in genvif duel to dynamic src_pdo.

"cc" command allows src, snk, srcdts, snkdts configurations.

BRANCH=None
BUG=b:72557427,b:143572158
TEST=charge through and also passive hub. Note Dru doesn't accept DTS hub.
TEST=make buildall -j

Change-Id: I650fac21275e79b9268046e9556a6b53070c4fbf
Signed-off-by: Nick Sanders <nsanders@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1096654
Reviewed-by: Aseda Aboagye <aaboagye@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1893978
Commit-Queue: Daisuke Nojiri <dnojiri@chromium.org>
Tested-by: Daisuke Nojiri <dnojiri@chromium.org>
Auto-Submit: Daisuke Nojiri <dnojiri@chromium.org>

40 files changed, 1136 insertions, 201 deletions

diff --git a/baseboard/dragonegg/usb_pd_policy.c b/baseboard/dragonegg/usb_pd_policy.c
new file mode 100644
index 0000000000..e7def7fe71
--- /dev/null
+++ b/baseboard/dragonegg/usb_pd_policy.c
@@ -0,0 +1,409 @@
+/* Copyright 2018 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Shared USB-C policy for DragonEgg boards */
+
+#include "charge_manager.h"
+#include "common.h"
+#include "compile_time_macros.h"
+#include "console.h"
+#include "ec_commands.h"
+#include "gpio.h"
+#include "system.h"
+#include "usb_mux.h"
+#include "usb_pd.h"
+#include "usbc_ppc.h"
+#include "util.h"
+
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
+
+#define PDO_FIXED_FLAGS (PDO_FIXED_DUAL_ROLE | PDO_FIXED_DATA_SWAP |\
+			 PDO_FIXED_COMM_CAP)
+
+const uint32_t pd_src_pdo[] = {
+	PDO_FIXED(5000, 1500, PDO_FIXED_FLAGS),
+};
+const int pd_src_pdo_cnt = ARRAY_SIZE(pd_src_pdo);
+const uint32_t pd_src_pdo_max[] = {
+	PDO_FIXED(5000, 3000, PDO_FIXED_FLAGS),
+};
+const int pd_src_pdo_max_cnt = ARRAY_SIZE(pd_src_pdo_max);
+
+const uint32_t pd_snk_pdo[] = {
+	PDO_FIXED(5000, 500, PDO_FIXED_FLAGS),
+	PDO_BATT(4750, 21000, 15000),
+	PDO_VAR(4750, 21000, 3000),
+};
+const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
+
+int pd_board_checks(void)
+{
+	return EC_SUCCESS;
+}
+
+int pd_check_data_swap(int port, int data_role)
+{
+	/*
+	 * Allow data swap if we are a UFP, otherwise don't allow.
+	 *
+	 * When we are still in the Read-Only firmware, avoid swapping roles
+	 * so we don't jump in RW as a SNK/DFP and potentially confuse the
+	 * power supply by sending a soft-reset with wrong data role.
+	 */
+	return (data_role == PD_ROLE_UFP) &&
+	       (system_get_image_copy() != SYSTEM_IMAGE_RO) ? 1 : 0;
+}
+
+void pd_check_dr_role(int port, int dr_role, int flags)
+{
+	/* If UFP, try to switch to DFP */
+	if ((flags & PD_FLAGS_PARTNER_DR_DATA) &&
+			dr_role == PD_ROLE_UFP &&
+			system_get_image_copy() != SYSTEM_IMAGE_RO)
+		pd_request_data_swap(port);
+}
+
+int pd_check_power_swap(int port)
+{
+	/*
+	 * Allow power swap as long as we are acting as a dual role device,
+	 * otherwise assume our role is fixed (not in S0 or console command
+	 * to fix our role).
+	 */
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
+}
+
+void pd_check_pr_role(int port, int pr_role, int flags)
+{
+	/*
+	 * If partner is dual-role power and dualrole toggling is on, consider
+	 * if a power swap is necessary.
+	 */
+	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
+		/*
+		 * If we are a sink and partner is not externally powered, then
+		 * swap to become a source. If we are source and partner is
+		 * externally powered, swap to become a sink.
+		 */
+		int partner_extpower = flags & PD_FLAGS_PARTNER_EXTPOWER;
+
+		if ((!partner_extpower && pr_role == PD_ROLE_SINK) ||
+		     (partner_extpower && pr_role == PD_ROLE_SOURCE))
+			pd_request_power_swap(port);
+	}
+}
+
+int pd_check_vconn_swap(int port)
+{
+	/* Only allow vconn swap if pp5000_A rail is enabled */
+	return gpio_get_level(GPIO_EN_PP5000);
+}
+
+void pd_execute_data_swap(int port, int data_role)
+{
+	/* On DragonEgg, only the first port can act as OTG */
+	if (port == 0)
+		gpio_set_level(GPIO_CHG_VAP_OTG_EN, (data_role == PD_ROLE_UFP));
+}
+
+int pd_is_valid_input_voltage(int mv)
+{
+	return 1;
+}
+
+void pd_power_supply_reset(int port)
+{
+	int prev_en;
+
+	prev_en = ppc_is_sourcing_vbus(port);
+
+	/* Disable VBUS. */
+	ppc_vbus_source_enable(port, 0);
+
+	/* Enable discharge if we were previously sourcing 5V */
+	if (prev_en)
+		pd_set_vbus_discharge(port, 1);
+
+#ifdef CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT
+	/* Give back the current quota we are no longer using */
+	charge_manager_source_port(port, 0);
+#endif /* defined(CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT) */
+
+	/* Notify host of power info change. */
+	pd_send_host_event(PD_EVENT_POWER_CHANGE);
+}
+
+int pd_set_power_supply_ready(int port)
+{
+	int rv;
+
+	/* Disable charging. */
+	rv = ppc_vbus_sink_enable(port, 0);
+	if (rv)
+		return rv;
+
+	pd_set_vbus_discharge(port, 0);
+
+	/* Provide Vbus. */
+	rv = ppc_vbus_source_enable(port, 1);
+	if (rv)
+		return rv;
+
+#ifdef CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT
+	/* Ensure we advertise the proper available current quota */
+	charge_manager_source_port(port, 1);
+#endif /* defined(CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT) */
+
+	/* Notify host of power info change. */
+	pd_send_host_event(PD_EVENT_POWER_CHANGE);
+
+	return EC_SUCCESS;
+}
+
+void pd_transition_voltage(int idx)
+{
+	/* No-operation: we are always 5V */
+}
+
+#ifdef CONFIG_USB_PD_VBUS_DETECT_PPC
+int pd_snk_is_vbus_provided(int port)
+{
+	return ppc_is_vbus_present(port);
+}
+#endif
+
+void typec_set_source_current_limit(int port, int rp)
+{
+	ppc_set_vbus_source_current_limit(port, rp);
+}
+
+int board_vbus_source_enabled(int port)
+{
+	return ppc_is_sourcing_vbus(port);
+}
+
+
+/* ----------------- Vendor Defined Messages ------------------ */
+const struct svdm_response svdm_rsp = {
+	.identity = NULL,
+	.svids = NULL,
+	.modes = NULL,
+};
+
+int pd_custom_vdm(int port, int cnt, uint32_t *payload,
+		  uint32_t **rpayload)
+{
+	int cmd = PD_VDO_CMD(payload[0]);
+	uint16_t dev_id = 0;
+	int is_rw, is_latest;
+
+	/* make sure we have some payload */
+	if (cnt == 0)
+		return 0;
+
+	switch (cmd) {
+	case VDO_CMD_VERSION:
+		/* guarantee last byte of payload is null character */
+		*(payload + cnt - 1) = 0;
+		CPRINTF("version: %s\n", (char *)(payload+1));
+		break;
+	case VDO_CMD_READ_INFO:
+	case VDO_CMD_SEND_INFO:
+		/* copy hash */
+		if (cnt == 7) {
+			dev_id = VDO_INFO_HW_DEV_ID(payload[6]);
+			is_rw = VDO_INFO_IS_RW(payload[6]);
+
+			is_latest = pd_dev_store_rw_hash(port,
+							 dev_id,
+							 payload + 1,
+							 is_rw ?
+							 SYSTEM_IMAGE_RW :
+							 SYSTEM_IMAGE_RO);
+			/*
+			 * Send update host event unless our RW hash is
+			 * already known to be the latest update RW.
+			 */
+			if (!is_rw || !is_latest)
+				pd_send_host_event(PD_EVENT_UPDATE_DEVICE);
+
+			CPRINTF("DevId:%d.%d SW:%d RW:%d\n",
+				HW_DEV_ID_MAJ(dev_id),
+				HW_DEV_ID_MIN(dev_id),
+				VDO_INFO_SW_DBG_VER(payload[6]),
+				is_rw);
+		} else if (cnt == 6) {
+			/* really old devices don't have last byte */
+			pd_dev_store_rw_hash(port, dev_id, payload + 1,
+					     SYSTEM_IMAGE_UNKNOWN);
+		}
+		break;
+	case VDO_CMD_CURRENT:
+		CPRINTF("Current: %dmA\n", payload[1]);
+		break;
+	case VDO_CMD_FLIP:
+		usb_mux_flip(port);
+		break;
+#ifdef CONFIG_USB_PD_LOGGING
+	case VDO_CMD_GET_LOG:
+		pd_log_recv_vdm(port, cnt, payload);
+		break;
+#endif /* CONFIG_USB_PD_LOGGING */
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_USB_PD_ALT_MODE_DFP
+static int dp_flags[CONFIG_USB_PD_PORT_COUNT];
+static uint32_t dp_status[CONFIG_USB_PD_PORT_COUNT];
+
+static void svdm_safe_dp_mode(int port)
+{
+	/* make DP interface safe until configure */
+	dp_flags[port] = 0;
+	dp_status[port] = 0;
+	usb_mux_set(port, TYPEC_MUX_NONE,
+		USB_SWITCH_CONNECT, pd_get_polarity(port));
+}
+
+static int svdm_enter_dp_mode(int port, uint32_t mode_caps)
+{
+	/* Only enter mode if device is DFP_D capable */
+	if (mode_caps & MODE_DP_SNK) {
+		svdm_safe_dp_mode(port);
+		return 0;
+	}
+
+	return -1;
+}
+
+static int svdm_dp_status(int port, uint32_t *payload)
+{
+	int opos = pd_alt_mode(port, 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[port] & DP_FLAGS_DP_ON)));
+	return 2;
+};
+
+static int svdm_dp_config(int port, uint32_t *payload)
+{
+	int opos = pd_alt_mode(port, USB_SID_DISPLAYPORT);
+	int mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
+	int pin_mode = pd_dfp_dp_get_pin_mode(port, dp_status[port]);
+
+	if (!pin_mode)
+		return 0;
+
+	usb_mux_set(port, mf_pref ? TYPEC_MUX_DOCK : TYPEC_MUX_DP,
+		    USB_SWITCH_CONNECT, pd_get_polarity(port));
+
+	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;
+};
+
+
+static void svdm_dp_post_config(int port)
+{
+	const struct usb_mux *mux = &usb_muxes[port];
+
+	dp_flags[port] |= DP_FLAGS_DP_ON;
+	if (!(dp_flags[port] & DP_FLAGS_HPD_HI_PENDING))
+		return;
+	mux->hpd_update(port, 1, 0);
+}
+
+static 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]);
+	const struct usb_mux *mux = &usb_muxes[port];
+
+	dp_status[port] = payload[1];
+	if (!(dp_flags[port] & DP_FLAGS_DP_ON)) {
+		if (lvl)
+			dp_flags[port] |= DP_FLAGS_HPD_HI_PENDING;
+		return 1;
+	}
+	mux->hpd_update(port, lvl, irq);
+
+	/* ack */
+	return 1;
+}
+
+static void svdm_exit_dp_mode(int port)
+{
+	const struct usb_mux *mux = &usb_muxes[port];
+
+	svdm_safe_dp_mode(port);
+	mux->hpd_update(port, 0, 0);
+}
+
+static int svdm_enter_gfu_mode(int port, uint32_t mode_caps)
+{
+	/* Always enter GFU mode */
+	return 0;
+}
+
+static void svdm_exit_gfu_mode(int port)
+{
+}
+
+static 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;
+}
+
+static int svdm_gfu_config(int port, uint32_t *payload)
+{
+	return 0;
+}
+
+static int svdm_gfu_attention(int port, uint32_t *payload)
+{
+	return 0;
+}
+
+const struct svdm_amode_fx supported_modes[] = {
+	{
+		.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,
+	},
+	{
+		.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);
+#endif /* CONFIG_USB_PD_ALT_MODE_DFP */
diff --git a/baseboard/grunt/usb_pd_policy.c b/baseboard/grunt/usb_pd_policy.c
index 4f5a5b5c51..5c8b7e2359 100644
--- a/baseboard/grunt/usb_pd_policy.c
+++ b/baseboard/grunt/usb_pd_policy.c
@@ -69,7 +69,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 void pd_check_pr_role(int port, int pr_role, int flags)
@@ -79,7 +79,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/baseboard/octopus/usb_pd_policy.c b/baseboard/octopus/usb_pd_policy.c
index 1daf344328..0078a027cd 100644
--- a/baseboard/octopus/usb_pd_policy.c
+++ b/baseboard/octopus/usb_pd_policy.c
@@ -73,7 +73,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 void pd_check_pr_role(int port, int pr_role, int flags)
@@ -83,7 +83,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/atlas/usb_pd_policy.c b/board/atlas/usb_pd_policy.c
index a8e98ee5e1..fbf1f4d299 100644
--- a/board/atlas/usb_pd_policy.c
+++ b/board/atlas/usb_pd_policy.c
@@ -144,7 +144,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -185,7 +185,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/chell/usb_pd_policy.c b/board/chell/usb_pd_policy.c
index 276f97f800..7031a39bcc 100644
--- a/board/chell/usb_pd_policy.c
+++ b/board/chell/usb_pd_policy.c
@@ -90,7 +90,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -117,7 +117,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/cheza/usb_pd_policy.c b/board/cheza/usb_pd_policy.c
index 46d746ffbb..5d35a41968 100644
--- a/board/cheza/usb_pd_policy.c
+++ b/board/cheza/usb_pd_policy.c
@@ -62,7 +62,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 void pd_check_pr_role(int port, int pr_role, int flags)
@@ -72,7 +72,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/coffeecake/board.c b/board/coffeecake/board.c
index 11b4180c0e..bddfb0d159 100644
--- a/board/coffeecake/board.c
+++ b/board/coffeecake/board.c
@@ -209,7 +209,7 @@ static void board_post_init(void)
 	 * AC powered  - DRP SOURCE
 	 * DUT powered - DRP SINK
 	 */
-	pd_set_dual_role(gpio_get_level(GPIO_AC_PRESENT_L) ?
+	pd_set_dual_role(0, gpio_get_level(GPIO_AC_PRESENT_L) ?
 			 PD_DRP_FORCE_SINK : PD_DRP_FORCE_SOURCE);
 }
 DECLARE_DEFERRED(board_post_init);
diff --git a/board/coral/usb_pd_policy.c b/board/coral/usb_pd_policy.c
index 20286af0a0..60cbdd3276 100644
--- a/board/coral/usb_pd_policy.c
+++ b/board/coral/usb_pd_policy.c
@@ -141,7 +141,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -175,7 +175,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/elm/usb_pd_policy.c b/board/elm/usb_pd_policy.c
index 2da68d7484..766d38cc5c 100644
--- a/board/elm/usb_pd_policy.c
+++ b/board/elm/usb_pd_policy.c
@@ -88,7 +88,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -115,7 +115,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/eve/usb_pd_policy.c b/board/eve/usb_pd_policy.c
index 8c2226b444..c27183510b 100644
--- a/board/eve/usb_pd_policy.c
+++ b/board/eve/usb_pd_policy.c
@@ -167,7 +167,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -208,7 +208,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/fizz/usb_pd_policy.c b/board/fizz/usb_pd_policy.c
index 711582a702..bac29cfa52 100644
--- a/board/fizz/usb_pd_policy.c
+++ b/board/fizz/usb_pd_policy.c
@@ -100,7 +100,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -127,7 +127,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/glados/usb_pd_policy.c b/board/glados/usb_pd_policy.c
index d7b607f1de..10fefd8c11 100644
--- a/board/glados/usb_pd_policy.c
+++ b/board/glados/usb_pd_policy.c
@@ -90,7 +90,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -117,7 +117,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/glkrvp/usb_pd_policy.c b/board/glkrvp/usb_pd_policy.c
index a20171a455..57b114fe5d 100644
--- a/board/glkrvp/usb_pd_policy.c
+++ b/board/glkrvp/usb_pd_policy.c
@@ -80,7 +80,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -108,7 +108,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-		pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+		pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/glkrvp_ite/usb_pd_policy.c b/board/glkrvp_ite/usb_pd_policy.c
index 5d5eb05283..bd11eb4d8f 100644
--- a/board/glkrvp_ite/usb_pd_policy.c
+++ b/board/glkrvp_ite/usb_pd_policy.c
@@ -80,7 +80,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -108,7 +108,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-		pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+		pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/it83xx_evb/usb_pd_policy.c b/board/it83xx_evb/usb_pd_policy.c
index 32060a587c..81e243dc61 100644
--- a/board/it83xx_evb/usb_pd_policy.c
+++ b/board/it83xx_evb/usb_pd_policy.c
@@ -89,7 +89,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -104,7 +104,7 @@ int pd_check_vconn_swap(int port)
 	 * VCONN is provided directly by the battery(PPVAR_SYS)
 	 * but use the same rules as power swap
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 void pd_execute_data_swap(int port, int data_role)
@@ -118,7 +118,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are source and partner is externally powered,
 		 * swap to become a sink.
diff --git a/board/kukui/usb_pd_policy.c b/board/kukui/usb_pd_policy.c
index 67f74c6715..c3bfa25760 100644
--- a/board/kukui/usb_pd_policy.c
+++ b/board/kukui/usb_pd_policy.c
@@ -107,7 +107,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -122,7 +122,7 @@ int pd_check_vconn_swap(int port)
 	 * VCONN is provided directly by the battery (PPVAR_SYS)
 	 * but use the same rules as power swap.
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 void pd_execute_data_swap(int port, int data_role)
@@ -137,7 +137,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/mchpevb1/usb_pd_policy.c b/board/mchpevb1/usb_pd_policy.c
index 933cd54cba..d5eff63316 100644
--- a/board/mchpevb1/usb_pd_policy.c
+++ b/board/mchpevb1/usb_pd_policy.c
@@ -92,7 +92,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -119,7 +119,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/nami/usb_pd_policy.c b/board/nami/usb_pd_policy.c
index 74c303342e..65baa5b6cf 100644
--- a/board/nami/usb_pd_policy.c
+++ b/board/nami/usb_pd_policy.c
@@ -139,7 +139,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -177,7 +177,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/nautilus/usb_pd_policy.c b/board/nautilus/usb_pd_policy.c
index f30882f475..aa1c185345 100644
--- a/board/nautilus/usb_pd_policy.c
+++ b/board/nautilus/usb_pd_policy.c
@@ -165,7 +165,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -206,7 +206,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/nocturne/usb_pd_policy.c b/board/nocturne/usb_pd_policy.c
index 684905fc83..92b2ed66cc 100644
--- a/board/nocturne/usb_pd_policy.c
+++ b/board/nocturne/usb_pd_policy.c
@@ -67,7 +67,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 void pd_check_pr_role(int port, int pr_role, int flags)
@@ -77,7 +77,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/oak/usb_pd_policy.c b/board/oak/usb_pd_policy.c
index d1932e9493..2268945144 100644
--- a/board/oak/usb_pd_policy.c
+++ b/board/oak/usb_pd_policy.c
@@ -88,7 +88,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -115,7 +115,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/pdeval-stm32f072/usb_pd_policy.c b/board/pdeval-stm32f072/usb_pd_policy.c
index 9b81222d58..0b858e58e3 100644
--- a/board/pdeval-stm32f072/usb_pd_policy.c
+++ b/board/pdeval-stm32f072/usb_pd_policy.c
@@ -158,7 +158,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -175,7 +175,7 @@ int pd_check_vconn_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON;
 }
 #endif
 
diff --git a/board/plankton/board.c b/board/plankton/board.c
index b725989047..5cd9e54486 100644
--- a/board/plankton/board.c
+++ b/board/plankton/board.c
@@ -279,7 +279,7 @@ static void update_usbc_dual_role(int dual_role)
 		hook_call_deferred(&detect_cc_cable_data, 0);
 	}
 	/* Update dual role setting used in USB PD protocol state machine */
-	pd_set_dual_role(dual_role);
+	pd_set_dual_role(0, dual_role);
 	cprintf(CC_USBPD, "DRP = %d, host_mode = %d\n", drp_enable, host_mode);
 }
 
diff --git a/board/poppy/usb_pd_policy.c b/board/poppy/usb_pd_policy.c
index c351febb85..19407900fb 100644
--- a/board/poppy/usb_pd_policy.c
+++ b/board/poppy/usb_pd_policy.c
@@ -166,7 +166,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -207,7 +207,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/rainier/usb_pd_policy.c b/board/rainier/usb_pd_policy.c
index b74a0503ba..f5e2e267f2 100644
--- a/board/rainier/usb_pd_policy.c
+++ b/board/rainier/usb_pd_policy.c
@@ -106,7 +106,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -121,7 +121,7 @@ int pd_check_vconn_swap(int port)
 	 * VCONN is provided directly by the battery (PPVAR_SYS)
 	 * but use the same rules as power swap.
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 void pd_execute_data_swap(int port, int data_role)
@@ -136,7 +136,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/rammus/usb_pd_policy.c b/board/rammus/usb_pd_policy.c
new file mode 100644
index 0000000000..5d6190f915
--- /dev/null
+++ b/board/rammus/usb_pd_policy.c
@@ -0,0 +1,420 @@
+/* Copyright 2018 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "atomic.h"
+#include "extpower.h"
+#include "charge_manager.h"
+#include "common.h"
+#include "console.h"
+#include "driver/tcpm/ps8xxx.h"
+#include "gpio.h"
+#include "hooks.h"
+#include "host_command.h"
+#include "registers.h"
+#include "system.h"
+#include "task.h"
+#include "timer.h"
+#include "util.h"
+#include "usb_mux.h"
+#include "usb_pd.h"
+#include "usb_pd_tcpm.h"
+
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
+
+#define PDO_FIXED_FLAGS (PDO_FIXED_DUAL_ROLE | PDO_FIXED_DATA_SWAP |\
+			 PDO_FIXED_COMM_CAP)
+
+/* TODO(crosbug.com/p/61098): fill in correct source and sink capabilities */
+const uint32_t pd_src_pdo[] = {
+	PDO_FIXED(5000, 1500, PDO_FIXED_FLAGS),
+};
+const int pd_src_pdo_cnt = ARRAY_SIZE(pd_src_pdo);
+const uint32_t pd_src_pdo_max[] = {
+		PDO_FIXED(5000, 3000, PDO_FIXED_FLAGS),
+};
+const int pd_src_pdo_max_cnt = ARRAY_SIZE(pd_src_pdo_max);
+
+const uint32_t pd_snk_pdo[] = {
+	PDO_FIXED(5000, 500, PDO_FIXED_FLAGS),
+	PDO_BATT(4750, 21000, 15000),
+	PDO_VAR(4750, 21000, 3000),
+};
+const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
+
+int pd_is_valid_input_voltage(int mv)
+{
+	return 1;
+}
+
+void pd_transition_voltage(int idx)
+{
+	/* No-operation: we are always 5V */
+}
+
+static uint8_t vbus_en[CONFIG_USB_PD_PORT_COUNT];
+static uint8_t vbus_rp[CONFIG_USB_PD_PORT_COUNT] = {TYPEC_RP_1A5, TYPEC_RP_1A5};
+
+int board_vbus_source_enabled(int port)
+{
+	return vbus_en[port];
+}
+
+static void board_vbus_update_source_current(int port)
+{
+	gpio_set_level(port ? GPIO_EN_USB_C1_3A : GPIO_EN_USB_C0_3A,
+		       vbus_rp[port] == TYPEC_RP_3A0 ? 1 : 0);
+	gpio_set_level(port ? GPIO_USB_C1_5V_EN : GPIO_USB_C0_5V_EN,
+		       vbus_en[port]);
+}
+
+void typec_set_source_current_limit(int port, int rp)
+{
+	vbus_rp[port] = rp;
+
+	/* change the GPIO driving the load switch if needed */
+	board_vbus_update_source_current(port);
+}
+
+int pd_set_power_supply_ready(int port)
+{
+	/* Disable charging */
+	gpio_set_level(port ? GPIO_EN_USB_C1_CHARGE_EC_L :
+			      GPIO_EN_USB_C0_CHARGE_EC_L, 1);
+
+	/* Ensure we advertise the proper available current quota */
+	charge_manager_source_port(port, 1);
+
+	/* Provide VBUS */
+	vbus_en[port] = 1;
+	board_vbus_update_source_current(port);
+
+	pd_set_vbus_discharge(port, 0);
+
+	/* notify host of power info change */
+	pd_send_host_event(PD_EVENT_POWER_CHANGE);
+
+	return EC_SUCCESS; /* we are ready */
+}
+
+void pd_power_supply_reset(int port)
+{
+	int prev_en;
+
+	prev_en = vbus_en[port];
+
+	/* Disable VBUS */
+	vbus_en[port] = 0;
+	board_vbus_update_source_current(port);
+
+	/* Enable discharge if we were previously sourcing 5V */
+	if (prev_en)
+		pd_set_vbus_discharge(port, 1);
+
+	/* Give back the current quota we are no longer using */
+	charge_manager_source_port(port, 0);
+
+	/* notify host of power info change */
+	pd_send_host_event(PD_EVENT_POWER_CHANGE);
+}
+
+int pd_snk_is_vbus_provided(int port)
+{
+	return !gpio_get_level(port ? GPIO_USB_C1_VBUS_DET_L :
+				      GPIO_USB_C0_VBUS_DET_L);
+}
+
+int pd_board_checks(void)
+{
+	return EC_SUCCESS;
+}
+
+int pd_check_power_swap(int port)
+{
+	/*
+	 * Allow power swap as long as we are acting as a dual role device,
+	 * otherwise assume our role is fixed (not in S0 or console command
+	 * to fix our role).
+	 */
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
+}
+
+int pd_check_data_swap(int port, int data_role)
+{
+	/*
+	 * Allow data swap if we are a UFP, otherwise don't allow.
+	 *
+	 * When we are still in the Read-Only firmware, avoid swapping roles
+	 * so we don't jump in RW as a SNK/DFP and potentially confuse the
+	 * power supply by sending a soft-reset with wrong data role.
+	 */
+	return (data_role == PD_ROLE_UFP) &&
+	       (system_get_image_copy() != SYSTEM_IMAGE_RO) ? 1 : 0;
+}
+
+int pd_check_vconn_swap(int port)
+{
+	/* in G3, do not allow vconn swap since pp5000_A rail is off */
+	return gpio_get_level(GPIO_SLP_SUS_L_PMIC);
+}
+
+void pd_execute_data_swap(int port, int data_role)
+{
+	/* Only port 0 supports device mode. */
+	if (port != 0)
+		return;
+
+	gpio_set_level(GPIO_USB2_ID2,
+		      (data_role == PD_ROLE_UFP) ? 1 : 0);
+}
+
+void pd_check_pr_role(int port, int pr_role, int flags)
+{
+	/*
+	 * If partner is dual-role power and dualrole toggling is on, consider
+	 * if a power swap is necessary.
+	 */
+	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
+		/*
+		 * If we are a sink and partner is not externally powered, then
+		 * swap to become a source. If we are source and partner is
+		 * externally powered, swap to become a sink.
+		 */
+		int partner_extpower = flags & PD_FLAGS_PARTNER_EXTPOWER;
+
+		if ((!partner_extpower && pr_role == PD_ROLE_SINK) ||
+		     (partner_extpower && pr_role == PD_ROLE_SOURCE))
+			pd_request_power_swap(port);
+	}
+}
+
+void pd_check_dr_role(int port, int dr_role, int flags)
+{
+	/* If UFP, try to switch to DFP */
+	if ((flags & PD_FLAGS_PARTNER_DR_DATA) &&
+			dr_role == PD_ROLE_UFP &&
+			system_get_image_copy() != SYSTEM_IMAGE_RO)
+		pd_request_data_swap(port);
+}
+/* ----------------- Vendor Defined Messages ------------------ */
+const struct svdm_response svdm_rsp = {
+	.identity = NULL,
+	.svids = NULL,
+	.modes = NULL,
+};
+
+int pd_custom_vdm(int port, int cnt, uint32_t *payload,
+		  uint32_t **rpayload)
+{
+	int cmd = PD_VDO_CMD(payload[0]);
+	uint16_t dev_id = 0;
+	int is_rw, is_latest;
+
+	/* make sure we have some payload */
+	if (cnt == 0)
+		return 0;
+
+	switch (cmd) {
+	case VDO_CMD_VERSION:
+		/* guarantee last byte of payload is null character */
+		*(payload + cnt - 1) = 0;
+		CPRINTF("version: %s\n", (char *)(payload+1));
+		break;
+	case VDO_CMD_READ_INFO:
+	case VDO_CMD_SEND_INFO:
+		/* copy hash */
+		if (cnt == 7) {
+			dev_id = VDO_INFO_HW_DEV_ID(payload[6]);
+			is_rw = VDO_INFO_IS_RW(payload[6]);
+
+			is_latest = pd_dev_store_rw_hash(port,
+							 dev_id,
+							 payload + 1,
+							  is_rw ?
+							 SYSTEM_IMAGE_RW :
+							 SYSTEM_IMAGE_RO);
+			/*
+			 * Send update host event unless our RW hash is
+			 * already known to be the latest update RW.
+			 */
+			if (!is_rw || !is_latest)
+				pd_send_host_event(PD_EVENT_UPDATE_DEVICE);
+
+			CPRINTF("DevId:%d.%d SW:%d RW:%d\n",
+				HW_DEV_ID_MAJ(dev_id),
+				HW_DEV_ID_MIN(dev_id),
+				VDO_INFO_SW_DBG_VER(payload[6]),
+				is_rw);
+		} else if (cnt == 6) {
+			/* really old devices don't have last byte */
+			pd_dev_store_rw_hash(port, dev_id, payload + 1,
+					     SYSTEM_IMAGE_UNKNOWN);
+		}
+		break;
+	case VDO_CMD_CURRENT:
+		CPRINTF("Current: %dmA\n", payload[1]);
+		break;
+	case VDO_CMD_FLIP:
+		usb_mux_flip(port);
+		break;
+#ifdef CONFIG_USB_PD_LOGGING
+	case VDO_CMD_GET_LOG:
+		pd_log_recv_vdm(port, cnt, payload);
+		break;
+#endif /* CONFIG_USB_PD_LOGGING */
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_USB_PD_ALT_MODE_DFP
+static int dp_flags[CONFIG_USB_PD_PORT_COUNT];
+static uint32_t dp_status[CONFIG_USB_PD_PORT_COUNT];
+
+static void svdm_safe_dp_mode(int port)
+{
+	/* make DP interface safe until configure */
+	dp_flags[port] = 0;
+	dp_status[port] = 0;
+	usb_mux_set(port, TYPEC_MUX_NONE,
+		USB_SWITCH_CONNECT, pd_get_polarity(port));
+}
+
+static int svdm_enter_dp_mode(int port, uint32_t mode_caps)
+{
+	/* Only enter mode if device is DFP_D capable */
+	if (mode_caps & MODE_DP_SNK) {
+		svdm_safe_dp_mode(port);
+		return 0;
+	}
+
+	return -1;
+}
+
+static int svdm_dp_status(int port, uint32_t *payload)
+{
+	int opos = pd_alt_mode(port, 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[port] & DP_FLAGS_DP_ON)));
+	return 2;
+};
+
+static int svdm_dp_config(int port, uint32_t *payload)
+{
+	int opos = pd_alt_mode(port, USB_SID_DISPLAYPORT);
+	int mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]);
+	int pin_mode = pd_dfp_dp_get_pin_mode(port, dp_status[port]);
+
+	if (!pin_mode)
+		return 0;
+
+	usb_mux_set(port, mf_pref ? TYPEC_MUX_DOCK : TYPEC_MUX_DP,
+		    USB_SWITCH_CONNECT, pd_get_polarity(port));
+
+	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;
+};
+
+static void svdm_dp_post_config(int port)
+{
+	const struct usb_mux *mux = &usb_muxes[port];
+
+	dp_flags[port] |= DP_FLAGS_DP_ON;
+	if (!(dp_flags[port] & DP_FLAGS_HPD_HI_PENDING))
+		return;
+	mux->hpd_update(port, 1, 0);
+}
+
+static 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]);
+	const struct usb_mux *mux = &usb_muxes[port];
+
+	dp_status[port] = payload[1];
+	if (!(dp_flags[port] & DP_FLAGS_DP_ON)) {
+		if (lvl)
+			dp_flags[port] |= DP_FLAGS_HPD_HI_PENDING;
+		return 1;
+	}
+	mux->hpd_update(port, lvl, irq);
+
+	/* ack */
+	return 1;
+}
+
+static void svdm_exit_dp_mode(int port)
+{
+	const struct usb_mux *mux = &usb_muxes[port];
+
+	svdm_safe_dp_mode(port);
+	mux->hpd_update(port, 0, 0);
+}
+
+static int svdm_enter_gfu_mode(int port, uint32_t mode_caps)
+{
+	/* Always enter GFU mode */
+	return 0;
+}
+
+static void svdm_exit_gfu_mode(int port)
+{
+}
+
+static 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;
+}
+
+static int svdm_gfu_config(int port, uint32_t *payload)
+{
+	return 0;
+}
+
+static int svdm_gfu_attention(int port, uint32_t *payload)
+{
+	return 0;
+}
+
+const struct svdm_amode_fx supported_modes[] = {
+	{
+		.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,
+	},
+	{
+		.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);
+#endif /* CONFIG_USB_PD_ALT_MODE_DFP */
diff --git a/board/reef/usb_pd_policy.c b/board/reef/usb_pd_policy.c
index 20286af0a0..60cbdd3276 100644
--- a/board/reef/usb_pd_policy.c
+++ b/board/reef/usb_pd_policy.c
@@ -141,7 +141,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -175,7 +175,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/reef_it8320/usb_pd_policy.c b/board/reef_it8320/usb_pd_policy.c
index 09afde1f40..8fd92ca908 100644
--- a/board/reef_it8320/usb_pd_policy.c
+++ b/board/reef_it8320/usb_pd_policy.c
@@ -134,7 +134,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -168,7 +168,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/reef_mchp/usb_pd_policy.c b/board/reef_mchp/usb_pd_policy.c
index 515fe0bfb3..ba8195248f 100644
--- a/board/reef_mchp/usb_pd_policy.c
+++ b/board/reef_mchp/usb_pd_policy.c
@@ -143,7 +143,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 /*
@@ -183,7 +183,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/rowan/usb_pd_policy.c b/board/rowan/usb_pd_policy.c
index db428b71d7..c83fb2835c 100644
--- a/board/rowan/usb_pd_policy.c
+++ b/board/rowan/usb_pd_policy.c
@@ -88,7 +88,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -115,7 +115,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/samus_pd/usb_pd_policy.c b/board/samus_pd/usb_pd_policy.c
index 6c9147bef4..e7cf6e4a50 100644
--- a/board/samus_pd/usb_pd_policy.c
+++ b/board/samus_pd/usb_pd_policy.c
@@ -99,7 +99,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -126,7 +126,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/scarlet/usb_pd_policy.c b/board/scarlet/usb_pd_policy.c
index 3bc3d26749..e75e99f01c 100644
--- a/board/scarlet/usb_pd_policy.c
+++ b/board/scarlet/usb_pd_policy.c
@@ -107,7 +107,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -122,7 +122,7 @@ int pd_check_vconn_swap(int port)
 	 * VCONN is provided directly by the battery (PPVAR_SYS)
 	 * but use the same rules as power swap.
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 void pd_execute_data_swap(int port, int data_role)
@@ -137,7 +137,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/servo_v4/board.h b/board/servo_v4/board.h
index 271daa2782..1493d24fc8 100644
--- a/board/servo_v4/board.h
+++ b/board/servo_v4/board.h
@@ -22,7 +22,6 @@
 
 /* Enable USART1,3,4 and USB streams */
 #define CONFIG_STREAM_USART
-
 #define CONFIG_STREAM_USART3
 #define CONFIG_STREAM_USART4
 #define CONFIG_STREAM_USB
@@ -102,12 +101,9 @@
 #define CONFIG_USB_PD_PULLUP TYPEC_RP_USB
 #define CONFIG_USB_PD_VBUS_MEASURE_NOT_PRESENT
 
-/* Override PD_ROLE_DEFAULT in usb_pd.h */
-#define PD_ROLE_DEFAULT(port) ((port) ? PD_ROLE_SOURCE : PD_ROLE_SINK)
-
 /* Don't automatically change roles */
 #undef CONFIG_USB_PD_INITIAL_DRP_STATE
-#define CONFIG_USB_PD_INITIAL_DRP_STATE PD_DRP_FREEZE
+#define CONFIG_USB_PD_INITIAL_DRP_STATE PD_DRP_FORCE_SINK
 
 /* Variable-current Rp no connect and Ra attach macros */
 #define CC_NC(port, cc, sel)  (pd_tcpc_cc_nc(port, cc, sel))
diff --git a/board/servo_v4/usb_pd_policy.c b/board/servo_v4/usb_pd_policy.c
index 0608060ad9..06462405f8 100644
--- a/board/servo_v4/usb_pd_policy.c
+++ b/board/servo_v4/usb_pd_policy.c
@@ -44,9 +44,7 @@ static const uint16_t pd_src_voltages_mv[] = {
 };
 static uint32_t pd_src_chg_pdo[ARRAY_SIZE(pd_src_voltages_mv)];
 static uint8_t chg_pdo_cnt;
-static const uint32_t pd_src_host_pdo[] = {
-		PDO_FIXED(5000, 500, DUT_PDO_FIXED_FLAGS),
-};
+
 const uint32_t pd_snk_pdo[] = {
 		PDO_FIXED(5000, 500, CHG_PDO_FIXED_FLAGS),
 		PDO_BATT(4750, 21000, 15000),
@@ -62,7 +60,14 @@ static struct vbus_prop vbus[CONFIG_USB_PD_PORT_COUNT];
 static int active_charge_port = CHARGE_PORT_NONE;
 static enum charge_supplier active_charge_supplier;
 static uint8_t vbus_rp = TYPEC_RP_RESERVED;
+
+/*
+ * DTS mode: enabled connects resistors to both CC line to activate cr50,
+ * disabled connects to one only as in the standard USBC cable.
+ */
 static int disable_dts_mode;
+/* Do we allow charge through by policy? */
+static int allow_src_mode = 1;
 
 /* Voltage thresholds for no connect in DTS mode */
 static int pd_src_vnc_dts[TYPEC_RP_RESERVED][2] = {
@@ -107,9 +112,26 @@ static int charge_port_is_active(void)
 	return active_charge_port == CHG && vbus[CHG].mv > 0;
 }
 
+static void dut_allow_charge(void)
+{
+	/*
+	 * Update to charge enable if charger still present and not
+	 * already charging.
+	 */
+	if (charge_port_is_active() && allow_src_mode &&
+	    pd_get_dual_role(DUT) != PD_DRP_FORCE_SOURCE) {
+		CPRINTS("Enable DUT charge through");
+		pd_set_dual_role(DUT, PD_DRP_FORCE_SOURCE);
+		pd_config_init(DUT, PD_ROLE_SOURCE);
+		pd_update_contract(DUT);
+	}
+}
+DECLARE_DEFERRED(dut_allow_charge);
+
 static void board_manage_dut_port(void)
 {
-	int rp;
+	enum pd_dual_role_states allowed_role;
+	enum pd_dual_role_states current_role;
 
 	/*
 	 * This function is called by the CHG port whenever there has been a
@@ -118,21 +140,28 @@ static void board_manage_dut_port(void)
 	 * contract if it is connected.
 	 */
 
-	/* Assume the default value of Rp */
-	rp = TYPEC_RP_USB;
-	if (vbus[CHG].mv == PD_MIN_MV && charge_port_is_active()) {
-		/* Only advertise higher current via Rp if vbus == 5V */
-		if (vbus[CHG].ma >= 3000)
-			/* CHG port is connected and DUt can advertise 3A */
-			rp = TYPEC_RP_3A0;
-		else if (vbus[CHG].ma >= 1500)
-			rp = TYPEC_RP_1A5;
-	}
+	/* Assume the default value of Rd */
+	allowed_role = PD_DRP_FORCE_SINK;
+
+	/* If VBUS charge through is available, mark as such. */
+	if (charge_port_is_active() && allow_src_mode)
+		allowed_role = PD_DRP_FORCE_SOURCE;
 
-	/* Check if Rp setting needs to change from current value */
-	if (vbus_rp != rp)
-		/* Present new Rp value */
-		tcpm_select_rp_value(DUT, rp);
+	current_role = pd_get_dual_role(DUT);
+	if (current_role != allowed_role) {
+		/* Update role. */
+		if (allowed_role == PD_DRP_FORCE_SINK) {
+			/* We've lost charge through. Disable VBUS. */
+			gpio_set_level(GPIO_DUT_CHG_EN, 0);
+
+			/* Mark as SNK only. */
+			pd_set_dual_role(DUT, PD_DRP_FORCE_SINK);
+			pd_config_init(DUT, PD_ROLE_SINK);
+		} else {
+			/* Allow charge through after PD negotiate. */
+			hook_call_deferred(&dut_allow_charge_data, 2000 * MSEC);
+		}
+	}
 
 	/*
 	 * Update PD contract to reflect new available CHG
@@ -151,7 +180,7 @@ static void update_ports(void)
 	 * state
 	 */
 	if (!charge_port_is_active()) {
-		/* CHG Vbus has dropped, so always source DUT Vbus from host */
+		/* CHG Vbus has dropped, so become SNK. */
 		chg_pdo_cnt = 0;
 	} else {
 		/* Advertise the 'best' PDOs at various discrete voltages */
@@ -196,12 +225,6 @@ static void update_ports(void)
 
 	/* Call DUT port manager to update Rp and possible PD contract */
 	board_manage_dut_port();
-
-	/*
-	 * Supply VBUS from the CHG port if available. This may glitch VBUS
-	 * on the DUT during switchover.
-	 */
-	gpio_set_level(GPIO_HOST_OR_CHG_CTL, chg_pdo_cnt > 0);
 }
 
 int board_set_active_charge_port(int charge_port)
@@ -399,18 +422,15 @@ int board_select_rp_value(int port, int rp)
 
 int charge_manager_get_source_pdo(const uint32_t **src_pdo, const int port)
 {
-	int pdo_cnt;
+	int pdo_cnt = 0;
 
 	/*
 	 * If CHG is providing VBUS, then advertise what's available on the CHG
-	 * port, otherwise used the fixed value that matches host capabilities.
+	 * port, otherwise we provide no power.
 	 */
 	if (charge_port_is_active()) {
 		*src_pdo =  pd_src_chg_pdo;
 		pdo_cnt = chg_pdo_cnt;
-	} else {
-		*src_pdo =  pd_src_host_pdo;
-		pdo_cnt = ARRAY_SIZE(pd_src_host_pdo);
 	}
 
 	return pdo_cnt;
@@ -463,20 +483,23 @@ int pd_set_power_supply_ready(int port)
 	if (port == CHG)
 		return EC_ERROR_INVAL;
 
-	/* Enable VBUS */
-	gpio_set_level(GPIO_DUT_CHG_EN, 1);
-
 	if (charge_port_is_active()) {
+		/* Enable VBUS */
+		gpio_set_level(GPIO_DUT_CHG_EN, 1);
+
 		if (vbus[CHG].mv != PD_MIN_MV)
 			CPRINTS("ERROR, CHG port voltage %d != PD_MIN_MV",
 				vbus[CHG].mv);
 
 		vbus[DUT].mv = vbus[CHG].mv;
 		vbus[DUT].ma = vbus[CHG].mv;
+		pd_set_dual_role(DUT, PD_DRP_FORCE_SOURCE);
 	} else {
-		/* Host vbus is always 5V/500mA */
-		vbus[DUT].mv = PD_MIN_MV;
-		vbus[DUT].ma = 500;
+		vbus[DUT].mv = 0;
+		vbus[DUT].ma = 0;
+		gpio_set_level(GPIO_DUT_CHG_EN, 0);
+		pd_set_dual_role(DUT, PD_DRP_FORCE_SINK);
+		return EC_ERROR_NOT_POWERED;
 	}
 
 	/* Enable CCD, if debuggable TS attached */
@@ -497,10 +520,6 @@ void pd_power_supply_reset(int port)
 	/* Disable VBUS */
 	gpio_set_level(GPIO_DUT_CHG_EN, 0);
 
-	/* Host vbus is always 5V/500mA */
-	vbus[DUT].mv = 0;
-	vbus[DUT].ma = 0;
-
 	/* DUT is lost, back to 5V limit on CHG */
 	pd_set_external_voltage_limit(CHG, PD_MIN_MV);
 }
@@ -526,6 +545,14 @@ int pd_check_power_swap(int port)
 	 * SRC. Let servo_v4 have more control over its power role by always
 	 * rejecting power swap requests from the DUT.
 	 */
+
+	/* Port 0 can never provide vbus. */
+	if (port == CHG)
+		return 0;
+
+	if (pd_snk_is_vbus_provided(CHG))
+		return 1;
+
 	return 0;
 }
 
@@ -597,14 +624,91 @@ int pd_custom_vdm(int port, int cnt, uint32_t *payload,
 const struct svdm_amode_fx supported_modes[] = {};
 const int supported_modes_cnt = ARRAY_SIZE(supported_modes);
 
+
+static void print_cc_mode(void)
+{
+	/* Get current CCD status */
+	ccprintf("dts mode: %s\n", disable_dts_mode ? "off" : "on");
+	ccprintf("chg mode: %s\n",
+		pd_get_dual_role(DUT) == PD_DRP_FORCE_SOURCE ?
+		"on" : "off");
+	ccprintf("chg allowed: %s\n", allow_src_mode ? "on" : "off");
+}
+
+
+static void do_cc(int disable_dts_new, int allow_src_new)
+{
+	if ((disable_dts_new != disable_dts_mode) ||
+	    (allow_src_new != allow_src_mode)) {
+		/* Force detach */
+		pd_power_supply_reset(DUT);
+		/* Always set to 0 here so both CC lines are changed */
+		disable_dts_mode = 0;
+		allow_src_mode = 0;
+		/* Remove Rp/Rd on both CC lines */
+		board_select_rp_value(DUT, TYPEC_RP_RESERVED);
+
+		/* Some time for DUT to detach, use tErrorRecovery */
+		msleep(25);
+
+		/* Accept new dts/src value */
+		disable_dts_mode = disable_dts_new;
+		allow_src_mode = allow_src_new;
+		/* Can we charge? */
+		pd_set_dual_role(DUT,
+			allow_src_mode && charge_port_is_active() ?
+			PD_DRP_FORCE_SOURCE : PD_DRP_FORCE_SINK);
+
+		/* Present Rp or Rd on CC1 and CC2 based on disable_dts_mode */
+		pd_config_init(DUT,
+			pd_get_dual_role(DUT) == PD_DRP_FORCE_SOURCE);
+	}
+
+	print_cc_mode();
+}
+
+static int command_cc(int argc, char **argv)
+{
+	int disable_dts_new;
+	int allow_src_new;
+
+	if (argc < 2) {
+		print_cc_mode();
+		return EC_SUCCESS;
+	}
+
+	if (!strcasecmp(argv[1], "src")) {
+		disable_dts_new = 1;
+		allow_src_new = 1;
+	} else if (!strcasecmp(argv[1], "snk")) {
+		disable_dts_new = 1;
+		allow_src_new = 0;
+	} else if (!strcasecmp(argv[1], "srcdts")) {
+		disable_dts_new = 0;
+		allow_src_new = 1;
+	} else if (!strcasecmp(argv[1], "snkdts")) {
+		disable_dts_new = 0;
+		allow_src_new = 0;
+	} else {
+		ccprintf("Try one of src, snk, srcdts, snkdts\n");
+		return EC_ERROR_PARAM2;
+	}
+	do_cc(disable_dts_new, allow_src_new);
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(cc, command_cc,
+			"src|snk|srcdts|snkdts",
+			"Servo_v4 DTS and CHG mode");
+
+
 static int command_dts(int argc, char **argv)
 {
 	int disable_dts_new;
 	int val;
 
 	if (argc < 2) {
-		/* Get current CCD status */
-		ccprintf("dts mode: %s\n", disable_dts_mode ? "off" : "on");
+		print_cc_mode();
 		return EC_SUCCESS;
 	}
 
@@ -612,21 +716,9 @@ static int command_dts(int argc, char **argv)
 		return EC_ERROR_PARAM2;
 
 	disable_dts_new = val ^ 1;
-	if (disable_dts_new != disable_dts_mode) {
-		/* Force detach */
-		pd_power_supply_reset(DUT);
-		/* Always set to 0 here so both CC lines are changed */
-		disable_dts_mode = 0;
-		/* Remove Rp/Rd on both CC lines */
-		board_select_rp_value(DUT, TYPEC_RP_RESERVED);
-		/* Accept new disable_dts value */
-		disable_dts_mode = disable_dts_new;
-		/* Some time for DUT to detach */
-		msleep(100);
-		/* Present RP_USB on CC1 and CC2 based on disable_dts_mode */
-		board_select_rp_value(DUT, TYPEC_RP_USB);
-		ccprintf("dts mode: %s\n", disable_dts_mode ? "off" : "on");
-	}
+
+	/* Change dts without changing src. */
+	do_cc(disable_dts_new, allow_src_mode);
 
 	return EC_SUCCESS;
 }
diff --git a/board/strago/usb_pd_policy.c b/board/strago/usb_pd_policy.c
index 64e0d4f863..748e08a40e 100644
--- a/board/strago/usb_pd_policy.c
+++ b/board/strago/usb_pd_policy.c
@@ -81,7 +81,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 int pd_check_data_swap(int port, int data_role)
@@ -102,7 +102,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/board/zoombini/usb_pd_policy.c b/board/zoombini/usb_pd_policy.c
index 2a0f580718..09ac156425 100644
--- a/board/zoombini/usb_pd_policy.c
+++ b/board/zoombini/usb_pd_policy.c
@@ -68,7 +68,7 @@ int pd_check_power_swap(int port)
 	 * otherwise assume our role is fixed (not in S0 or console command
 	 * to fix our role).
 	 */
-	return pd_get_dual_role() == PD_DRP_TOGGLE_ON ? 1 : 0;
+	return pd_get_dual_role(port) == PD_DRP_TOGGLE_ON ? 1 : 0;
 }
 
 void pd_check_pr_role(int port, int pr_role, int flags)
@@ -78,7 +78,7 @@ void pd_check_pr_role(int port, int pr_role, int flags)
 	 * if a power swap is necessary.
 	 */
 	if ((flags & PD_FLAGS_PARTNER_DR_POWER) &&
-	    pd_get_dual_role() == PD_DRP_TOGGLE_ON) {
+	    pd_get_dual_role(port) == PD_DRP_TOGGLE_ON) {
 		/*
 		 * If we are a sink and partner is not externally powered, then
 		 * swap to become a source. If we are source and partner is
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index f96d9468d9..945faddd8e 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -105,7 +105,9 @@ enum vdm_states {
 
 #ifdef CONFIG_USB_PD_DUAL_ROLE
 /* Port dual-role state */
-enum pd_dual_role_states drp_state = CONFIG_USB_PD_INITIAL_DRP_STATE;
+enum pd_dual_role_states drp_state[CONFIG_USB_PD_PORT_COUNT] = {
+	[0 ... (CONFIG_USB_PD_PORT_COUNT - 1)] =
+		CONFIG_USB_PD_INITIAL_DRP_STATE};
 
 /* Enable variable for Try.SRC states */
 static uint8_t pd_try_src_enable;
@@ -1645,7 +1647,6 @@ static void handle_request(int port, uint16_t head,
 	 * is one such legal action.
 	 */
 	if (pd[port].data_role == data_role) {
-		CPRINTF("C%d DR conflict!\n", port);
 		/*
 		 * If the port doesn't support removing the terminations, just
 		 * go to the unattached state.
@@ -1837,15 +1838,16 @@ int pd_dev_store_rw_hash(int port, uint16_t dev_id, uint32_t *rw_hash,
 }
 
 #ifdef CONFIG_USB_PD_DUAL_ROLE
-enum pd_dual_role_states pd_get_dual_role(void)
+enum pd_dual_role_states pd_get_dual_role(int port)
 {
-	return drp_state;
+	return drp_state[port];
 }
 
 #ifdef CONFIG_USB_PD_TRY_SRC
 static void pd_update_try_source(void)
 {
 	int i;
+	int try_src = 0;
 
 #ifndef CONFIG_CHARGER
 	int batt_soc = board_get_battery_soc();
@@ -1853,11 +1855,15 @@ static void pd_update_try_source(void)
 	int batt_soc = charge_get_percent();
 #endif
 
+	try_src = 0;
+	for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++)
+		try_src |= drp_state[i] == PD_DRP_TOGGLE_ON;
+
 	/*
 	 * Enable try source when dual-role toggling AND battery is present
 	 * and at some minimum percentage.
 	 */
-	pd_try_src_enable = drp_state == PD_DRP_TOGGLE_ON &&
+	pd_try_src_enable = try_src &&
 			    batt_soc >= CONFIG_USB_PD_TRY_SRC_MIN_BATT_SOC;
 #if defined(CONFIG_BATTERY_PRESENT_CUSTOM) || \
 	defined(CONFIG_BATTERY_PRESENT_GPIO)
@@ -1881,10 +1887,11 @@ static void pd_update_try_source(void)
 DECLARE_HOOK(HOOK_BATTERY_SOC_CHANGE, pd_update_try_source, HOOK_PRIO_DEFAULT);
 #endif
 
-void pd_set_dual_role(enum pd_dual_role_states state)
+void pd_set_dual_role(int port, enum pd_dual_role_states state)
 {
 	int i;
-	drp_state = state;
+
+	drp_state[port] = state;
 
 #ifdef CONFIG_USB_PD_TRY_SRC
 	pd_update_try_source();
@@ -1919,9 +1926,9 @@ void pd_update_dual_role_config(int port)
 	 * disconnected state).
 	 */
 	if (pd[port].power_role == PD_ROLE_SOURCE &&
-	    ((drp_state == PD_DRP_FORCE_SINK && !pd_ts_dts_plugged(port)) ||
-	     (drp_state == PD_DRP_TOGGLE_OFF
-	      && pd[port].task_state == PD_STATE_SRC_DISCONNECTED))) {
+	    ((drp_state[port] == PD_DRP_FORCE_SINK && !pd_ts_dts_plugged(port))
+	     || (drp_state[port] == PD_DRP_TOGGLE_OFF
+		 && pd[port].task_state == PD_STATE_SRC_DISCONNECTED))) {
 		pd_set_power_role(port, PD_ROLE_SINK);
 		set_state(port, PD_STATE_SNK_DISCONNECTED);
 		tcpm_set_cc(port, TYPEC_CC_RD);
@@ -1934,7 +1941,7 @@ void pd_update_dual_role_config(int port)
 	 * new DRP state is force source.
 	 */
 	if (pd[port].power_role == PD_ROLE_SINK &&
-	    drp_state == PD_DRP_FORCE_SOURCE) {
+	    drp_state[port] == PD_DRP_FORCE_SOURCE) {
 		pd_set_power_role(port, PD_ROLE_SOURCE);
 		set_state(port, PD_STATE_SRC_DISCONNECTED);
 		tcpm_set_cc(port, TYPEC_CC_RP);
@@ -2144,11 +2151,14 @@ static void pd_init_tasks(void)
 #if defined(HAS_TASK_CHIPSET) && defined(CONFIG_USB_PD_DUAL_ROLE)
 	/* Set dual-role state based on chipset power state */
 	if (chipset_in_state(CHIPSET_STATE_ANY_OFF))
-		drp_state = PD_DRP_FORCE_SINK;
+		for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++)
+			drp_state[i] = PD_DRP_FORCE_SINK;
 	else if (chipset_in_state(CHIPSET_STATE_ANY_SUSPEND))
-		drp_state = PD_DRP_TOGGLE_OFF;
+		for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++)
+			drp_state[i] = PD_DRP_TOGGLE_OFF;
 	else /* CHIPSET_STATE_ON */
-		drp_state = PD_DRP_TOGGLE_ON;
+		for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++)
+			drp_state[i] = PD_DRP_TOGGLE_ON;
 #endif
 
 #if defined(CONFIG_USB_PD_COMM_DISABLED)
@@ -2557,8 +2567,8 @@ void pd_task(void *u)
 			else if ((pd[port].flags & PD_FLAGS_TRY_SRC &&
 				 get_time().val >= pd[port].try_src_marker) ||
 				 (!(pd[port].flags & PD_FLAGS_TRY_SRC) &&
-				  drp_state != PD_DRP_FORCE_SOURCE &&
-				  drp_state != PD_DRP_FREEZE &&
+				  drp_state[port] != PD_DRP_FORCE_SOURCE &&
+				  drp_state[port] != PD_DRP_FREEZE &&
 				 get_time().val >= next_role_swap)) {
 				pd_set_power_role(port, PD_ROLE_SINK);
 				set_state(port, PD_STATE_SNK_DISCONNECTED);
@@ -2994,8 +3004,8 @@ void pd_task(void *u)
 		}
 		case PD_STATE_SNK_DISCONNECTED:
 #ifdef CONFIG_USB_PD_LOW_POWER
-			timeout = drp_state != PD_DRP_TOGGLE_ON ? SECOND
-								: 10*MSEC;
+			timeout = (drp_state[port] !=
+				PD_DRP_TOGGLE_ON ? SECOND : 10*MSEC);
 #else
 			timeout = 10*MSEC;
 #endif
@@ -3044,7 +3054,7 @@ void pd_task(void *u)
 			}
 
 			/* If no source detected, check for role toggle. */
-			if (drp_state == PD_DRP_TOGGLE_ON &&
+			if (drp_state[port] == PD_DRP_TOGGLE_ON &&
 			    get_time().val >= next_role_swap) {
 				/* Swap roles to source */
 				pd_set_power_role(port, PD_ROLE_SOURCE);
@@ -3599,15 +3609,15 @@ void pd_task(void *u)
 				/* nothing connected, keep toggling*/
 				next_state = PD_STATE_DRP_AUTO_TOGGLE;
 			else if ((cc_is_rp(cc1) || cc_is_rp(cc2)) &&
-				 drp_state != PD_DRP_FORCE_SOURCE)
+				 drp_state[port] != PD_DRP_FORCE_SOURCE)
 				/* SNK allowed unless ForceSRC */
 				next_state = PD_STATE_SNK_DISCONNECTED;
 			else if (((cc1 == TYPEC_CC_VOLT_RD ||
 				   cc2 == TYPEC_CC_VOLT_RD) ||
 				  (cc1 == TYPEC_CC_VOLT_RA &&
 				   cc2 == TYPEC_CC_VOLT_RA)) &&
-				 (drp_state != PD_DRP_TOGGLE_OFF &&
-				  drp_state != PD_DRP_FORCE_SINK))
+				 (drp_state[port] != PD_DRP_TOGGLE_OFF &&
+				  drp_state[port] != PD_DRP_FORCE_SINK))
 				/* SRC allowed unless ForceSNK or Toggle Off */
 				next_state = PD_STATE_SRC_DISCONNECTED;
 			else
@@ -3730,16 +3740,19 @@ static void pd_chipset_resume(void)
 #endif
 			pd[i].flags |= PD_FLAGS_CHECK_PR_ROLE |
 				       PD_FLAGS_CHECK_DR_ROLE;
+		pd_set_dual_role(i, PD_DRP_TOGGLE_ON);
 	}
 
-	pd_set_dual_role(PD_DRP_TOGGLE_ON);
 	CPRINTS("PD:S3->S0");
 }
 DECLARE_HOOK(HOOK_CHIPSET_RESUME, pd_chipset_resume, HOOK_PRIO_DEFAULT);
 
 static void pd_chipset_suspend(void)
 {
-	pd_set_dual_role(PD_DRP_TOGGLE_OFF);
+	int i;
+
+	for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++)
+		pd_set_dual_role(i, PD_DRP_TOGGLE_OFF);
 	CPRINTS("PD:S0->S3");
 }
 DECLARE_HOOK(HOOK_CHIPSET_SUSPEND, pd_chipset_suspend, HOOK_PRIO_DEFAULT);
@@ -3747,9 +3760,11 @@ DECLARE_HOOK(HOOK_CHIPSET_SUSPEND, pd_chipset_suspend, HOOK_PRIO_DEFAULT);
 static void pd_chipset_startup(void)
 {
 	int i;
-	pd_set_dual_role(PD_DRP_TOGGLE_OFF);
-	for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++)
+
+	for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++) {
+		pd_set_dual_role(i, PD_DRP_TOGGLE_OFF);
 		pd[i].flags |= PD_FLAGS_CHECK_IDENTITY;
+	}
 	CPRINTS("PD:S5->S3");
 }
 DECLARE_HOOK(HOOK_CHIPSET_STARTUP, pd_chipset_startup, HOOK_PRIO_DEFAULT);
@@ -3757,10 +3772,11 @@ DECLARE_HOOK(HOOK_CHIPSET_STARTUP, pd_chipset_startup, HOOK_PRIO_DEFAULT);
 static void pd_chipset_shutdown(void)
 {
 	int i;
-	for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++)
+	for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++) {
+		pd_set_dual_role(i, PD_DRP_FORCE_SINK);
 		task_set_event(PD_PORT_TO_TASK_ID(i),
 			       PD_EVENT_DP_DISCONNECT, 0);
-	pd_set_dual_role(PD_DRP_FORCE_SINK);
+	}
 	CPRINTS("PD:S3->S5");
 }
 DECLARE_HOOK(HOOK_CHIPSET_SHUTDOWN, pd_chipset_shutdown, HOOK_PRIO_DEFAULT);
@@ -3998,45 +4014,6 @@ static int command_pd(int argc, char **argv)
 	if (argc < 2)
 		return EC_ERROR_PARAM_COUNT;
 
-#if defined(CONFIG_CMD_PD) && defined(CONFIG_USB_PD_DUAL_ROLE)
-	/* command: pd <subcmd> <args> */
-	if (!strcasecmp(argv[1], "dualrole")) {
-		if (argc < 3) {
-			ccprintf("dual-role toggling: ");
-			switch (drp_state) {
-			case PD_DRP_TOGGLE_ON:
-				ccprintf("on\n");
-				break;
-			case PD_DRP_TOGGLE_OFF:
-				ccprintf("off\n");
-				break;
-			case PD_DRP_FREEZE:
-				ccprintf("freeze\n");
-				break;
-			case PD_DRP_FORCE_SINK:
-				ccprintf("force sink\n");
-				break;
-			case PD_DRP_FORCE_SOURCE:
-				ccprintf("force source\n");
-				break;
-			}
-		} else {
-			if (!strcasecmp(argv[2], "on"))
-				pd_set_dual_role(PD_DRP_TOGGLE_ON);
-			else if (!strcasecmp(argv[2], "off"))
-				pd_set_dual_role(PD_DRP_TOGGLE_OFF);
-			else if (!strcasecmp(argv[2], "freeze"))
-				pd_set_dual_role(PD_DRP_FREEZE);
-			else if (!strcasecmp(argv[2], "sink"))
-				pd_set_dual_role(PD_DRP_FORCE_SINK);
-			else if (!strcasecmp(argv[2], "source"))
-				pd_set_dual_role(PD_DRP_FORCE_SOURCE);
-			else
-				return EC_ERROR_PARAM3;
-		}
-		return EC_SUCCESS;
-	} else
-#endif
 	if (!strcasecmp(argv[1], "dump")) {
 #ifndef CONFIG_USB_PD_DEBUG_LEVEL
 		int level;
@@ -4189,6 +4166,44 @@ static int command_pd(int argc, char **argv)
 	} else if (!strncasecmp(argv[2], "flash", 4)) {
 		return remote_flashing(argc, argv);
 #endif
+#if defined(CONFIG_CMD_PD) && defined(CONFIG_USB_PD_DUAL_ROLE)
+	} else if (!strcasecmp(argv[2], "dualrole")) {
+		if (argc < 4) {
+			ccprintf("dual-role toggling: ");
+			switch (drp_state[port]) {
+			case PD_DRP_TOGGLE_ON:
+				ccprintf("on\n");
+				break;
+			case PD_DRP_TOGGLE_OFF:
+				ccprintf("off\n");
+				break;
+			case PD_DRP_FREEZE:
+				ccprintf("freeze\n");
+				break;
+			case PD_DRP_FORCE_SINK:
+				ccprintf("force sink\n");
+				break;
+			case PD_DRP_FORCE_SOURCE:
+				ccprintf("force source\n");
+				break;
+			}
+		} else {
+			if (!strcasecmp(argv[3], "on"))
+				pd_set_dual_role(port, PD_DRP_TOGGLE_ON);
+			else if (!strcasecmp(argv[3], "off"))
+				pd_set_dual_role(port, PD_DRP_TOGGLE_OFF);
+			else if (!strcasecmp(argv[3], "freeze"))
+				pd_set_dual_role(port, PD_DRP_FREEZE);
+			else if (!strcasecmp(argv[3], "sink"))
+				pd_set_dual_role(port, PD_DRP_FORCE_SINK);
+			else if (!strcasecmp(argv[3], "source"))
+				pd_set_dual_role(port,
+					PD_DRP_FORCE_SOURCE);
+			else
+				return EC_ERROR_PARAM4;
+		}
+		return EC_SUCCESS;
+#endif
 	} else
 #endif
 	if (!strncasecmp(argv[2], "state", 5)) {
@@ -4261,7 +4276,7 @@ static int hc_usb_pd_control(struct host_cmd_handler_args *args)
 		return EC_RES_INVALID_PARAM;
 
 	if (p->role != USB_PD_CTRL_ROLE_NO_CHANGE)
-		pd_set_dual_role(dual_role_map[p->role]);
+		pd_set_dual_role(p->port, dual_role_map[p->role]);
 
 #ifdef CONFIG_USBC_SS_MUX
 	if (p->mux != USB_PD_CTRL_MUX_NO_CHANGE)
diff --git a/include/usb_pd.h b/include/usb_pd.h
index 36ee209990..c0f2f01b71 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -768,16 +768,18 @@ enum pd_dual_role_states {
 /**
  * Get dual role state
  *
+ * @param port Port number from which to get state
  * @return Current dual-role state, from enum pd_dual_role_states
  */
-enum pd_dual_role_states pd_get_dual_role(void);
+enum pd_dual_role_states pd_get_dual_role(int port);
 /**
  * Set dual role state, from among enum pd_dual_role_states
  *
+ * @param port Port number of which to set state
  * @param state New state of dual-role port, selected from
  *              enum pd_dual_role_states
  */
-void pd_set_dual_role(enum pd_dual_role_states state);
+void pd_set_dual_role(int port, enum pd_dual_role_states state);
 
 /**
  * Get role, from among PD_ROLE_SINK and PD_ROLE_SOURCE
diff --git a/test/usb_pd.c b/test/usb_pd.c
index 69b7448ea6..1d8f91f8d8 100644
--- a/test/usb_pd.c
+++ b/test/usb_pd.c
@@ -819,7 +819,8 @@ void run_test(void)
 {
 	test_reset();
 	init_ports();
-	pd_set_dual_role(PD_DRP_TOGGLE_ON);
+	pd_set_dual_role(PORT0, PD_DRP_TOGGLE_ON);
+	pd_set_dual_role(PORT1, PD_DRP_TOGGLE_ON);
 
 	RUN_TEST(test_request);
 	RUN_TEST(test_sink);
diff --git a/util/genvif.c b/util/genvif.c
index 912a16708a..37bf0c26e4 100644
--- a/util/genvif.c
+++ b/util/genvif.c
@@ -152,7 +152,7 @@ static char *is_comms_cap(void)
 
 static char *dr_swap_to_ufp_supported(void)
 {
-	if (src_pdo[0] & PDO_FIXED_DATA_SWAP)
+	if (is_src() && (src_pdo[0] & PDO_FIXED_DATA_SWAP))
 		return yes_no(pd_check_data_swap(0, PD_ROLE_DFP));
 
 	return "NO";
@@ -160,7 +160,7 @@ static char *dr_swap_to_ufp_supported(void)
 
 static char *dr_swap_to_dfp_supported(void)
 {
-	if (src_pdo[0] & PDO_FIXED_DATA_SWAP)
+	if (is_src() && (src_pdo[0] & PDO_FIXED_DATA_SWAP))
 		return yes_no(pd_check_data_swap(0, PD_ROLE_UFP));
 
 	return "NO";