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>

40 files changed, 312 insertions, 204 deletions

diff --git a/baseboard/dragonegg/usb_pd_policy.c b/baseboard/dragonegg/usb_pd_policy.c
index bca4961d45..e7def7fe71 100644
--- a/baseboard/dragonegg/usb_pd_policy.c
+++ b/baseboard/dragonegg/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/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 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
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index 8f5bafe2c1..e165523676 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -106,7 +106,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;
@@ -1796,7 +1798,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.
@@ -1988,15 +1989,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();
@@ -2004,11 +2006,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)
@@ -2032,10 +2038,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();
@@ -2056,9 +2063,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);
@@ -2071,7 +2078,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);
@@ -2281,11 +2288,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)
@@ -2637,8 +2647,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);
@@ -3074,8 +3084,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
@@ -3124,7 +3134,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);
@@ -3688,15 +3698,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
@@ -3830,16 +3840,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);
@@ -3847,16 +3860,21 @@ 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);
 
 static void pd_chipset_shutdown(void)
 {
-	pd_set_dual_role(PD_DRP_FORCE_SINK);
+	int i;
+
+	for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++)
+		pd_set_dual_role(i, PD_DRP_FORCE_SINK);
 	CPRINTS("PD:S3->S5");
 }
 DECLARE_HOOK(HOOK_CHIPSET_SHUTDOWN, pd_chipset_shutdown, HOOK_PRIO_DEFAULT);
@@ -4085,45 +4103,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;
@@ -4276,6 +4255,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)) {
@@ -4348,7 +4365,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 fce59707b3..78c142bf43 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -779,16 +779,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";