diff options
| -rw-r--r-- | common/usbc/usb_pe_drp_sm.c | 121 |
1 files changed, 120 insertions, 1 deletions
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c index 64dc785fc1..b7d5bf2b5c 100644 --- a/common/usbc/usb_pe_drp_sm.c +++ b/common/usbc/usb_pe_drp_sm.c @@ -236,6 +236,7 @@ enum usb_pe_state { PE_VDM_SEND_REQUEST, PE_VDM_IDENTITY_REQUEST_CBL, PE_INIT_PORT_VDM_IDENTITY_REQUEST, + PE_INIT_VDM_SVIDS_REQUEST, PE_VDM_REQUEST, PE_VDM_ACKED, PE_VDM_RESPONSE, @@ -321,6 +322,7 @@ static const char * const pe_state_names[] = { [PE_VDM_IDENTITY_REQUEST_CBL] = "PE_VDM_Identity_Request_Cbl", [PE_INIT_PORT_VDM_IDENTITY_REQUEST] = "PE_INIT_PORT_VDM_Identity_Request", + [PE_INIT_VDM_SVIDS_REQUEST] = "PE_INIT_VDM_SVIDs_Request", [PE_VDM_REQUEST] = "PE_VDM_Request", [PE_VDM_ACKED] = "PE_VDM_Acked", [PE_VDM_RESPONSE] = "PE_VDM_Response", @@ -1288,6 +1290,12 @@ static bool pe_attempt_port_discovery(int port) set_state_pe(port, PE_INIT_PORT_VDM_IDENTITY_REQUEST); return true; + } else if (pd_get_svids_discovery(port, TCPC_TX_SOP) == + PD_DISC_NEEDED && + pe_can_send_sop_vdm(port, CMD_DISCOVER_SVID)) { + pe[port].tx_type = TCPC_TX_SOP; + set_state_pe(port, PE_INIT_VDM_SVIDS_REQUEST); + return true; /* * Note: determine if next VDM can be sent by taking advantage * of discovery following the VDM command enum ordering. @@ -4464,6 +4472,111 @@ static void pe_init_port_vdm_identity_request_exit(int port) */ pd_set_identity_discovery(port, TCPC_TX_SOP, PD_DISC_FAIL); } + + /* Do not attempt further discovery if identity discovery failed. */ + if (pd_get_identity_discovery(port, TCPC_TX_SOP) == PD_DISC_FAIL) + pd_set_svids_discovery(port, TCPC_TX_SOP, PD_DISC_FAIL); +} + +/** + * PE_INIT_VDM_SVIDs_Request + * + * Used for SOP and SOP' requests, selected by pe[port].tx_type prior to entry. + */ +static void pe_init_vdm_svids_request_entry(int port) +{ + uint32_t *msg = (uint32_t *)tx_emsg[port].buf; + + 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(USB_SID_PD, 1, + VDO_SVDM_VERS(pd_get_vdo_ver(port, pe[port].tx_type)) | + CMD_DISCOVER_SVID); + 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_svids_request_run(int port) +{ + uint32_t *payload; + int sop; + uint8_t type; + uint8_t cnt; + uint8_t ext; + + /* No message received */ + if (!PE_CHK_FLAG(port, PE_FLAGS_MSG_RECEIVED)) + return; + + PE_CLR_FLAG(port, PE_FLAGS_MSG_RECEIVED); + + /* 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 + * (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); + /* + * TODO(b:152419850): Fake vdm_cmd for now to ensure + * existing discovery process continues. + */ + pe[port].vdm_cmd = DISCOVER_SVIDS; + } 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 { + /* + * 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_svids_request_exit(int port) +{ + /* Invalidate TX type so that it must be set before next call */ + pe[port].tx_type = TCPC_TX_INVALID; } /** @@ -4575,7 +4688,7 @@ static void pe_vdm_request_run(int port) /* Fake busy response so we try to send command again */ PE_SET_FLAG(port, PE_FLAGS_VDM_REQUEST_BUSY); } else if (get_time().val > pe[port].vdm_response_timer) { - CPRINTF("VDM %s Response Timeout\n", + CPRINTF("C%d: VDM %s Response Timeout\n", port, pe[port].partner_type ? "Cable" : "Port"); PE_SET_FLAG(port, PE_FLAGS_VDM_REQUEST_NAKED); @@ -5545,6 +5658,12 @@ static const struct usb_state pe_states[] = { .exit = pe_init_port_vdm_identity_request_exit, .parent = &pe_states[PE_VDM_SEND_REQUEST], }, + [PE_INIT_VDM_SVIDS_REQUEST] = { + .entry = pe_init_vdm_svids_request_entry, + .run = pe_init_vdm_svids_request_run, + .exit = pe_init_vdm_svids_request_exit, + .parent = &pe_states[PE_VDM_SEND_REQUEST], + }, [PE_VDM_REQUEST] = { .entry = pe_vdm_request_entry, .run = pe_vdm_request_run, |