diff options
-rw-r--r-- | common/usb_pd_alt_mode_dfp.c | 91 | ||||
-rw-r--r-- | common/usbc/usb_pe_drp_sm.c | 141 | ||||
-rw-r--r-- | include/usb_pd.h | 44 |
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 * |