TCPMv2: Separate DPM and DP from PE

Create separate modules representing DisplayPort (which decides whether
and how we can set up the DP alt mode) and the Device Policy Manager
(which decides whether we should). The end goal is to have essentially
all code with DP-specific decisions or information in the DP module and
all code with DPM-specific decisions or information in the DPM module.
This will allow the PE to map as directly as practical to its behaviors
defined in the PD spec.

Essentially detach PE_DO_PORT_DISCOVERY from the rest of the PE state
machine in preparation for deleting it.

This change paves the way for
1) Allowing the AP to drive mode entry and
2) Allowing new alt modes to be supported via largely self-contained
   modules.

BUG=b:155890173
TEST=Attach DP dongle; observe discovery and DP setup via Twinkie
BRANCH=none

Change-Id: Ie63a2e62d1ac6178722477dc53244898a04ef92f
Signed-off-by: Abe Levkoy <alevkoy@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2203842
Reviewed-by: Diana Z <dzigterman@chromium.org>

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