pd: Add Fast role swap to the PE state machine

Optimized code to use PRS path when possible in FRS execution.
Added super state for shared path to default PRS path for debug.
Unit tests will be created after the state machine changes are completed.

BUG=b:138599955
BRANCH=none
TEST=make buildall -j

Change-Id: Ic9098dba6b86f161df9e6636194476aac5ecba56
Signed-off-by: Denis Brockus <dbrockus@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1809385

1 files changed, 145 insertions, 30 deletions

diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index 45d7050e37..b2aa88902f 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -71,6 +71,7 @@
 #define PE_FLAGS_RUN_SOURCE_START_TIMER         BIT(19)
 #define PE_FLAGS_VDM_REQUEST_BUSY               BIT(20)
 #define PE_FLAGS_VDM_REQUEST_NAKED              BIT(21)
+#define PE_FLAGS_FAST_ROLE_SWAP                 BIT(22)
 
 /* 6.7.3 Hard Reset Counter */
 #define N_HARD_RESET_COUNT 2
@@ -131,6 +132,7 @@ enum usb_pe_state {
 	PE_PRS_SNK_SRC_ASSERT_RP,
 	PE_PRS_SNK_SRC_SOURCE_ON,
 	PE_PRS_SNK_SRC_SEND_SWAP,
+	PE_FRS_SNK_SRC_START_AMS,
 	PE_VCS_EVALUATE_SWAP,
 	PE_VCS_SEND_SWAP,
 	PE_VCS_WAIT_FOR_VCONN_SWAP,
@@ -144,6 +146,9 @@ enum usb_pe_state {
 	PE_HANDLE_CUSTOM_VDM_REQUEST,
 	PE_WAIT_FOR_ERROR_RECOVERY,
 	PE_BIST,
+
+	/* Super States */
+	PE_PRS_FRS_SHARED,
 };
 
 /* Forward declare the full list of states. This is indexed by usb_pe_state */
@@ -188,11 +193,12 @@ static const char * const pe_state_names[] = {
 	[PE_PRS_SRC_SNK_TRANSITION_TO_OFF] = "PE_PRS_SRC_SNK_Transition_To_Off",
 	[PE_PRS_SRC_SNK_WAIT_SOURCE_ON] = "PE_PRS_SRC_SNK_Wait_Source_On",
 	[PE_PRS_SRC_SNK_SEND_SWAP] = "PE_PRS_SRC_SNK_Send_Swap",
-	[PE_PRS_SNK_SRC_EVALUATE_SWAP] = "PE_SNK_SRC_Evaluate_Swap",
-	[PE_PRS_SNK_SRC_TRANSITION_TO_OFF] = "PE_SNK_SRC_Transition_To_Off",
-	[PE_PRS_SNK_SRC_ASSERT_RP] = "PE_SNK_SRC_Assert_Rp",
-	[PE_PRS_SNK_SRC_SOURCE_ON] = "PE_SNK_SRC_Source_On",
-	[PE_PRS_SNK_SRC_SEND_SWAP] = "PE_SNK_SRC_Send_Swap",
+	[PE_PRS_SNK_SRC_EVALUATE_SWAP] = "PE_PRS_SNK_SRC_Evaluate_Swap",
+	[PE_PRS_SNK_SRC_TRANSITION_TO_OFF] = "PE_PRS_SNK_SRC_Transition_To_Off",
+	[PE_PRS_SNK_SRC_ASSERT_RP] = "PE_PRS_SNK_SRC_Assert_Rp",
+	[PE_PRS_SNK_SRC_SOURCE_ON] = "PE_PRS_SNK_SRC_Source_On",
+	[PE_PRS_SNK_SRC_SEND_SWAP] = "PE_PRS_SNK_SRC_Send_Swap",
+	[PE_FRS_SNK_SRC_START_AMS] = "PE_FRS_SNK_SRC_Start_Ams",
 	[PE_VCS_EVALUATE_SWAP] = "PE_VCS_Evaluate_Swap",
 	[PE_VCS_SEND_SWAP] = "PE_VCS_Send_Swap",
 	[PE_VCS_WAIT_FOR_VCONN_SWAP] = "PE_VCS_Wait_For_Vconn_Swap",
@@ -673,7 +679,12 @@ static enum usb_pe_state get_last_state_pe(const int port)
 
 static void print_current_state(const int port)
 {
-	CPRINTS("C%d: %s", port, pe_state_names[get_state_pe(port)]);
+	const char *mode = "";
+
+	if (PE_CHK_FLAG(port, PE_FLAGS_FAST_ROLE_SWAP))
+		mode = " FRS-MODE";
+
+	CPRINTS("C%d: %s%s", port, pe_state_names[get_state_pe(port)], mode);
 }
 
 static void send_source_cap(int port)
@@ -2897,11 +2908,17 @@ static void pe_prs_snk_src_evaluate_swap_run(int port)
 
 /**
  * PE_PRS_SNK_SRC_Transition_To_Off
+ * PE_FRS_SNK_SRC_Transition_To_Off
+ *
+ * NOTE: Shared action code used for Power Role Swap and Fast Role Swap
  */
 static void pe_prs_snk_src_transition_to_off_entry(int port)
 {
 	print_current_state(port);
-	tc_snk_power_off(port);
+
+	if (!PE_CHK_FLAG(port, PE_FLAGS_FAST_ROLE_SWAP))
+		tc_snk_power_off(port);
+
 	pe[port].ps_source_timer = get_time().val + PD_T_PS_SOURCE_OFF;
 }
 
@@ -2915,36 +2932,43 @@ static void pe_prs_snk_src_transition_to_off_run(int port)
 	 * Transition to ErrorRecovery state when:
 	 *   1) The PSSourceOffTimer times out.
 	 */
-	if (get_time().val > pe[port].ps_source_timer) {
+	if (get_time().val > pe[port].ps_source_timer)
 		set_state_pe(port, PE_WAIT_FOR_ERROR_RECOVERY);
-		return;
-	}
 
 	/*
 	 * Transition to PE_PRS_SNK_SRC_Assert_Rp when:
 	 *   1) An PS_RDY Message is received.
 	 */
-	if (PE_CHK_FLAG(port, PE_FLAGS_MSG_RECEIVED)) {
+	else if (PE_CHK_FLAG(port, PE_FLAGS_MSG_RECEIVED)) {
 		PE_CLR_FLAG(port, PE_FLAGS_MSG_RECEIVED);
 
 		type = PD_HEADER_TYPE(emsg[port].header);
 		cnt = PD_HEADER_CNT(emsg[port].header);
 		ext = PD_HEADER_EXT(emsg[port].header);
 
-		if ((ext == 0) && (cnt == 0) && (type == PD_CTRL_PS_RDY))
+		if ((ext == 0) && (cnt == 0) && (type == PD_CTRL_PS_RDY)) {
+			/*
+			 * FRS: We are always ready to drive vSafe5v, so just
+			 * skip PE_FRS_SNK_SRC_Vbus_Applied and go direct to
+			 * PE_FRS_SNK_SRC_Assert_Rp
+			 */
 			set_state_pe(port, PE_PRS_SNK_SRC_ASSERT_RP);
+		}
 	}
 }
 
 /**
  * PE_PRS_SNK_SRC_Assert_Rp
+ * PE_FRS_SNK_SRC_Assert_Rp
+ *
+ * NOTE: Shared action code used for Power Role Swap and Fast Role Swap
  */
 static void pe_prs_snk_src_assert_rp_entry(int port)
 {
 	print_current_state(port);
 
 	/*
-	 * Tell TypeC to Power Role Swap (PRS) from
+	 * Tell TypeC to Power/Fast Role Swap (PRS/FRS) from
 	 * Attached.SNK to Attached.SRC
 	 */
 	tc_prs_snk_src_assert_rp(port);
@@ -2954,14 +2978,19 @@ static void pe_prs_snk_src_assert_rp_run(int port)
 {
 	/* Wait until TypeC is in the Attached.SRC state */
 	if (tc_is_attached_src(port)) {
-		/* Contract is invalid now */
-		PE_CLR_FLAG(port, PE_FLAGS_EXPLICIT_CONTRACT);
+		if (!PE_CHK_FLAG(port, PE_FLAGS_FAST_ROLE_SWAP))
+			/* Contract is invalid now */
+			PE_CLR_FLAG(port, PE_FLAGS_EXPLICIT_CONTRACT);
+
 		set_state_pe(port, PE_PRS_SNK_SRC_SOURCE_ON);
 	}
 }
 
 /**
  * PE_PRS_SNK_SRC_Source_On
+ * PE_FRS_SNK_SRC_Source_On
+ *
+ * NOTE: Shared action code used for Power Role Swap and Fast Role Swap
  */
 static void pe_prs_snk_src_source_on_entry(int port)
 {
@@ -2987,20 +3016,18 @@ static void pe_prs_snk_src_source_on_run(int port)
 		prl_send_ctrl_msg(port, TCPC_TX_SOP, PD_CTRL_PS_RDY);
 		/* reset timer so PD_CTRL_PS_RDY isn't sent again */
 		pe[port].ps_source_timer = 0;
-		return;
 	}
 
 	/*
 	 * Transition to ErrorRecovery state when:
 	 *   1) On protocol error
 	 */
-	if (PE_CHK_FLAG(port, PE_FLAGS_PROTOCOL_ERROR)) {
+	else if (PE_CHK_FLAG(port, PE_FLAGS_PROTOCOL_ERROR)) {
 		PE_CLR_FLAG(port, PE_FLAGS_PROTOCOL_ERROR);
 		set_state_pe(port, PE_WAIT_FOR_ERROR_RECOVERY);
-		return;
 	}
 
-	if (PE_CHK_FLAG(port, PE_FLAGS_TX_COMPLETE)) {
+	else if (PE_CHK_FLAG(port, PE_FLAGS_TX_COMPLETE)) {
 		PE_CLR_FLAG(port, PE_FLAGS_TX_COMPLETE);
 
 		/* Run swap source timer on entry to pe_src_startup */
@@ -3012,13 +3039,28 @@ static void pe_prs_snk_src_source_on_run(int port)
 
 /**
  * PE_PRS_SNK_SRC_Send_Swap
+ * PE_FRS_SNK_SRC_Send_Swap
+ *
+ * NOTE: Shared action code used for Power Role Swap and Fast Role Swap
  */
 static void pe_prs_snk_src_send_swap_entry(int port)
 {
 	print_current_state(port);
 
-	/* Request the Protocol Layer to send a PR_Swap Message. */
-	prl_send_ctrl_msg(port, TCPC_TX_SOP, PD_CTRL_PR_SWAP);
+	/*
+	 * PRS_SNK_SRC_SEND_SWAP
+	 *     Request the Protocol Layer to send a PR_Swap Message.
+	 *
+	 * FRS_SNK_SRC_SEND_SWAP
+	 *     Hardware should have turned off sink power and started
+	 *     bringing Vbus to vSafe5.
+	 *     Request the Protocol Layer to send a FR_Swap Message.
+	 */
+	prl_send_ctrl_msg(port,
+		TCPC_TX_SOP,
+		PE_CHK_FLAG(port, PE_FLAGS_FAST_ROLE_SWAP)
+			? PD_CTRL_FR_SWAP
+			: PD_CTRL_PR_SWAP);
 
 	/* Start the SenderResponseTimer */
 	pe[port].sender_response_timer =
@@ -3032,23 +3074,26 @@ static void pe_prs_snk_src_send_swap_run(int port)
 	int ext;
 
 	/*
-	 * Transition to PE_SNK_Ready state when:
+	 * PRS: Transition to PE_SNK_Ready state when:
+	 * FRS: Transition to ErrorRecovery state when:
 	 *   1) The SenderResponseTimer times out.
 	 */
-	if (get_time().val > pe[port].sender_response_timer) {
-		set_state_pe(port, PE_SNK_READY);
-		return;
-	}
+	if (get_time().val > pe[port].sender_response_timer)
+		set_state_pe(port,
+			     PE_CHK_FLAG(port, PE_FLAGS_FAST_ROLE_SWAP)
+				? PE_WAIT_FOR_ERROR_RECOVERY
+				: PE_SNK_READY);
 
 	/*
 	 * Transition to PE_PRS_SNK_SRC_Transition_to_off when:
 	 *   1) An Accept Message is received.
 	 *
-	 * Transition to PE_SNK_Ready state when:
+	 * PRS: Transition to PE_SNK_Ready state when:
+	 * FRS: Transition to ErrorRecovery state when:
 	 *   1) A Reject Message is received.
 	 *   2) Or a Wait Message is received.
 	 */
-	if (PE_CHK_FLAG(port, PE_FLAGS_MSG_RECEIVED)) {
+	else if (PE_CHK_FLAG(port, PE_FLAGS_MSG_RECEIVED)) {
 		PE_CLR_FLAG(port, PE_FLAGS_MSG_RECEIVED);
 
 		type = PD_HEADER_TYPE(emsg[port].header);
@@ -3058,10 +3103,14 @@ static void pe_prs_snk_src_send_swap_run(int port)
 		if ((ext == 0) && (cnt == 0)) {
 			if (type == PD_CTRL_ACCEPT)
 				set_state_pe(port,
-					PE_PRS_SNK_SRC_TRANSITION_TO_OFF);
+					     PE_PRS_SNK_SRC_TRANSITION_TO_OFF);
 			else if ((type == PD_CTRL_REJECT) ||
 						(type == PD_CTRL_WAIT))
-				set_state_pe(port, PE_SNK_READY);
+				set_state_pe(port,
+					PE_CHK_FLAG(port,
+						    PE_FLAGS_FAST_ROLE_SWAP)
+					   ? PE_WAIT_FOR_ERROR_RECOVERY
+					   : PE_SNK_READY);
 		}
 	}
 }
@@ -3073,6 +3122,50 @@ static void pe_prs_snk_src_send_swap_exit(int port)
 }
 
 /**
+ * PE_FRS_SNK_SRC_Start_AMS
+ */
+static void pe_frs_snk_src_start_ams_entry(int port)
+{
+	print_current_state(port);
+
+	/* Contract is invalid now */
+	PE_CLR_FLAG(port, PE_FLAGS_EXPLICIT_CONTRACT);
+
+	/* Inform Protocol Layer this is start of AMS */
+	prl_start_ams(port);
+	tc_set_timeout(port, 2 * MSEC);
+
+	/* Shared PRS/FRS code, indicate FRS path */
+	PE_SET_FLAG(port, PE_FLAGS_FAST_ROLE_SWAP);
+	set_state_pe(port, PE_PRS_SNK_SRC_SEND_SWAP);
+}
+
+/**
+ * PE_PRS_FRS_SHARED
+ */
+static void pe_prs_frs_shared_entry(int port)
+{
+	/*
+	 * Shared PRS/FRS code, assume PRS path
+	 *
+	 * This is the super state entry. It will be called before
+	 * the first entry state to get into the PRS/FRS path.
+	 * For FRS, PE_FRS_SNK_SRC_START_AMS entry will be called
+	 * after this and that will set for the FRS path.
+	 */
+	PE_CLR_FLAG(port, PE_FLAGS_FAST_ROLE_SWAP);
+}
+
+static void pe_prs_frs_shared_exit(int port)
+{
+	/*
+	 * Shared PRS/FRS code, when not in shared path
+	 * indicate PRS path
+	 */
+	PE_CLR_FLAG(port, PE_FLAGS_FAST_ROLE_SWAP);
+}
+
+/**
  * BIST
  */
 static void pe_bist_entry(int port)
@@ -4645,6 +4738,12 @@ DECLARE_HOST_COMMAND(EC_CMD_USB_PD_GET_AMODE,
 #endif /* CONFIG_USB_PD_ALT_MODE_DFP */
 
 static const struct usb_state pe_states[] = {
+	/* Super States */
+	[PE_PRS_FRS_SHARED] = {
+		.entry = pe_prs_frs_shared_entry,
+		.exit  = pe_prs_frs_shared_exit,
+	},
+
 	/* Normal States */
 	[PE_SRC_STARTUP] = {
 		.entry = pe_src_startup_entry,
@@ -4796,22 +4895,38 @@ static const struct usb_state pe_states[] = {
 		.entry = pe_prs_snk_src_evaluate_swap_entry,
 		.run   = pe_prs_snk_src_evaluate_swap_run,
 	},
+	/*
+	 * Some of the Power Role Swap actions are shared with the very
+	 * similar actions of Fast Role Swap.
+	 */
+	/* State actions are shared with PE_FRS_SNK_SRC_TRANSITION_TO_OFF */
 	[PE_PRS_SNK_SRC_TRANSITION_TO_OFF] = {
 		.entry = pe_prs_snk_src_transition_to_off_entry,
 		.run   = pe_prs_snk_src_transition_to_off_run,
+		.parent = &pe_states[PE_PRS_FRS_SHARED],
 	},
+	/* State actions are shared with PE_FRS_SNK_SRC_ASSERT_RP */
 	[PE_PRS_SNK_SRC_ASSERT_RP] = {
 		.entry = pe_prs_snk_src_assert_rp_entry,
 		.run   = pe_prs_snk_src_assert_rp_run,
+		.parent = &pe_states[PE_PRS_FRS_SHARED],
 	},
+	/* State actions are shared with PE_FRS_SNK_SRC_SOURCE_ON */
 	[PE_PRS_SNK_SRC_SOURCE_ON] = {
 		.entry = pe_prs_snk_src_source_on_entry,
 		.run   = pe_prs_snk_src_source_on_run,
+		.parent = &pe_states[PE_PRS_FRS_SHARED],
 	},
+	/* State actions are shared with PE_FRS_SNK_SRC_SEND_SWAP */
 	[PE_PRS_SNK_SRC_SEND_SWAP] = {
 		.entry = pe_prs_snk_src_send_swap_entry,
 		.run   = pe_prs_snk_src_send_swap_run,
 		.exit  = pe_prs_snk_src_send_swap_exit,
+		.parent = &pe_states[PE_PRS_FRS_SHARED],
+	},
+	[PE_FRS_SNK_SRC_START_AMS] = {
+		.entry = pe_frs_snk_src_start_ams_entry,
+		.parent = &pe_states[PE_PRS_FRS_SHARED],
 	},
 	[PE_VCS_EVALUATE_SWAP] = {
 		.entry = pe_vcs_evaluate_swap_entry,