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
TEST=charge through and also passive hub. Note Dru doesn't accept DTS hub.
TEST=make buildall -j

Change-Id: I19f1d1a5c37647fec72202191faa4821c06fb460
Signed-off-by: Nick Sanders <nsanders@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1096654
Reviewed-by: Aseda Aboagye <aaboagye@chromium.org>

33 files changed, 214 insertions, 126 deletions

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 cf2d5e7428..8b7b407c09 100644
--- a/board/cheza/usb_pd_policy.c
+++ b/board/cheza/usb_pd_policy.c
@@ -61,7 +61,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)
@@ -71,7 +71,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 f4ad8afead..214dc226d9 100644
--- a/board/nami/usb_pd_policy.c
+++ b/board/nami/usb_pd_policy.c
@@ -138,7 +138,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)
@@ -176,7 +176,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 5f10efaa9e..3b07ff7461 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
index 33695d9636..5d6190f915 100644
--- a/board/rammus/usb_pd_policy.c
+++ b/board/rammus/usb_pd_policy.c
@@ -138,7 +138,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/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