diff options
| -rw-r--r-- | board/dingdong/usb_pd_policy.c | 10 | ||||
| -rw-r--r-- | board/hoho/usb_pd_policy.c | 10 | ||||
| -rw-r--r-- | common/usb_pd_policy.c | 41 | ||||
| -rw-r--r-- | include/usb_pd.h | 1 |
4 files changed, 41 insertions, 21 deletions
diff --git a/board/dingdong/usb_pd_policy.c b/board/dingdong/usb_pd_policy.c index 8a0d736757..42b094fb1c 100644 --- a/board/dingdong/usb_pd_policy.c +++ b/board/dingdong/usb_pd_policy.c @@ -131,15 +131,15 @@ const uint32_t vdo_dp_mode[1] = { static int svdm_response_modes(int port, uint32_t *payload) { + int mode_cnt = ARRAY_SIZE(vdo_dp_mode); + if (PD_VDO_VID(payload[0]) != USB_SID_DISPLAYPORT) { /* TODO(tbroch) USB billboard enabled here then */ return 1; /* will generate a NAK */ } - memset(payload + 1, 0, sizeof(uint32_t) * PDO_MODES); - payload[1] = vdo_dp_mode[0]; - /* TODO(tbroch) does spec have mechanism for identifying valid modes - * returned for svid? */ - return PDO_MAX_OBJECTS; + + memcpy(payload + 1, vdo_dp_mode, sizeof(vdo_dp_mode)); + return mode_cnt + 1; } static int svdm_enter_mode(int port, uint32_t *payload) diff --git a/board/hoho/usb_pd_policy.c b/board/hoho/usb_pd_policy.c index 20097e80c1..b647219085 100644 --- a/board/hoho/usb_pd_policy.c +++ b/board/hoho/usb_pd_policy.c @@ -131,15 +131,15 @@ const uint32_t vdo_dp_mode[1] = { static int svdm_response_modes(int port, uint32_t *payload) { + int mode_cnt = ARRAY_SIZE(vdo_dp_mode); + if (PD_VDO_VID(payload[0]) != USB_SID_DISPLAYPORT) { /* TODO(tbroch) USB billboard enabled here then */ return 1; /* will generate a NAK */ } - memset(payload + 1, 0, sizeof(uint32_t) * PDO_MODES); - payload[1] = vdo_dp_mode[0]; - /* TODO(tbroch) does spec have mechanism for identifying valid modes - * returned for svid? */ - return PDO_MAX_OBJECTS; + + memcpy(payload + 1, vdo_dp_mode, sizeof(vdo_dp_mode)); + return mode_cnt + 1; } static int svdm_enter_mode(int port, uint32_t *payload) diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c index d1f47b6254..2738a2f254 100644 --- a/common/usb_pd_policy.c +++ b/common/usb_pd_policy.c @@ -16,6 +16,8 @@ #include "usb_pd_config.h" #include "version.h" +#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args) + #ifdef CONFIG_USB_PD_ALT_MODE #ifdef CONFIG_USB_PD_ALT_MODE_DFP @@ -90,10 +92,17 @@ static int dfp_discover_modes(int port, uint32_t *payload) return 1; } -static int dfp_consume_modes(int port, uint32_t *payload) +static int dfp_consume_modes(int port, int cnt, uint32_t *payload) { - memcpy(pe[port].svids[pe[port].svid_idx].mode_vdo, &payload[1], - sizeof(uint32_t) * PDO_MODES); + int idx = pe[port].svid_idx; + pe[port].svids[idx].mode_cnt = cnt - 1; + if (pe[port].svids[idx].mode_cnt < 0) { + CPRINTF("PE ERR: no modes provided for SVID\n"); + } else { + memcpy(pe[port].svids[pe[port].svid_idx].mode_vdo, &payload[1], + sizeof(uint32_t) * pe[port].svids[idx].mode_cnt); + } + pe[port].svid_idx++; return (pe[port].svid_idx < pe[port].svid_cnt); } @@ -140,17 +149,26 @@ int pd_exit_mode(int port, uint32_t *payload) static void dump_pe(int port) { int i, j; - struct svdm_amode_data *modep = &pe[port].amode; + + if (pe[port].svid_cnt < 1) { + ccprintf("No SVIDS discovered yet.\n"); + return; + } for (i = 0; i < pe[port].svid_cnt; i++) { - ccprintf("SVID[%d]: %04x", i, pe[port].svids[i].svid); - for (j = 0; j < (PDO_MAX_OBJECTS - 1); j++) - ccprintf(" [%d] %08x", j, + ccprintf("SVID[%d]: %04x MODES:", i, pe[port].svids[i].svid); + for (j = 0; j < pe[port].svids[j].mode_cnt; j++) + ccprintf(" [%d] %08x", j + 1, pe[port].svids[i].mode_vdo[j]); ccprintf("\n"); } - ccprintf("MODE[%d]: svid:%04x mode:%d caps:%08x\n", i, - modep->fx->svid, modep->index + 1, modep->mode_caps); + if (pe[port].amode.index == -1) { + ccprintf("No mode chosen yet.\n"); + return; + } + + ccprintf("MODE[%d]: svid:%04x caps:%08x\n", pe[port].amode.index + 1, + pe[port].amode.fx->svid, pe[port].amode.mode_caps); } static int command_pe(int argc, char **argv) @@ -234,13 +252,14 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload) rsize = dfp_discover_modes(port, payload); break; case CMD_DISCOVER_MODES: - if (dfp_consume_modes(port, payload)) + if (dfp_consume_modes(port, cnt, payload)) rsize = dfp_discover_modes(port, payload); else rsize = dfp_enter_mode(port, payload); break; case CMD_ENTER_MODE: - rsize = dfp_enter_mode(port, payload); + rsize = 0; + break; case CMD_EXIT_MODE: rsize = pd_exit_mode(port, payload); break; diff --git a/include/usb_pd.h b/include/usb_pd.h index 201ad057bd..82fdaec122 100644 --- a/include/usb_pd.h +++ b/include/usb_pd.h @@ -117,6 +117,7 @@ struct svdm_response { struct svdm_svid_data { uint16_t svid; + int mode_cnt; uint32_t mode_vdo[PDO_MODES]; }; |