pd: allow policy layer to ask for PR/DR_swap on new contract

Allow policy layer to request a PR or DR swap upon formation of
a power contract. Zinger always asks for a data swap so it can
be a UFP, and Samus asks for a data swap only if it is a UFP to
become a DFP.

BUG=chrome-os-partner:33754, chrome-os-partner:31195
BRANCH=samus
TEST=load onto samus and zinger and make sure they swap roles
upon connect with no collisions

Change-Id: I275c9669549c26f25c58f80845daad8edab11313
Signed-off-by: Alec Berg <alecaberg@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/229327
Reviewed-by: Vincent Palatin <vpalatin@chromium.org>

14 files changed, 244 insertions, 84 deletions

diff --git a/board/dingdong/usb_pd_policy.c b/board/dingdong/usb_pd_policy.c
index 82386a0c96..d082f99c60 100644
--- a/board/dingdong/usb_pd_policy.c
+++ b/board/dingdong/usb_pd_policy.c
@@ -116,13 +116,13 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* Always refuse power swap */
 	return 0;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Always refuse data swap */
 	return 0;
@@ -132,6 +132,11 @@ void pd_execute_data_swap(int port, int data_role)
 {
 	/* Do nothing */
 }
+
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+}
 /* ----------------- Vendor Defined Messages ------------------ */
 const uint32_t vdo_idh = VDO_IDH(0, /* data caps as USB host */
 				 0, /* data caps as USB device */
diff --git a/board/firefly/usb_pd_policy.c b/board/firefly/usb_pd_policy.c
index 8fe1add0b0..4a04988290 100644
--- a/board/firefly/usb_pd_policy.c
+++ b/board/firefly/usb_pd_policy.c
@@ -154,13 +154,13 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* Always refuse power swap */
 	return 0;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Always refuse data swap */
 	return 0;
@@ -170,3 +170,8 @@ void pd_execute_data_swap(int port, int data_role)
 {
 	/* Do nothing */
 }
+
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+}
diff --git a/board/fruitpie/usb_pd_policy.c b/board/fruitpie/usb_pd_policy.c
index 3344872a43..fcc25d1345 100644
--- a/board/fruitpie/usb_pd_policy.c
+++ b/board/fruitpie/usb_pd_policy.c
@@ -143,13 +143,13 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* Always allow power swap */
 	return 1;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Always allow data swap */
 	return 1;
@@ -159,6 +159,11 @@ void pd_execute_data_swap(int port, int data_role)
 {
 	/* Do nothing */
 }
+
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+}
 /* ----------------- Vendor Defined Messages ------------------ */
 const struct svdm_response svdm_rsp = {
 	.identity = NULL,
diff --git a/board/hoho/usb_pd_policy.c b/board/hoho/usb_pd_policy.c
index 2164918a7a..adb3e1ddc4 100644
--- a/board/hoho/usb_pd_policy.c
+++ b/board/hoho/usb_pd_policy.c
@@ -116,13 +116,13 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* Always refuse power swap */
 	return 0;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Always refuse data swap */
 	return 0;
@@ -132,6 +132,11 @@ void pd_execute_data_swap(int port, int data_role)
 {
 	/* Do nothing */
 }
+
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+}
 /* ----------------- Vendor Defined Messages ------------------ */
 const uint32_t vdo_idh = VDO_IDH(0, /* data caps as USB host */
 				 0, /* data caps as USB device */
diff --git a/board/host/usb_pd_policy.c b/board/host/usb_pd_policy.c
index 8d927d0cb4..6432e0ae7b 100644
--- a/board/host/usb_pd_policy.c
+++ b/board/host/usb_pd_policy.c
@@ -145,13 +145,13 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* Always allow power swap */
 	return 1;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Always allow data swap */
 	return 1;
@@ -162,6 +162,11 @@ void pd_execute_data_swap(int port, int data_role)
 	/* Do nothing */
 }
 
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+}
+
 int pd_custom_vdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 {
 	return 0;
diff --git a/board/plankton/usb_pd_policy.c b/board/plankton/usb_pd_policy.c
index ea240107c2..d5274ba0b5 100644
--- a/board/plankton/usb_pd_policy.c
+++ b/board/plankton/usb_pd_policy.c
@@ -170,13 +170,13 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* Always allow power swap */
 	return 1;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Always allow data swap */
 	return 1;
@@ -186,3 +186,8 @@ void pd_execute_data_swap(int port, int data_role)
 {
 	/* Do nothing */
 }
+
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+}
diff --git a/board/ryu/usb_pd_policy.c b/board/ryu/usb_pd_policy.c
index 8026a413df..aea73188d4 100644
--- a/board/ryu/usb_pd_policy.c
+++ b/board/ryu/usb_pd_policy.c
@@ -158,19 +158,24 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* TODO: use battery level to decide to accept/reject power swap */
 	/* Always allow power swap */
 	return 1;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Always allow data swap */
 	return 1;
 }
 
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+}
+
 void pd_execute_data_swap(int port, int data_role)
 {
 	/* TODO: what do we need to do to change host controller data role? */
diff --git a/board/ryu_p1/usb_pd_policy.c b/board/ryu_p1/usb_pd_policy.c
index 8c58452a6e..9ee61dde65 100644
--- a/board/ryu_p1/usb_pd_policy.c
+++ b/board/ryu_p1/usb_pd_policy.c
@@ -158,19 +158,24 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* TODO: use battery level to decide to accept/reject power swap */
 	/* Always allow power swap */
 	return 1;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Always allow data swap */
 	return 1;
 }
 
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+}
+
 void pd_execute_data_swap(int port, int data_role)
 {
 	/* TODO: what do we need to do to change host controller data role? */
diff --git a/board/samus_pd/usb_pd_policy.c b/board/samus_pd/usb_pd_policy.c
index c63debcf16..2004f6c53f 100644
--- a/board/samus_pd/usb_pd_policy.c
+++ b/board/samus_pd/usb_pd_policy.c
@@ -206,14 +206,14 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* TODO: use battery level to decide to accept/reject power swap */
 	/* Always allow power swap */
 	return 1;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Allow data swap if we are a UFP, otherwise don't allow */
 	return (data_role == PD_ROLE_UFP) ? 1 : 0;
@@ -223,6 +223,14 @@ void pd_execute_data_swap(int port, int data_role)
 {
 	/* TODO: when switching to UFP need to open D+/D- switches */
 }
+
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+	/* If UFP, try to switch to DFP */
+	if (partner_dr_swap && dr_role == PD_ROLE_UFP)
+		pd_request_data_swap(port);
+}
 /* ----------------- Vendor Defined Messages ------------------ */
 const struct svdm_response svdm_rsp = {
 	.identity = NULL,
diff --git a/board/twinkie/usb_pd_policy.c b/board/twinkie/usb_pd_policy.c
index f38f522698..ec59b82769 100644
--- a/board/twinkie/usb_pd_policy.c
+++ b/board/twinkie/usb_pd_policy.c
@@ -156,18 +156,23 @@ int pd_board_checks(void)
 	return EC_SUCCESS;
 }
 
-int pd_power_swap(int port)
+int pd_check_power_swap(int port)
 {
 	/* Always refuse power swap */
 	return 0;
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Always allow data swap */
 	return 1;
 }
 
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+}
+
 void pd_execute_data_swap(int port, int data_role)
 {
 	/* Do nothing */
diff --git a/board/zinger/usb_pd_policy.c b/board/zinger/usb_pd_policy.c
index be86f26271..d2d0fbcb8a 100644
--- a/board/zinger/usb_pd_policy.c
+++ b/board/zinger/usb_pd_policy.c
@@ -253,7 +253,7 @@ void pd_power_supply_reset(int port)
 		discharge_voltage(voltages[0].ovp);
 }
 
-int pd_data_swap(int port, int data_role)
+int pd_check_data_swap(int port, int data_role)
 {
 	/* Allow data swap if we are a DFP, otherwise don't allow */
 	return (data_role == PD_ROLE_DFP) ? 1 : 0;
@@ -264,6 +264,14 @@ void pd_execute_data_swap(int port, int data_role)
 	/* Do nothing */
 }
 
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap)
+{
+	/* If DFP, try to switch to UFP */
+	if (partner_dr_swap && dr_role == PD_ROLE_DFP)
+		pd_request_data_swap(port);
+}
+
 int pd_board_checks(void)
 {
 #ifdef CONFIG_HIBERNATE
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index e513d02772..6c9dc18e04 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -231,11 +231,15 @@ static int pd_src_cap_cnt[PD_PORT_COUNT];
 #define PD_FLAGS_PARTNER_DR_DATA  (1 << 2) /* port partner is dual-role data */
 #define PD_FLAGS_DATA_SWAPPED     (1 << 3) /* data swap complete */
 #define PD_FLAGS_SNK_CAP_RECVD    (1 << 4) /* sink capabilities received */
+#define PD_FLAGS_GET_SNK_CAP_SENT (1 << 5) /* get sink cap sent */
+#define PD_FLAGS_NEW_CONTRACT     (1 << 6) /* new power contract established */
 /* Flags to clear on a disconnect */
 #define PD_FLAGS_RESET_ON_DISCONNECT_MASK (PD_FLAGS_PARTNER_DR_POWER | \
 					   PD_FLAGS_PARTNER_DR_DATA | \
 					   PD_FLAGS_DATA_SWAPPED | \
-					   PD_FLAGS_SNK_CAP_RECVD)
+					   PD_FLAGS_SNK_CAP_RECVD | \
+					   PD_FLAGS_GET_SNK_CAP_SENT | \
+					   PD_FLAGS_NEW_CONTRACT)
 
 static struct pd_protocol {
 	/* current port power role (SOURCE or SINK) */
@@ -884,6 +888,28 @@ static void handle_data_request(int port, uint16_t head,
 	}
 }
 
+#ifdef CONFIG_USB_PD_DUAL_ROLE
+void pd_request_power_swap(int port)
+{
+	if (pd[port].task_state == PD_STATE_SRC_READY)
+		set_state(port, PD_STATE_SRC_SWAP_INIT);
+	else if (pd[port].task_state == PD_STATE_SNK_READY)
+		set_state(port, PD_STATE_SNK_SWAP_INIT);
+	task_wake(PORT_TO_TASK_ID(port));
+}
+#endif
+
+void pd_request_data_swap(int port)
+{
+	if (pd[port].task_state == PD_STATE_SRC_READY)
+		set_state(port, PD_STATE_SRC_DR_SWAP);
+#ifdef CONFIG_USB_PD_DUAL_ROLE
+	else if (pd[port].task_state == PD_STATE_SNK_READY)
+		set_state(port, PD_STATE_SNK_DR_SWAP);
+#endif
+	task_wake(PORT_TO_TASK_ID(port));
+}
+
 static void pd_dr_swap(int port)
 {
 	pd[port].data_role = !pd[port].data_role;
@@ -987,7 +1013,7 @@ static void handle_ctrl_request(int port, uint16_t head,
 		break;
 	case PD_CTRL_PR_SWAP:
 #ifdef CONFIG_USB_PD_DUAL_ROLE
-		if (pd_power_swap(port)) {
+		if (pd_check_power_swap(port)) {
 			send_control(port, PD_CTRL_ACCEPT);
 			if (pd[port].power_role == PD_ROLE_SINK)
 				set_state(port, PD_STATE_SNK_SWAP_SNK_DISABLE);
@@ -1001,7 +1027,7 @@ static void handle_ctrl_request(int port, uint16_t head,
 #endif
 		break;
 	case PD_CTRL_DR_SWAP:
-		if (pd_data_swap(port, pd[port].data_role)) {
+		if (pd_check_data_swap(port, pd[port].data_role)) {
 			/* Accept switch and perform data swap */
 			if (send_control(port, PD_CTRL_ACCEPT) >= 0)
 				pd_dr_swap(port);
@@ -1419,6 +1445,7 @@ static inline int get_typec_current_limit(int cc_voltage)
 void pd_set_new_power_request(int port)
 {
 	pd[port].new_power_request = 1;
+	task_wake(PORT_TO_TASK_ID(port));
 }
 #endif /* CONFIG_CHARGE_MANAGER */
 
@@ -1429,7 +1456,7 @@ void pd_task(void)
 	uint32_t payload[7];
 	int timeout = 10*MSEC;
 	int cc1_volt, cc2_volt;
-	int res;
+	int res, incoming_packet;
 #ifdef CONFIG_USB_PD_DUAL_ROLE
 	uint64_t next_role_swap = PD_T_DRP_SNK;
 	int hard_reset_count = 0;
@@ -1479,12 +1506,15 @@ void pd_task(void)
 		task_wait_event(timeout);
 		/* incoming packet ? */
 		if (pd_rx_started(port) && pd_comm_enabled) {
+			incoming_packet = 1;
 			head = analyze_rx(port, payload);
 			pd_rx_complete(port);
 			if (head > 0)
 				handle_request(port,  head, payload);
 			else if (head == PD_ERR_HARD_RESET)
 				execute_hard_reset(port);
+		} else {
+			incoming_packet = 0;
 		}
 		/* if nothing to do, verify the state of the world in 500ms */
 		this_state = pd[port].task_state;
@@ -1550,6 +1580,11 @@ void pd_task(void)
 		case PD_STATE_SRC_STARTUP:
 			/* Wait for power source to enable */
 			if (pd[port].last_state != pd[port].task_state) {
+				/*
+				 * fake set data role swapped flag so we send
+				 * discover identity when we enter SRC_READY
+				 */
+				pd[port].flags |= PD_FLAGS_DATA_SWAPPED;
 				caps_count = 0;
 				src_connected = 0;
 				set_state_timeout(
@@ -1614,12 +1649,8 @@ void pd_task(void)
 			/* the voltage output is good, notify the source */
 			res = send_control(port, PD_CTRL_PS_RDY);
 			if (res >= 0) {
-				timeout =  PD_T_SEND_SOURCE_CAP;
-				/*
-				 * fake set data role swapped flag so we send
-				 * discover identity when we enter SRC_READY
-				 */
-				pd[port].flags |= PD_FLAGS_DATA_SWAPPED;
+				timeout = 10*MSEC;
+				pd[port].flags |= PD_FLAGS_NEW_CONTRACT;
 				/* it'a time to ping regularly the sink */
 				set_state(port, PD_STATE_SRC_READY);
 			} else {
@@ -1630,28 +1661,43 @@ void pd_task(void)
 			break;
 		case PD_STATE_SRC_READY:
 			timeout = PD_T_SOURCE_ACTIVITY;
-			if (pd[port].last_state != pd[port].task_state &&
+
+			if (pd[port].last_state != pd[port].task_state)
+				pd[port].flags |= PD_FLAGS_GET_SNK_CAP_SENT;
+
+			/*
+			 * Don't send any PD traffic if we woke up due to
+			 * incoming packet to avoid collisions
+			 */
+			if (incoming_packet)
+				break;
+
+			/* Send get sink cap if haven't received it yet */
+			if ((pd[port].flags & PD_FLAGS_GET_SNK_CAP_SENT) &&
 			    !(pd[port].flags & PD_FLAGS_SNK_CAP_RECVD)) {
 				/* Get sink cap to know if dual-role device */
 				send_control(port, PD_CTRL_GET_SINK_CAP);
+				pd[port].flags &= ~PD_FLAGS_GET_SNK_CAP_SENT;
 				break;
 			}
 
-			/* Send VDMs once after get sink cap */
+			/* Check our role policy, which may trigger a swap */
+			if (pd[port].flags & PD_FLAGS_NEW_CONTRACT) {
+				pd_new_contract(
+					port, PD_ROLE_SOURCE,
+					pd[port].data_role,
+					pd[port].flags &
+						PD_FLAGS_PARTNER_DR_POWER,
+					pd[port].flags &
+						PD_FLAGS_PARTNER_DR_DATA);
+				pd[port].flags &= ~PD_FLAGS_NEW_CONTRACT;
+				break;
+			}
+
+			/* Send discovery SVDMs last */
 			if (pd[port].data_role == PD_ROLE_DFP &&
 			    (pd[port].flags & PD_FLAGS_DATA_SWAPPED)) {
-#ifdef CONFIG_USB_PD_SIMPLE_DFP
-				/*
-				 * For simple devices that don't support
-				 * alternate mode and are only sources (ie power
-				 * adapters), send custom VDM with info about
-				 * this device once power contract has been
-				 * negotiated.
-				 */
-				pd_send_vdm(port, USB_VID_GOOGLE,
-					    VDO_CMD_SEND_INFO,
-					    pd_get_info(), 6);
-#else
+#ifndef CONFIG_USB_PD_SIMPLE_DFP
 				pd_send_vdm(port, USB_SID_PD,
 					    CMD_DISCOVER_IDENT, NULL, 0);
 #endif
@@ -1813,11 +1859,18 @@ void pd_task(void)
 			 */
 			if ((pd[port].last_state != pd[port].task_state)
 			    && hard_reset_count < PD_HARD_RESET_COUNT
-			    && pd_comm_enabled)
+			    && pd_comm_enabled) {
+				/*
+				 * fake set data role swapped flag so we send
+				 * discover identity when we enter SRC_READY
+				 */
+				pd[port].flags |= PD_FLAGS_DATA_SWAPPED;
+				pd[port].flags |= PD_FLAGS_NEW_CONTRACT;
 				set_state_timeout(port,
 						  get_time().val +
 						  PD_T_SINK_WAIT_CAP,
 						  PD_STATE_HARD_RESET);
+			}
 			break;
 		case PD_STATE_SNK_REQUESTED:
 			/* Ensure the power supply actually becomes ready */
@@ -1834,16 +1887,16 @@ void pd_task(void)
 						  PD_STATE_HARD_RESET);
 			break;
 		case PD_STATE_SNK_READY:
-			/* if DFP, send SVDM on entry */
-			if (pd[port].data_role == PD_ROLE_DFP &&
-			    (pd[port].last_state != pd[port].task_state ||
-			     (pd[port].flags & PD_FLAGS_DATA_SWAPPED))) {
-				pd_send_vdm(port, USB_SID_PD,
-					    CMD_DISCOVER_IDENT, NULL, 0);
-				pd[port].flags &= ~PD_FLAGS_DATA_SWAPPED;
-			}
+			timeout = 20*MSEC;
 
-			/* we have power, check vitals from time to time */
+			/*
+			 * Don't send any PD traffic if we woke up due to
+			 * incoming packet to avoid collisions
+			 */
+			if (incoming_packet)
+				break;
+
+			/* Check for new power to request */
 			if (pd[port].new_power_request) {
 				pd[port].new_power_request = 0;
 #ifdef CONFIG_CHARGE_MANAGER
@@ -1858,8 +1911,37 @@ void pd_task(void)
 #endif
 					pd_send_request_msg(port,
 							    PD_REQUEST_MAX);
+				break;
 			}
-			timeout = 100*MSEC;
+
+			/*
+			 * Give time for source to check role policy first by
+			 * not running this the first time through state.
+			 */
+			if ((pd[port].flags & PD_FLAGS_NEW_CONTRACT) &&
+			    pd[port].last_state == pd[port].task_state) {
+				pd_new_contract(
+					port, PD_ROLE_SINK,
+					pd[port].data_role,
+					pd[port].flags &
+						PD_FLAGS_PARTNER_DR_POWER,
+					pd[port].flags &
+						PD_FLAGS_PARTNER_DR_DATA);
+				pd[port].flags &= ~PD_FLAGS_NEW_CONTRACT;
+				break;
+			}
+
+			/* If DFP, send discovery SVDMs */
+			if (pd[port].data_role == PD_ROLE_DFP &&
+			     (pd[port].flags & PD_FLAGS_DATA_SWAPPED)) {
+				pd_send_vdm(port, USB_SID_PD,
+					    CMD_DISCOVER_IDENT, NULL, 0);
+				pd[port].flags &= ~PD_FLAGS_DATA_SWAPPED;
+				break;
+			}
+
+			/* Sent all messages, don't need to wake very often */
+			timeout = 200*MSEC;
 			break;
 		case PD_STATE_SNK_DR_SWAP:
 			if (pd[port].last_state != pd[port].task_state) {
@@ -2322,21 +2404,12 @@ static int command_pd(int argc, char **argv)
 		if (argc < 4)
 			return EC_ERROR_PARAM_COUNT;
 
-		if (!strncasecmp(argv[3], "power", 5)) {
-			if (pd[port].power_role == PD_ROLE_SINK)
-				set_state(port, PD_STATE_SNK_SWAP_INIT);
-			else
-				set_state(port, PD_STATE_SRC_SWAP_INIT);
-			task_wake(PORT_TO_TASK_ID(port));
-		} else if (!strncasecmp(argv[3], "data", 4)) {
-			if (pd[port].power_role == PD_ROLE_SINK)
-				set_state(port, PD_STATE_SNK_DR_SWAP);
-			else
-				set_state(port, PD_STATE_SRC_DR_SWAP);
-			task_wake(PORT_TO_TASK_ID(port));
-		} else {
+		if (!strncasecmp(argv[3], "power", 5))
+			pd_request_power_swap(port);
+		else if (!strncasecmp(argv[3], "data", 4))
+			pd_request_data_swap(port);
+		else
 			return EC_ERROR_PARAM3;
-		}
 	} else if (!strncasecmp(argv[2], "ping", 4)) {
 		int enable;
 
diff --git a/include/config.h b/include/config.h
index cd0f5ed4d4..b0e2536e2a 100644
--- a/include/config.h
+++ b/include/config.h
@@ -1100,7 +1100,7 @@
 /* Define if using internal comparator for PD receive */
 #undef CONFIG_USB_PD_INTERNAL_COMP
 
-/* Simple DFP, such as power adapter, will send info CVDM on connect */
+/* Simple DFP, such as power adapter, will not send discovery VDM on connect */
 #undef CONFIG_USB_PD_SIMPLE_DFP
 
 /* Use comparator module for PD RX interrupt */
diff --git a/include/usb_pd.h b/include/usb_pd.h
index 8bc4f6dd16..22d753e7d8 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -689,9 +689,9 @@ void pd_request_source_voltage(int port, int mv);
 /**
  * Set the PD input current limit.
  *
- * @port USB-C port number
- * @max_ma Maximum current limit
- * @supply_voltage Voltage at which current limit is applied
+ * @param port USB-C port number
+ * @param max_ma Maximum current limit
+ * @param supply_voltage Voltage at which current limit is applied
  */
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage);
@@ -699,9 +699,9 @@ void pd_set_input_current_limit(int port, uint32_t max_ma,
 /**
  * Set the type-C input current limit.
  *
- * @port USB-C port number
- * @max_ma Maximum current limit
- * @supply_voltage Voltage at which current limit is applied
+ * @param port USB-C port number
+ * @param max_ma Maximum current limit
+ * @param supply_voltage Voltage at which current limit is applied
  */
 void typec_set_input_current_limit(int port, uint32_t max_ma,
 				   uint32_t supply_voltage);
@@ -716,25 +716,37 @@ int pd_board_checks(void);
 /**
  * Check if power swap is allowed.
  *
- * @port USB-C port number
+ * @param port USB-C port number
  * @return True if power swap is allowed, False otherwise
  */
-int pd_power_swap(int port);
+int pd_check_power_swap(int port);
 
 /**
  * Check if data swap is allowed.
  *
- * @port USB-C port number
- * @data_role current data role
+ * @param port USB-C port number
+ * @param data_role current data role
  * @return True if data swap is allowed, False otherwise
  */
-int pd_data_swap(int port, int data_role);
+int pd_check_data_swap(int port, int data_role);
+
+/**
+ * A new power contract has been established
+ *
+ * @param port USB-C port number
+ * @param pr_role Our power role
+ * @param dr_role Our data role
+ * @param partner_pr_swap Partner supports PR_SWAP
+ * @param partner_dr_swap Partner supports DR_SWAP
+ */
+void pd_new_contract(int port, int pr_role, int dr_role,
+		     int partner_pr_swap, int partner_dr_swap);
 
 /**
  * Execute data swap.
  *
- * @port USB-C port number
- * @data_role new data role
+ * @param port USB-C port number
+ * @param data_role new data role
  */
 void pd_execute_data_swap(int port, int data_role);
 
@@ -1047,6 +1059,20 @@ int pd_get_partner_dualrole_capable(int port);
 int pd_get_partner_data_swap_capable(int port);
 
 /**
+ * Request power swap command to be issued
+ *
+ * @param port USB-C port number
+ */
+void pd_request_power_swap(int port);
+
+/**
+ * Request data swap command to be issued
+ *
+ * @param port USB-C port number
+ */
+void pd_request_data_swap(int port);
+
+/**
  * Set the PD communication enabled flag. When communication is disabled,
  * the port can still detect connection and source power but will not
  * send or respond to any PD communication.