TCPMv2: Implement mode discovery for port partner

Implement PE_INIT_VDM_Modes_{Request,ACKed,NAKed}. Store mode discovery
state for each discovered SVID and provide accessors for that state.
Consider transmit type when consuming modes.

BUG=b:152420269,b:152419795
TEST=Attach port partner; TCPM discovers modes for previously discovered
TEST=SVIDs and then continues with discovery/mode-entering process.
BRANCH=none

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

3 files changed, 266 insertions, 10 deletions

diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
index d0d337e0ab..1c04c51591 100644
--- a/common/usb_pd_alt_mode_dfp.c
+++ b/common/usb_pd_alt_mode_dfp.c
@@ -366,19 +366,48 @@ void dfp_consume_svids(int port, enum tcpm_transmit_type type, int cnt,
 void dfp_consume_modes(int port, enum tcpm_transmit_type type, int cnt,
 		uint32_t *payload)
 {
+	int svid_idx;
+	struct svid_mode_data *mode_discovery = NULL;
 	struct pd_discovery *disc = pd_get_am_discovery(port, type);
-	int idx = disc->svid_idx;
+	uint16_t response_svid = (uint16_t) PD_VDO_VID(payload[0]);
 
-	disc->svids[idx].mode_cnt = cnt - 1;
+	for (svid_idx = 0; svid_idx < disc->svid_cnt; ++svid_idx) {
+		uint16_t svid = disc->svids[svid_idx].svid;
 
-	if (disc->svids[idx].mode_cnt < 0) {
+		if (svid == response_svid) {
+			mode_discovery = &disc->svids[svid_idx];
+			break;
+		}
+	}
+	if (!mode_discovery) {
+		const struct svid_mode_data *requested_mode_data =
+			pd_get_next_mode(port, type);
+		CPRINTF("C%d: Mode response for undiscovered SVID %x, but TCPM "
+				"requested SVID %x\n",
+				port, response_svid, requested_mode_data->svid);
+		/*
+		 * Although SVIDs discovery seemed like it succeeded before, the
+		 * partner is now responding with undiscovered SVIDs. Discovery
+		 * cannot reasonably continue under these circumstances.
+		 */
+		pd_set_modes_discovery(port, type, requested_mode_data->svid,
+				PD_DISC_FAIL);
+		return;
+	}
+
+	mode_discovery->mode_cnt = cnt - 1;
+	if (mode_discovery->mode_cnt < 1) {
 		CPRINTF("ERR:NOMODE\n");
-	} else {
-		memcpy(disc->svids[idx].mode_vdo, &payload[1],
-		       sizeof(uint32_t) * disc->svids[idx].mode_cnt);
+		pd_set_modes_discovery(port, type, mode_discovery->svid,
+				PD_DISC_FAIL);
+		return;
 	}
 
+	memcpy(mode_discovery->mode_vdo, &payload[1],
+			sizeof(uint32_t) * mode_discovery->mode_cnt);
 	disc->svid_idx++;
+	pd_set_modes_discovery(port, type, mode_discovery->svid,
+			PD_DISC_COMPLETE);
 }
 
 /*
@@ -485,6 +514,56 @@ uint16_t pd_get_svid(int port, uint16_t svid_idx, enum tcpm_transmit_type type)
 	return disc->svids[svid_idx].svid;
 }
 
+void pd_set_modes_discovery(int port, enum tcpm_transmit_type type,
+		uint16_t svid, enum pd_discovery_state disc)
+{
+	struct pd_discovery *pd = pd_get_am_discovery(port, type);
+	int svid_idx;
+
+	for (svid_idx = 0; svid_idx < pd->svid_cnt; ++svid_idx) {
+		struct svid_mode_data *mode_data = &pd->svids[svid_idx];
+
+		if (mode_data->svid != svid)
+			continue;
+
+		mode_data->discovery = disc;
+		return;
+	}
+}
+
+enum pd_discovery_state pd_get_modes_discovery(int port,
+		enum tcpm_transmit_type type)
+{
+	const struct svid_mode_data *mode_data = pd_get_next_mode(port, type);
+
+	/*
+	 * If there are no SVIDs for which to discover modes, mode discovery is
+	 * trivially complete.
+	 */
+	if (!mode_data)
+		return PD_DISC_COMPLETE;
+
+	return mode_data->discovery;
+}
+
+struct svid_mode_data *pd_get_next_mode(int port,
+		enum tcpm_transmit_type type)
+{
+	struct pd_discovery *disc = pd_get_am_discovery(port, type);
+	int svid_idx;
+
+	for (svid_idx = 0; svid_idx < disc->svid_cnt; ++svid_idx) {
+		struct svid_mode_data *mode_data = &disc->svids[svid_idx];
+
+		if (mode_data->discovery == PD_DISC_COMPLETE)
+			continue;
+
+		return mode_data;
+	}
+
+	return NULL;
+}
+
 uint32_t *pd_get_mode_vdo(int port, uint16_t svid_idx,
 		enum tcpm_transmit_type type)
 {
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index 6c8c1f3356..d144f07ba7 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -237,6 +237,7 @@ enum usb_pe_state {
 	PE_VDM_IDENTITY_REQUEST_CBL,
 	PE_INIT_PORT_VDM_IDENTITY_REQUEST,
 	PE_INIT_VDM_SVIDS_REQUEST,
+	PE_INIT_VDM_MODES_REQUEST,
 	PE_VDM_REQUEST,
 	PE_VDM_ACKED,
 	PE_VDM_RESPONSE,
@@ -323,6 +324,7 @@ static const char * const pe_state_names[] = {
 	[PE_INIT_PORT_VDM_IDENTITY_REQUEST] =
 					   "PE_INIT_PORT_VDM_Identity_Request",
 	[PE_INIT_VDM_SVIDS_REQUEST] = "PE_INIT_VDM_SVIDs_Request",
+	[PE_INIT_VDM_MODES_REQUEST] = "PE_INIT_VDM_Modes_Request",
 	[PE_VDM_REQUEST] = "PE_VDM_Request",
 	[PE_VDM_ACKED] = "PE_VDM_Acked",
 	[PE_VDM_RESPONSE] = "PE_VDM_Response",
@@ -1296,6 +1298,12 @@ static bool pe_attempt_port_discovery(int port)
 			pe[port].tx_type = TCPC_TX_SOP;
 			set_state_pe(port, PE_INIT_VDM_SVIDS_REQUEST);
 			return true;
+		} else if (pd_get_modes_discovery(port, TCPC_TX_SOP) ==
+				PD_DISC_NEEDED &&
+				pe_can_send_sop_vdm(port, CMD_DISCOVER_MODES)) {
+			pe[port].tx_type = TCPC_TX_SOP;
+			set_state_pe(port, PE_INIT_VDM_MODES_REQUEST);
+			return true;
 		/*
 		 * Note: determine if next VDM can be sent by taking advantage
 		 * of discovery following the VDM command enum ordering.
@@ -4528,7 +4536,7 @@ static void pe_init_vdm_svids_request_run(int port)
 			ext == 0) {
 		/*
 		 * Valid Discover SVIDs ACKs should have at least 2 objects
-		 * (header, SVID VDO).
+		 * (VDM header, SVID VDO).
 		 */
 		if (PD_VDO_CMDT(payload[0]) == CMDT_RSP_ACK &&
 							cnt >= 2) {
@@ -4577,6 +4585,131 @@ static void pe_init_vdm_svids_request_exit(int port)
 {
 	/* Invalidate TX type so that it must be set before next call */
 	pe[port].tx_type = TCPC_TX_INVALID;
+
+	/*
+	 * No need to mark modes discovery as failed. If no SVIDs were
+	 * discovered, mode discovery is trivially complete.
+	 */
+}
+
+/**
+ * PE_INIT_VDM_Modes_Request
+ *
+ * Used for SOP and SOP' requests, selected by pe[port].tx_type prior to entry.
+ */
+static void pe_init_vdm_modes_request_entry(int port)
+{
+	uint32_t *msg = (uint32_t *)tx_emsg[port].buf;
+	const struct svid_mode_data *mode_data =
+		pd_get_next_mode(port, pe[port].tx_type);
+	uint16_t svid;
+	/*
+	 * The caller should have checked that there was something to discover
+	 * before entering this state.
+	 */
+	assert(mode_data);
+	assert(mode_data->discovery == PD_DISC_NEEDED);
+	svid = mode_data->svid;
+
+	print_current_state(port);
+
+	if (pe[port].tx_type == TCPC_TX_INVALID) {
+		CPRINTS("C%d: TX type expected to be set, returning", port);
+		set_state_pe(port, get_last_state_pe(port));
+		return;
+	}
+
+	msg[0] = VDO((uint16_t) svid, 1,
+			VDO_SVDM_VERS(pd_get_vdo_ver(port, pe[port].tx_type)) |
+			CMD_DISCOVER_MODES);
+	tx_emsg[port].len = sizeof(uint32_t);
+
+	prl_send_data_msg(port, pe[port].tx_type, PD_DATA_VENDOR_DEF);
+}
+
+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;
+
+	/* No message received */
+	if (!PE_CHK_FLAG(port, PE_FLAGS_MSG_RECEIVED))
+		return;
+
+	assert(mode_data);
+	assert(mode_data->discovery == PD_DISC_NEEDED);
+	requested_svid = mode_data->svid;
+
+	/* 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);
+			/*
+			 * TODO(b:152419850): Fake vdm_cmd for now to
+			 * ensure existing discovery process continues.
+			 */
+			pe[port].vdm_cmd = DISCOVER_MODES;
+		} 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 {
+		/*
+		 * Unexpected Message Received. Src.Ready or Snk.Ready can
+		 * handle it.
+		 */
+		PE_SET_FLAG(port, PE_FLAGS_MSG_RECEIVED);
+	}
+
+	/* Return to calling state (PE_{SRC,SNK}_Ready) */
+	set_state_pe(port, get_last_state_pe(port));
+}
+
+static void pe_init_vdm_modes_request_exit(int port)
+{
+	/* Invalidate TX type so it must be set before next call */
+	pe[port].tx_type = TCPC_TX_INVALID;
 }
 
 /**
@@ -5664,6 +5797,12 @@ static const struct usb_state pe_states[] = {
 		.exit   = pe_init_vdm_svids_request_exit,
 		.parent = &pe_states[PE_VDM_SEND_REQUEST],
 	},
+	[PE_INIT_VDM_MODES_REQUEST] = {
+		.entry	= pe_init_vdm_modes_request_entry,
+		.run	= pe_init_vdm_modes_request_run,
+		.exit   = pe_init_vdm_modes_request_exit,
+		.parent = &pe_states[PE_VDM_SEND_REQUEST],
+	},
 	[PE_VDM_REQUEST] = {
 		.entry = pe_vdm_request_entry,
 		.run   = pe_vdm_request_run,
diff --git a/include/usb_pd.h b/include/usb_pd.h
index a5d6bbf34c..436b217a9a 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -285,6 +285,8 @@ struct svid_mode_data {
 	int mode_cnt;
 	/* The discovered mode VDOs */
 	uint32_t mode_vdo[PDO_MODES];
+	/* State of mode discovery for this SVID */
+	enum pd_discovery_state discovery;
 };
 
 struct svdm_amode_fx {
@@ -1696,14 +1698,50 @@ void pd_set_svids_discovery(int port, enum tcpm_transmit_type type,
 /**
  * Get SVID discovery state for this type and port
  *
- * @param port  USB-C port number
- * @param type	SOP* type to retrieve
- * @return      Current discovery state (failed or complete)
+ * @param port USB-C port number
+ * @param type SOP* type to retrieve
+ * @return     Current discovery state (failed or complete)
  */
 enum pd_discovery_state pd_get_svids_discovery(int port,
 		enum tcpm_transmit_type type);
 
 /**
+ * Set Modes discovery state for this port, SOP* type, and SVID.
+ *
+ * @param port USB-C port number
+ * @param type SOP* type to set
+ * @param svid SVID to set mode discovery state for
+ * @param disc Discovery state to set (failed or complete)
+ */
+void pd_set_modes_discovery(int port, enum tcpm_transmit_type type,
+		uint16_t svid, enum pd_discovery_state disc);
+
+/**
+ * Get Modes discovery state for this port and SOP* type. Modes discover is
+ * considered complete for a port and type when modes have been discovered for
+ * all discovered SVIDs. Mode discovery is failed if mode discovery for any SVID
+ * failed.
+ *
+ * @param port USB-C port number
+ * @param type SOP* type to retrieve
+ * @return      Current discovery state (failed or complete)
+ */
+enum pd_discovery_state pd_get_modes_discovery(int port,
+		enum tcpm_transmit_type type);
+
+/**
+ * Get a pointer to mode data for the next SVID with undiscovered modes. This
+ * data may indicate that discovery failed.
+ *
+ * @param port USB-C port number
+ * @param type SOP* type to retrieve
+ * @return     Pointer to the first SVID-mode structure with undiscovered mode;
+ *             discovery may be needed or failed; returns NULL if all SVIDs have
+ *             discovered modes
+ */
+struct svid_mode_data *pd_get_next_mode(int port, enum tcpm_transmit_type type);
+
+/**
  * Return a pointer to the discover identity response structure for this SOP*
  * type
  *