diff options
| -rw-r--r-- | board/oak/usb_pd_policy.c | 22 |
1 files changed, 17 insertions, 5 deletions
diff --git a/board/oak/usb_pd_policy.c b/board/oak/usb_pd_policy.c index e60f8745db..86ceb9fe3c 100644 --- a/board/oak/usb_pd_policy.c +++ b/board/oak/usb_pd_policy.c @@ -242,13 +242,16 @@ int pd_custom_vdm(int port, int cnt, uint32_t *payload, #ifdef CONFIG_USB_PD_ALT_MODE_DFP static int dp_flags[CONFIG_USB_PD_PORT_COUNT]; +/* DP Status VDM as returned by UFP */ +static uint32_t dp_status[CONFIG_USB_PD_PORT_COUNT]; static void svdm_safe_dp_mode(int port) { /* make DP interface safe until configure */ dp_flags[port] = 0; + dp_status[port] = 0; usb_mux_set(port, TYPEC_MUX_NONE, - USB_SWITCH_CONNECT, pd_get_polarity(port)); + USB_SWITCH_CONNECT, pd_get_polarity(port)); } static int svdm_enter_dp_mode(int port, uint32_t mode_caps) @@ -281,13 +284,21 @@ static int svdm_dp_status(int port, uint32_t *payload) static int svdm_dp_config(int port, uint32_t *payload) { int opos = pd_alt_mode(port, USB_SID_DISPLAYPORT); - usb_mux_set(port, TYPEC_MUX_DP, - USB_SWITCH_CONNECT, pd_get_polarity(port)); + int mf_pref = PD_VDO_DPSTS_MF_PREF(dp_status[port]); + int pin_mode = pd_dfp_dp_get_pin_mode(port, dp_status[port]); + + if (!pin_mode) + return 0; + + usb_mux_set(port, mf_pref ? TYPEC_MUX_DOCK : TYPEC_MUX_DP, + USB_SWITCH_CONNECT, pd_get_polarity(port)); + payload[0] = VDO(USB_SID_DISPLAYPORT, 1, CMD_DP_CONFIG | VDO_OPOS(opos)); - payload[1] = VDO_DP_CFG(MODE_DP_PIN_E, /* pin mode */ + + payload[1] = VDO_DP_CFG(pin_mode, /* pin mode */ 1, /* DPv1.3 signaling */ - 2); /* UFP connected */ + 2); /* UFP_U connected as UFP_D */ return 2; }; @@ -305,6 +316,7 @@ static int svdm_dp_attention(int port, uint32_t *payload) int lvl = PD_VDO_DPSTS_HPD_LVL(payload[1]); int irq = PD_VDO_DPSTS_HPD_IRQ(payload[1]); + dp_status[port] = payload[1]; cur_lvl = gpio_get_level(GPIO_USB_DP_HPD); /* Its initial DP status message prior to config */ |