5 files changed, 357 insertions, 69 deletions

diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
index e4a7eb294a..42b7c9e465 100644
--- a/common/usb_pd_alt_mode_dfp.c
+++ b/common/usb_pd_alt_mode_dfp.c
@@ -11,6 +11,7 @@
 #include "task_id.h"
 #include "timer.h"
 #include "usb_charge.h"
+#include "usb_dp_alt_mode.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
@@ -85,6 +86,12 @@ static int pd_allocate_mode(int port, uint16_t svid)
 		for (j = 0; j < disc->svid_cnt; j++) {
 			struct svid_mode_data *svidp = &disc->svids[j];
 
+			/*
+			 * Looking for a match between supported_modes and
+			 * discovered SVIDs; must also match the passed-in SVID
+			 * if that was non-zero. Otherwise, go to the next
+			 * discovered SVID.
+			 */
 			if ((svidp->svid != supported_modes[i].svid) ||
 			    (svid && (svidp->svid != svid)))
 				continue;
@@ -571,6 +578,22 @@ uint32_t *pd_get_mode_vdo(int port, uint16_t svid_idx,
 	return disc->svids[svid_idx].mode_vdo;
 }
 
+bool pd_is_mode_discovered_for_svid(int port, enum tcpm_transmit_type type,
+		uint16_t svid)
+{
+	const struct pd_discovery *disc = pd_get_am_discovery(port, type);
+	const struct svid_mode_data *mode_data;
+
+	for (mode_data = disc->svids; mode_data < disc->svids + disc->svid_cnt;
+			++mode_data) {
+		if (mode_data->svid == svid &&
+				mode_data->discovery == PD_DISC_COMPLETE)
+			return true;
+	}
+
+	return false;
+}
+
 void notify_sysjump_ready(void)
 {
 	/*
@@ -1187,6 +1210,9 @@ __overridable void svdm_exit_dp_mode(int port)
 	if (port == USB_PD_PORT_TCPC_MST)
 		baseboard_mst_enable_control(port, 0);
 #endif
+#ifdef CONFIG_USB_PD_TCPMV2
+	dp_reset_next_command(port);
+#endif
 }
 
 __overridable int svdm_enter_gfu_mode(int port, uint32_t mode_caps)
diff --git a/common/usbc/build.mk b/common/usbc/build.mk
index 3b29d5d9ad..b2e36cf81e 100644
--- a/common/usbc/build.mk
+++ b/common/usbc/build.mk
@@ -28,6 +28,8 @@ ifneq ($(CONFIG_USB_PE_SM),)
 all-obj-$(CONFIG_USB_VPD)+=$(_usbc_dir)usb_pe_ctvpd_sm.o
 all-obj-$(CONFIG_USB_CTVPD)+=$(_usbc_dir)usb_pe_ctvpd_sm.o
 all-obj-$(CONFIG_USB_DRP_ACC_TRYSRC)+=$(_usbc_dir)usb_pe_drp_sm.o
+all-obj-$(CONFIG_USB_DRP_ACC_TRYSRC)+=$(_usbc_dir)usb_pd_dpm.o
+all-obj-$(CONFIG_USB_DRP_ACC_TRYSRC)+=$(_usbc_dir)dp_alt_mode.o
 all-obj-$(CONFIG_CMD_PD)+=$(_usbc_dir)usb_pd_console.o
 endif # CONFIG_USB_PE_SM
 
diff --git a/common/usbc/dp_alt_mode.c b/common/usbc/dp_alt_mode.c
new file mode 100644
index 0000000000..ba0b35550b
--- /dev/null
+++ b/common/usbc/dp_alt_mode.c
@@ -0,0 +1,153 @@
+/* Copyright 2020 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/*
+ * DisplayPort alternate mode support
+ * Refer to VESA DisplayPort Alt Mode on USB Type-C Standard, version 2.0,
+ * section 5.2
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include "assert.h"
+#include "usb_pd.h"
+#include "usb_dp_alt_mode.h"
+#include "usb_pd_dpm.h"
+#include "usb_pd_tcpm.h"
+
+#ifdef CONFIG_COMMON_RUNTIME
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
+#else
+#define CPRINTF(format, args...)
+#define CPRINTS(format, args...)
+#endif
+
+/* The next VDM command to send for DP setup */
+static int next_vdm_cmd[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+void dp_init(int port)
+{
+	dp_reset_next_command(port);
+}
+
+static void print_unexpected_response(int port, enum tcpm_transmit_type type,
+		int vdm_cmd_type, int vdm_cmd)
+{
+	char *cmdt_str;
+
+	switch (vdm_cmd_type) {
+	case CMDT_RSP_ACK:
+		cmdt_str = "ACK";
+		break;
+	case CMDT_RSP_NAK:
+		cmdt_str = "NAK";
+		break;
+	default:
+		assert(false);
+	}
+
+	CPRINTS("C%d: Received unexpected DP VDM %s (cmd %d) from %s", port,
+			cmdt_str, vdm_cmd,
+			type == TCPC_TX_SOP ? "port partner" : "cable plug");
+}
+
+void dp_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
+		uint32_t *vdm)
+{
+	const struct svdm_amode_data *modep =
+		pd_get_amode_data(port, USB_SID_DISPLAYPORT);
+	const uint8_t vdm_cmd = PD_VDO_CMD(vdm[0]);
+
+	if (type != TCPC_TX_SOP || next_vdm_cmd[port] != vdm_cmd) {
+		print_unexpected_response(port, type, CMDT_RSP_ACK, vdm_cmd);
+		return;
+	}
+
+	/* TODO(b/155890173): Validate VDO count for specific commands */
+
+	switch (vdm_cmd) {
+	case CMD_ENTER_MODE:
+		next_vdm_cmd[port] = CMD_DP_STATUS;
+		break;
+	case CMD_DP_STATUS:
+		/* DP status response & UFP's DP attention have same payload. */
+		dfp_consume_attention(port, vdm);
+		next_vdm_cmd[port] = CMD_DP_CONFIG;
+		break;
+	case CMD_DP_CONFIG:
+		if (modep && modep->opos && modep->fx->post_config)
+			modep->fx->post_config(port);
+		dpm_set_mode_entry_done(port);
+		break;
+	default:
+		/* This should never happen */
+		assert(false);
+	}
+}
+
+void dp_vdm_naked(int port, enum tcpm_transmit_type type, uint8_t vdm_cmd)
+{
+	if (type != TCPC_TX_SOP || next_vdm_cmd[port] != vdm_cmd) {
+		print_unexpected_response(port, type, CMDT_RSP_NAK, vdm_cmd);
+		return;
+	}
+
+	dpm_set_mode_entry_done(port);
+}
+
+void dp_reset_next_command(int port)
+{
+	next_vdm_cmd[port] = CMD_ENTER_MODE;
+}
+
+int dp_setup_next_vdm(int port, int vdo_count, uint32_t *vdm)
+{
+	const struct svdm_amode_data *modep =
+				pd_get_amode_data(port, USB_SID_DISPLAYPORT);
+	int vdo_count_ret;
+
+	if (vdo_count < VDO_MAX_SIZE)
+		return -1;
+
+	switch (next_vdm_cmd[port]) {
+	case CMD_ENTER_MODE:
+		/* Enter the first supported mode for DisplayPort. */
+		vdm[0] = pd_dfp_enter_mode(port, USB_SID_DISPLAYPORT, 0);
+		if (vdm[0] == 0)
+			return -1;
+		/* CMDT_INIT is 0, so this is a no-op */
+		vdm[0] |= VDO_CMDT(CMDT_INIT);
+		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPC_TX_SOP));
+		vdo_count_ret = 1;
+		break;
+	case CMD_DP_STATUS:
+		if (!(modep && modep->opos))
+			return -1;
+
+		vdo_count_ret = modep->fx->status(port, vdm);
+		if (vdo_count_ret == 0)
+			return -1;
+		vdm[0] |= PD_VDO_OPOS(modep->opos);
+		vdm[0] |= VDO_CMDT(CMDT_INIT);
+		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPC_TX_SOP));
+		break;
+	case CMD_DP_CONFIG:
+		if (!(modep && modep->opos))
+			return -1;
+
+		vdo_count_ret = modep->fx->config(port, vdm);
+		if (vdo_count_ret == 0)
+			return -1;
+		vdm[0] |= VDO_CMDT(CMDT_INIT);
+		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPC_TX_SOP));
+		break;
+	default:
+		CPRINTF("%s called with invalid next VDM command %d\n",
+				__func__, next_vdm_cmd[port]);
+		return -1;
+	}
+	return vdo_count_ret;
+}
diff --git a/common/usbc/usb_pd_dpm.c b/common/usbc/usb_pd_dpm.c
new file mode 100644
index 0000000000..44ec46e588
--- /dev/null
+++ b/common/usbc/usb_pd_dpm.c
@@ -0,0 +1,110 @@
+/* Copyright 2020 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/*
+ * Device Policy Manager implementation
+ * Refer to USB PD 3.0 spec, version 2.0, sections 8.2 and 8.3
+ */
+
+#include "compile_time_macros.h"
+#include "console.h"
+#include "usb_dp_alt_mode.h"
+#include "usb_pd.h"
+#include "usb_pd_dpm.h"
+#include "usb_pe_sm.h"
+#include "tcpm.h"
+
+#ifdef CONFIG_COMMON_RUNTIME
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
+#else
+#define CPRINTF(format, args...)
+#define CPRINTS(format, args...)
+#endif
+
+static struct {
+	bool mode_entry_done;
+} dpm[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+void dpm_init(int port)
+{
+	dpm[port].mode_entry_done = false;
+}
+
+void dpm_set_mode_entry_done(int port)
+{
+	dpm[port].mode_entry_done = true;
+}
+
+void dpm_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
+		uint32_t *vdm)
+{
+	const uint16_t svid = PD_VDO_VID(vdm[0]);
+
+	assert(vdo_count >= 1);
+
+	switch (svid) {
+	case USB_SID_DISPLAYPORT:
+		dp_vdm_acked(port, type, vdo_count, vdm);
+		break;
+	default:
+		CPRINTS("C%d: Received unexpected VDM ACK for SVID %d", port,
+				svid);
+	}
+}
+
+void dpm_vdm_naked(int port, enum tcpm_transmit_type type, uint16_t svid,
+		uint8_t vdm_cmd)
+{
+	switch (svid) {
+	case USB_SID_DISPLAYPORT:
+		dp_vdm_naked(port, type, vdm_cmd);
+		break;
+	default:
+		CPRINTS("C%d: Received unexpected VDM NAK for SVID %d", port,
+				svid);
+	}
+}
+
+void dpm_attempt_mode_entry(int port)
+{
+	uint32_t vdo_count;
+	uint32_t vdm[VDO_MAX_SIZE];
+
+	if (dpm[port].mode_entry_done)
+		return;
+
+	if (pd_get_data_role(port) != PD_ROLE_DFP)
+		return;
+
+	/*
+	 * Check if we even discovered a DisplayPort mode; if not, just
+	 * mark discovery done and get out of here.
+	 */
+	if (!pd_is_mode_discovered_for_svid(port, TCPC_TX_SOP,
+				USB_SID_DISPLAYPORT)) {
+		CPRINTF("C%d: No DP mode discovered\n", port);
+		dpm_set_mode_entry_done(port);
+		return;
+	}
+
+	vdo_count = dp_setup_next_vdm(port, ARRAY_SIZE(vdm), vdm);
+	if (vdo_count < 0) {
+		dpm_set_mode_entry_done(port);
+		CPRINTF("C%d: Couldn't set up DP VDM\n", port);
+		return;
+	}
+
+	/*
+	 * TODO(b/155890173): Provide a host command to request that the PE send
+	 * an arbitrary VDM via this mechanism.
+	 */
+	if (!pd_setup_vdm_request(port, vdm, vdo_count)) {
+		dpm_set_mode_entry_done(port);
+		return;
+	}
+
+	pe_dpm_request(port, DPM_REQUEST_VDM);
+}
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index 7a1e63014d..cbbe4c60f1 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -17,6 +17,8 @@
 #include "tcpm.h"
 #include "util.h"
 #include "usb_common.h"
+#include "usb_dp_alt_mode.h"
+#include "usb_pd_dpm.h"
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 #include "usb_pe_sm.h"
@@ -1137,6 +1139,10 @@ static bool common_src_snk_dpm_requests(int port)
 					CMD_EXIT_MODE);
 		pe[port].vdm_cnt = 1;
 
+	} else if (PE_CHK_DPM_REQUEST(port, DPM_REQUEST_VDM)) {
+		PE_CLR_DPM_REQUEST(port, DPM_REQUEST_VDM);
+
+		/* Send previously set up SVDM. */
 		set_state_pe(port, PE_VDM_REQUEST);
 		return true;
 	}
@@ -1378,15 +1384,6 @@ static bool pe_attempt_port_discovery(int port)
 			pe[port].tx_type = TCPC_TX_SOP_PRIME;
 			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.
-		 * Remove once do_port_discovery can be removed.
-		 */
-		} else if (pe_can_send_sop_vdm(port, pe[port].vdm_cmd + 1)) {
-			PE_SET_FLAG(port, PE_FLAGS_LOCALLY_INITIATED_AMS);
-			set_state_pe(port, PE_DO_PORT_DISCOVERY);
-			return true;
 		}
 	}
 
@@ -1394,6 +1391,19 @@ static bool pe_attempt_port_discovery(int port)
 }
 #endif
 
+bool pd_setup_vdm_request(int port, uint32_t *vdm, uint32_t vdo_cnt)
+{
+	if (vdo_cnt < VDO_HDR_SIZE || vdo_cnt > VDO_MAX_SIZE)
+		return false;
+
+	/* TODO(b/155890173): Support cable plug */
+	pe[port].partner_type = PORT;
+	memcpy(pe[port].vdm_data, vdm, vdo_cnt * sizeof(*vdm));
+	pe[port].vdm_cnt = vdo_cnt;
+
+	return true;
+}
+
 int pd_dev_store_rw_hash(int port, uint16_t dev_id, uint32_t *rw_hash,
 					uint32_t current_image)
 {
@@ -2063,6 +2073,9 @@ static void pe_src_ready_run(int port)
 
 			return;
 		}
+
+		/* No DPM requests; attempt mode entry if needed */
+		dpm_attempt_mode_entry(port);
 	}
 }
 
@@ -2834,6 +2847,9 @@ static void pe_snk_ready_run(int port)
 
 			return;
 		}
+
+		/* No DPM requests; attempt mode entry if needed */
+		dpm_attempt_mode_entry(port);
 	}
 }
 
@@ -4867,6 +4883,7 @@ static void pe_vdm_request_entry(int port)
 		tx_emsg[port].len = pe[port].vdm_cnt * 4;
 	}
 
+	/* TODO(b/155890173): Support cable plug */
 	prl_send_data_msg(port, TCPC_TX_SOP, PD_DATA_VENDOR_DEF);
 
 	pe[port].vdm_response_timer = TIMER_DISABLED;
@@ -4880,6 +4897,7 @@ static void pe_vdm_request_run(int port)
 		PE_CLR_FLAG(port, PE_FLAGS_TX_COMPLETE);
 
 		/* Start no response timer */
+		/* TODO(b/155890173): Support DPM-supplied timeout */
 		pe[port].vdm_response_timer =
 			get_time().val + PD_T_VDM_SNDR_RSP;
 	}
@@ -4918,11 +4936,10 @@ static void pe_vdm_request_run(int port)
 			}
 		} else {
 			if ((sop == TCPC_TX_SOP || sop == TCPC_TX_SOP_PRIME) &&
-				type == PD_CTRL_NOT_SUPPORTED && cnt == 0 &&
-				ext == 0) {
-				/* Do not continue port discovery */
-				PE_SET_FLAG(port,
-					PE_FLAGS_DISCOVER_PORT_IDENTITY_DONE);
+					type == PD_CTRL_NOT_SUPPORTED &&
+					cnt == 0 && ext == 0) {
+				/* Equivalent meaning to a NAK */
+				PE_SET_FLAG(port, PE_FLAGS_VDM_REQUEST_NAKED);
 			} else {
 				/* Unexpected Message Received. */
 
@@ -4960,15 +4977,24 @@ static void pe_vdm_request_run(int port)
 		PE_SET_FLAG(port, PE_FLAGS_VDM_REQUEST_NAKED);
 	}
 
+	/*
+	 * Because Not Supported messages or response timeouts are treated as
+	 * NAKs, there may not be a NAK message to parse. Extract the needed
+	 * information from the sent VDM.
+	 */
+	if (PE_CHK_FLAG(port, PE_FLAGS_VDM_REQUEST_NAKED)) {
+		PE_SET_FLAG(port, PE_FLAGS_DISCOVER_PORT_IDENTITY_DONE);
+
+		dpm_vdm_naked(port, pe[port].tx_type,
+				PD_VDO_VID(pe[port].vdm_data[0]),
+				PD_VDO_CMD(pe[port].vdm_data[0]));
+	}
+
+
 	if (PE_CHK_FLAG(port, PE_FLAGS_VDM_REQUEST_NAKED |
 					PE_FLAGS_VDM_REQUEST_BUSY)) {
-		/* Return to previous state */
-		if (get_last_state_pe(port) == PE_DO_PORT_DISCOVERY)
-			set_state_pe(port, PE_DO_PORT_DISCOVERY);
-		else if (pe[port].power_role == PD_ROLE_SOURCE)
-			set_state_pe(port, PE_SRC_READY);
-		else
-			set_state_pe(port, PE_SNK_READY);
+		/* Return to previous Ready state */
+		set_state_pe(port, get_last_state_pe(port));
 	}
 }
 
@@ -4982,70 +5008,37 @@ static void pe_vdm_request_exit(int port)
  */
 static void pe_vdm_acked_entry(int port)
 {
+	int sop;
+	int vdo_count;
 	uint32_t *payload;
+	uint16_t svid;
 	uint8_t vdo_cmd;
-	int sop;
 
 	print_current_state(port);
 
 	/* Get the message */
+	sop = PD_HEADER_GET_SOP(rx_emsg[port].header);
+	vdo_count = PD_HEADER_CNT(rx_emsg[port].header);
 	payload = (uint32_t *)rx_emsg[port].buf;
+	svid = PD_VDO_VID(payload[0]);
 	vdo_cmd = PD_VDO_CMD(payload[0]);
 	sop = PD_HEADER_GET_SOP(rx_emsg[port].header);
 
+	/* TODO(b/155890173): Support cable plug */
 	if (sop == TCPC_TX_SOP) {
 		/*
 		 * Handle Message From Port Partner
 		 */
 
-#ifdef CONFIG_USB_PD_ALT_MODE_DFP
-		int cnt = PD_HEADER_CNT(rx_emsg[port].header);
-		struct svdm_amode_data *modep;
+		/* vdo_count must have been >= 1 to get into this state. */
+		dpm_vdm_acked(port, sop, vdo_count, payload);
 
-		modep = pd_get_amode_data(port, PD_VDO_VID(payload[0]));
-#endif
-
-		switch (vdo_cmd) {
-#ifdef CONFIG_USB_PD_ALT_MODE_DFP
-		case CMD_DISCOVER_IDENT:
-			dfp_consume_identity(port, cnt, payload);
-#ifdef CONFIG_CHARGE_MANAGER
-			if (pd_charge_from_device(pd_get_identity_vid(port),
-						pd_get_identity_pid(port))) {
-				charge_manager_update_dualrole(port,
-								CAP_DEDICATED);
-			}
-#endif
-			break;
-		case CMD_DISCOVER_SVID:
-			dfp_consume_svids(port, TCPC_TX_SOP, cnt, payload);
-			break;
-		case CMD_DISCOVER_MODES:
-			dfp_consume_modes(port, TCPC_TX_SOP, cnt, payload);
-			break;
-		case CMD_ENTER_MODE:
-			break;
-		case CMD_DP_STATUS:
-			/*
-			 * DP status response & UFP's DP attention have same
-			 * payload
-			 */
-			dfp_consume_attention(port, payload);
-			break;
-		case CMD_DP_CONFIG:
-			if (modep && modep->opos && modep->fx->post_config)
-				modep->fx->post_config(port);
-			break;
-		case CMD_EXIT_MODE:
-			/* Do nothing */
-			break;
-#endif
-		case CMD_ATTENTION:
-			/* Do nothing */
-			break;
-		default:
-			CPRINTF("ERR:CMD:%d\n", vdo_cmd);
-		}
+		/*
+		 * TODO(b/155890173): Respect distinction between discovery and
+		 * mode entry in flags.
+		 */
+		if (svid == USB_SID_DISPLAYPORT && vdo_cmd == CMD_DP_CONFIG)
+			PE_SET_FLAG(port, PE_FLAGS_DISCOVER_PORT_IDENTITY_DONE);
 	}
 
 	if (pe[port].power_role == PD_ROLE_SOURCE) {
@@ -5676,6 +5669,10 @@ uint8_t pd_get_src_cap_cnt(int port)
 void pd_dfp_discovery_init(int port)
 {
 	memset(&pe[port].discovery, 0, sizeof(pe[port].discovery));
+
+	/* Reset the DPM and DP modules to enable alternate mode entry. */
+	dpm_init(port);
+	dp_init(port);
 }
 
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP