TCPMv2: Consolidate common code for VDM states

Move common run code for VDM states to
pe_discovery_vdm_request_run_common.

Reduces flash size by an average of 337 bytes.
volteer RO reduced by 320 bytes. volteer RW reduced by 304 bytes.

BUG=b:158572770
TEST=Normal discovery traffic on Volteer, observed via Twinkie
BRANCH=none

Change-Id: I3082b9a81983e406f5f98d4c0e0f27ee66dce14e
Signed-off-by: Abe Levkoy <alevkoy@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2248250
Reviewed-by: Diana Z <dzigterman@chromium.org>

1 files changed, 92 insertions, 102 deletions

diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index 45e2ed4206..4b62ffe59b 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -4185,6 +4185,64 @@ static void pe_vdm_send_request_exit(int port)
 	PE_CLR_FLAG(port, PE_FLAGS_INTERRUPTIBLE_AMS);
 }
 
+static enum pd_discovery_state pe_discovery_vdm_request_run_common(int port,
+		uint8_t min_data_objects)
+{
+	uint32_t *payload;
+	int sop;
+	uint8_t type;
+	uint8_t cnt;
+	uint8_t ext;
+
+	/* Retrieve the message information */
+	payload = (uint32_t *)rx_emsg[port].buf;
+	sop = PD_HEADER_GET_SOP(rx_emsg[port].header);
+	type = PD_HEADER_TYPE(rx_emsg[port].header);
+	cnt = PD_HEADER_CNT(rx_emsg[port].header);
+	ext = PD_HEADER_EXT(rx_emsg[port].header);
+
+	if (sop == pe[port].tx_type && type == PD_DATA_VENDOR_DEF && cnt >= 1
+			&& ext == 0) {
+		if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_ACK &&
+				cnt >= min_data_objects) {
+			/* Handle ACKs in state-specific code. */
+			return PD_DISC_COMPLETE;
+		} else if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_NAK) {
+			/* Handle NAKs in state-specific code. */
+			return PD_DISC_FAIL;
+		} else if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_BUSY) {
+			/*
+			 * Don't fill in the discovery field so we re-probe in
+			 * tVDMBusy
+			 */
+			CPRINTS("C%d: Partner BUSY, request will be retried",
+					port);
+			pe[port].discover_identity_timer =
+					get_time().val + PD_T_VDM_BUSY;
+			return PD_DISC_NEEDED;
+		}
+
+		/*
+		 * Partner gave us an incorrect size or command; mark discovery
+		 * as failed.
+		 */
+		CPRINTS("C%d: Unexpected VDM response: 0x%04x 0x%04x",
+				port, rx_emsg[port].header, payload[0]);
+		return PD_DISC_FAIL;
+	} else if (sop == pe[port].tx_type && ext == 0 && cnt == 0 &&
+			type == PD_CTRL_NOT_SUPPORTED) {
+		/*
+		 * A NAK would be more expected here, but Not Supported is still
+		 * allowed with the same meaning.
+		 */
+		return PD_DISC_FAIL;
+	}
+
+	/* Unexpected Message Received. Src.Ready or Snk.Ready can handle it. */
+	PE_SET_FLAG(port, PE_FLAGS_MSG_RECEIVED);
+	return PD_DISC_NEEDED;
+}
+
 /**
  * PE_VDM_IDENTITY_REQUEST_CBL
  * Combination of PE_INIT_PORT_VDM_Identity_Request State specific to the
@@ -4505,69 +4563,36 @@ static void pe_init_vdm_svids_request_run(int port)
 {
 	uint32_t *payload;
 	int sop;
-	uint8_t type;
 	uint8_t cnt;
-	uint8_t ext;
+	enum pd_discovery_state response_result;
 
 	/* No message received */
 	if (!PE_CHK_FLAG(port, PE_FLAGS_MSG_RECEIVED))
 		return;
-
 	PE_CLR_FLAG(port, PE_FLAGS_MSG_RECEIVED);
 
+	/*
+	 * Valid Discover SVIDs ACKs should have at least 2 objects (VDM header,
+	 * SVID VDO).
+	 */
+	response_result = pe_discovery_vdm_request_run_common(port, 2);
+
 	/* Retrieve the message information */
 	payload = (uint32_t *)rx_emsg[port].buf;
 	sop = PD_HEADER_GET_SOP(rx_emsg[port].header);
-	type = PD_HEADER_TYPE(rx_emsg[port].header);
 	cnt = PD_HEADER_CNT(rx_emsg[port].header);
-	ext = PD_HEADER_EXT(rx_emsg[port].header);
 
-	if (sop == pe[port].tx_type && type == PD_DATA_VENDOR_DEF && cnt > 0 &&
-			ext == 0) {
-		/*
-		 * Valid Discover SVIDs ACKs should have at least 2 objects
-		 * (VDM header, SVID VDO).
-		 */
-		if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_ACK &&
-							cnt >= 2) {
-			/* PE_INIT_VDM_SVIDs_ACKed embedded here */
-			dfp_consume_svids(port, sop, cnt, payload);
-		} else if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_NAK) {
-			/* PE_INIT_VDM_SVIDs_NAKed embedded here */
-			pd_set_svids_discovery(port, sop, PD_DISC_FAIL);
-		} else if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_BUSY) {
-			/*
-			 * Don't fill in the discovery field so we
-			 * re-probe in tVDMBusy
-			 */
-			CPRINTS("C%d: Partner Busy, Discover SVIDs will be "
-					"re-tried", port);
-			pe[port].discover_identity_timer =
-					get_time().val + PD_T_VDM_BUSY;
-		} else {
-			/*
-			 * Partner gave us an incorrect size or command,
-			 * mark discovery as failed
-			 */
-			pd_set_svids_discovery(port, sop, PD_DISC_FAIL);
-			CPRINTS("C%d: Unexpected DiscSVID response: 0x%04x "
-					"0x%04x",
-					port, rx_emsg[port].header, payload[0]);
-		}
-	} else if (sop == pe[port].tx_type && ext == 0 && cnt == 0 &&
-			type == PD_CTRL_NOT_SUPPORTED) {
-		/*
-		 * A NAK would be more expected here, but Not Supported is still
-		 * allowed with the same meaning.
-		 */
+	if (response_result == PD_DISC_COMPLETE) {
+		/* PE_INIT_VDM_SVIDs_ACKed embedded here */
+		dfp_consume_svids(port, sop, cnt, payload);
+	} else if (response_result == PD_DISC_FAIL) {
+		/* PE_INIT_VDM_SVIDs_NAKed embedded here */
 		pd_set_svids_discovery(port, sop, PD_DISC_FAIL);
-	} else {
-		/*
-		 * Unexpected Message Received. Src.Ready or Snk.Ready can
-		 * handle it.
-		 */
-		PE_SET_FLAG(port, PE_FLAGS_MSG_RECEIVED);
 	}
+	/*
+	 * If the received message doesn't change the discovery state, there is
+	 * nothing to do but return to the ready previous state.
+	 */
 
 	/* Return to calling state (PE_{SRC,SNK}_Ready) */
 	set_state_pe(port, get_last_state_pe(port));
@@ -4623,78 +4648,43 @@ static void pe_init_vdm_modes_request_run(int port)
 {
 	uint32_t *payload;
 	int sop;
-	uint8_t type;
 	uint8_t cnt;
-	uint8_t ext;
 	struct svid_mode_data *mode_data =
 		pd_get_next_mode(port, pe[port].tx_type);
 	uint16_t requested_svid;
+	enum pd_discovery_state response_result;
 
 	/* No message received */
 	if (!PE_CHK_FLAG(port, PE_FLAGS_MSG_RECEIVED))
 		return;
+	PE_CLR_FLAG(port, PE_FLAGS_MSG_RECEIVED);
 
 	assert(mode_data);
 	assert(mode_data->discovery == PD_DISC_NEEDED);
 	requested_svid = mode_data->svid;
 
+	/*
+	 * Valid Discover Modes responses should have at least 2 objects (VDM
+	 * header and at least one mode VDO).
+	 */
+	response_result = pe_discovery_vdm_request_run_common(port, 2);
+
 	/* Retrieve the message information */
 	payload = (uint32_t *)rx_emsg[port].buf;
 	sop = PD_HEADER_GET_SOP(rx_emsg[port].header);
-	type = PD_HEADER_TYPE(rx_emsg[port].header);
 	cnt = PD_HEADER_CNT(rx_emsg[port].header);
-	ext = PD_HEADER_EXT(rx_emsg[port].header);
 
-	PE_CLR_FLAG(port, PE_FLAGS_MSG_RECEIVED);
-
-	if (sop == pe[port].tx_type && type == PD_DATA_VENDOR_DEF &&
-						cnt >= 2 && ext == 0) {
-		/*
-		 * Valid Discover Modes responses should have at least 2 objects
-		 * (VDM header and at least one mode VDO).
-		 */
-		if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_ACK) {
-			/* PE_INIT_VDM_Modes_ACKed embedded here */
-			dfp_consume_modes(port, sop, cnt, payload);
-		} else if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_NAK) {
-			/* PE_INIT_VDM_Modes_NAKed embedded here */
-			pd_set_modes_discovery(port, sop,
-					requested_svid, PD_DISC_FAIL);
-		} else if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_BUSY) {
-			/*
-			 * Don't fill in the discovery field so we
-			 * re-probe in tVDMBusy
-			 */
-			CPRINTS("C%d: Partner Busy, Discover Modes will be "
-					"re-tried", port);
-			pe[port].discover_identity_timer =
-					get_time().val + PD_T_VDM_BUSY;
-		} else {
-			/*
-			 * Partner gave us an incorrect size or command,
-			 * mark discovery as failed
-			 */
-			pd_set_modes_discovery(port, sop,
-					requested_svid, PD_DISC_FAIL);
-			CPRINTS("C%d: Unexpected Discover Modes response: "
-					"0x%04x 0x%04x",
-					port, rx_emsg[port].header,
-					payload[0]);
-		}
-	} else if (sop == pe[port].tx_type && ext == 0 && cnt == 0 &&
-			type == PD_CTRL_NOT_SUPPORTED) {
-		/*
-		 * A NAK would be more expected here, but Not Supported is still
-		 * allowed with the same meaning.
-		 */
-		pd_set_modes_discovery(port, requested_svid, sop, PD_DISC_FAIL);
-	} else {
-		/*
-		 * Unexpected Message Received. Src.Ready or Snk.Ready can
-		 * handle it.
-		 */
-		PE_SET_FLAG(port, PE_FLAGS_MSG_RECEIVED);
+	if (response_result == PD_DISC_COMPLETE) {
+		/* PE_INIT_VDM_Modes embedded here */
+		dfp_consume_modes(port, sop, cnt, payload);
+	} else if (response_result == PD_DISC_FAIL) {
+		/* PE_INIT_VDM_Modes embedded here */
+		pd_set_modes_discovery(port, sop, requested_svid, PD_DISC_FAIL);
 	}
+	/*
+	 * If the received message doesn't change the discovery state, there is
+	 * nothing to do but return to the ready previous state.
+	 */
 
 	/* Return to calling state (PE_{SRC,SNK}_Ready) */
 	set_state_pe(port, get_last_state_pe(port));