1 files changed, 172 insertions, 54 deletions

diff --git a/common/usbc/dp_alt_mode.c b/common/usbc/dp_alt_mode.c
index 33ee3f9729..4888383c9d 100644
--- a/common/usbc/dp_alt_mode.c
+++ b/common/usbc/dp_alt_mode.c
@@ -25,33 +25,76 @@
 #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];
+/* The state of the DP negotiation */
+enum dp_states {
+	DP_START = 0,
+	DP_ENTER_SENT,
+	DP_ENTER_ACKED,
+	DP_ENTER_NAKED,
+	DP_STATUS_SENT,
+	DP_STATUS_ACKED,
+	DP_CONFIG_SENT,
+	DP_ACTIVE,
+	DP_EXIT_SENT,
+	DP_EXIT_RETRY_SENT,
+	DP_ENTER_RETRY,
+	DP_ENTER_RETRY_SENT,
+	DP_INACTIVE,
+	DP_STATE_COUNT
+};
+static enum dp_states dp_state[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+/*
+ * Map of states to expected VDM commands in responses.
+ * Default of 0 indicates no command expected.
+ */
+static const uint8_t state_vdm_cmd[DP_STATE_COUNT] = {
+	[DP_ENTER_SENT] = CMD_ENTER_MODE,
+	[DP_STATUS_SENT] = CMD_DP_STATUS,
+	[DP_CONFIG_SENT] = CMD_DP_CONFIG,
+	[DP_ACTIVE] = CMD_EXIT_MODE,
+	[DP_EXIT_SENT] = CMD_EXIT_MODE,
+	[DP_EXIT_RETRY_SENT] = CMD_EXIT_MODE,
+	[DP_ENTER_RETRY_SENT] = CMD_ENTER_MODE,
+};
 
 void dp_init(int port)
 {
-	dp_reset_next_command(port);
+	dp_state[port] = DP_START;
 }
 
-static void print_unexpected_response(int port, enum tcpm_transmit_type type,
-		int vdm_cmd_type, int vdm_cmd)
+void dp_teardown(int port)
 {
-	char *cmdt_str;
+	CPRINTS("C%d: DP teardown", port);
+	dp_state[port] = DP_INACTIVE;
+}
 
-	switch (vdm_cmd_type) {
-	case CMDT_RSP_ACK:
-		cmdt_str = "ACK";
-		break;
-	case CMDT_RSP_NAK:
-		cmdt_str = "NAK";
-		break;
-	default:
-		assert(false);
-	}
+static void dp_entry_failed(int port)
+{
+	CPRINTS("C%d: DP alt mode protocol failed!", port);
+	dp_state[port] = DP_INACTIVE;
+	dpm_set_mode_entry_done(port);
+}
+
+static bool dp_response_valid(int port, enum tcpm_transmit_type type,
+			     char *cmdt, int vdm_cmd)
+{
+	enum dp_states st = dp_state[port];
+
+	/*
+	 * Check for an unexpected response.
+	 * If DP is inactive, ignore the command.
+	 */
+	if (type != TCPC_TX_SOP ||
+	    (st != DP_INACTIVE && state_vdm_cmd[st] != vdm_cmd)) {
 
-	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");
+		CPRINTS("C%d: Received unexpected DP VDM %s (cmd %d) from"
+			" %s in state %d", port, cmdt, vdm_cmd,
+		type == TCPC_TX_SOP ? "port partner" : "cable plug", st);
+		dp_entry_failed(port);
+		return false;
+	}
+	return true;
 }
 
 void dp_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
@@ -61,55 +104,90 @@ void dp_vdm_acked(int port, enum tcpm_transmit_type type, int vdo_count,
 		pd_get_amode_data(port, type, USB_SID_DISPLAYPORT);
 	const uint8_t vdm_cmd = PD_VDO_CMD(vdm[0]);
 
-	/*
-	 * Handle the ACK of a request to exit alt mode.
-	 */
-	if (type == TCPC_TX_SOP && vdm_cmd == CMD_EXIT_MODE) {
-		pd_dfp_discovery_init(port);
-		return;
-	}
-
-	if (type != TCPC_TX_SOP || next_vdm_cmd[port] != vdm_cmd) {
-		print_unexpected_response(port, type, CMDT_RSP_ACK, vdm_cmd);
-		dpm_set_mode_entry_done(port);
+	if (!dp_response_valid(port, type, "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;
+	switch (dp_state[port]) {
+	case DP_ENTER_SENT:
+	case DP_ENTER_RETRY_SENT:
+		dp_state[port] = DP_ENTER_ACKED;
 		break;
-	case CMD_DP_STATUS:
+	case DP_STATUS_SENT:
 		/* DP status response & UFP's DP attention have same payload. */
 		dfp_consume_attention(port, vdm);
-		next_vdm_cmd[port] = CMD_DP_CONFIG;
+		dp_state[port] = DP_STATUS_ACKED;
 		break;
-	case CMD_DP_CONFIG:
+	case DP_CONFIG_SENT:
 		if (modep && modep->opos && modep->fx->post_config)
 			modep->fx->post_config(port);
 		dpm_set_mode_entry_done(port);
+		dp_state[port] = DP_ACTIVE;
+		CPRINTS("C%d: Entered DP mode", port);
+		break;
+	case DP_ACTIVE:
+	case DP_EXIT_SENT:
+		/*
+		 * Request to exit mode successful, so put it in
+		 * inactive state.
+		 */
+		CPRINTS("C%d: Exited DP mode", port);
+		dp_state[port] = DP_INACTIVE;
+		break;
+	case DP_EXIT_RETRY_SENT:
+		/*
+		 * The request to exit the mode was successful,
+		 * so try to enter the mode again.
+		 */
+		dp_state[port] = DP_ENTER_RETRY;
+		break;
+	case DP_INACTIVE:
+		/*
+		 * This can occur if the mode is shutdown because
+		 * the CPU is being turned off, and an exit mode
+		 * command has been sent.
+		 */
 		break;
 	default:
-		/* This should never happen */
-		assert(false);
+		/* Invalid or unexpected negotiation state */
+		CPRINTF("%s called with invalid state %d\n",
+				__func__, dp_state[port]);
+		dp_entry_failed(port);
+		break;
 	}
 }
 
 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);
+	if (!dp_response_valid(port, type, "NAK", vdm_cmd))
 		return;
-	}
-
-	dpm_set_mode_entry_done(port);
-}
 
-void dp_reset_next_command(int port)
-{
-	next_vdm_cmd[port] = CMD_ENTER_MODE;
+	switch (dp_state[port]) {
+	case DP_ENTER_SENT:
+		/*
+		 * If a request to enter DP mode is NAK'ed, this likely
+		 * means the partner is already in DP alt mode, so
+		 * request to exit the mode first before retrying
+		 * the enter command. This can happen if the EC
+		 * is restarted (e.g to go into recovery mode) while
+		 * DP alt mode is active.
+		 */
+		dp_state[port] = DP_ENTER_NAKED;
+		break;
+	case DP_ENTER_RETRY_SENT:
+		/*
+		 * Another NAK on the second attempt to enter DP mode.
+		 * Give up.
+		 */
+		dp_entry_failed(port);
+		break;
+	default:
+		CPRINTS("C%d: NAK for cmd %d in state %d", port,
+			vdm_cmd, dp_state[port]);
+		dp_entry_failed(port);
+		break;
+	}
 }
 
 int dp_setup_next_vdm(int port, int vdo_count, uint32_t *vdm)
@@ -121,8 +199,9 @@ int dp_setup_next_vdm(int port, int vdo_count, uint32_t *vdm)
 	if (vdo_count < VDO_MAX_SIZE)
 		return -1;
 
-	switch (next_vdm_cmd[port]) {
-	case CMD_ENTER_MODE:
+	switch (dp_state[port]) {
+	case DP_START:
+	case DP_ENTER_RETRY:
 		/* Enter the first supported mode for DisplayPort. */
 		vdm[0] = pd_dfp_enter_mode(port, TCPC_TX_SOP,
 				USB_SID_DISPLAYPORT, 0);
@@ -132,8 +211,13 @@ int dp_setup_next_vdm(int port, int vdo_count, uint32_t *vdm)
 		vdm[0] |= VDO_CMDT(CMDT_INIT);
 		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPC_TX_SOP));
 		vdo_count_ret = 1;
+		if (dp_state[port] == DP_START) {
+			CPRINTS("C%d: Attempting to enter DP mode", port);
+			dp_state[port] = DP_ENTER_SENT;
+		} else
+			dp_state[port] = DP_ENTER_RETRY_SENT;
 		break;
-	case CMD_DP_STATUS:
+	case DP_ENTER_ACKED:
 		if (!(modep && modep->opos))
 			return -1;
 
@@ -143,8 +227,9 @@ int dp_setup_next_vdm(int port, int vdo_count, uint32_t *vdm)
 		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));
+		dp_state[port] = DP_STATUS_SENT;
 		break;
-	case CMD_DP_CONFIG:
+	case DP_STATUS_ACKED:
 		if (!(modep && modep->opos))
 			return -1;
 
@@ -153,10 +238,43 @@ int dp_setup_next_vdm(int port, int vdo_count, uint32_t *vdm)
 			return -1;
 		vdm[0] |= VDO_CMDT(CMDT_INIT);
 		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPC_TX_SOP));
+		dp_state[port] = DP_CONFIG_SENT;
 		break;
+	case DP_ENTER_NAKED:
+	case DP_ACTIVE:
+		/*
+		 * Called to exit DP alt mode, either when the mode
+		 * is active and the system is shutting down, or
+		 * when an initial request to enter the mode is NAK'ed.
+		 * This can happen if the EC is restarted (e.g to go
+		 * into recovery mode) while DP alt mode is active.
+		 * It would be good to invoke modep->fx->exit but
+		 * this doesn't set up the VDM, it clears state.
+		 * TODO: Clean up the API to the fx functions.
+		 */
+		if (!(modep && modep->opos))
+			return -1;
+
+		vdm[0] = VDO(USB_SID_DISPLAYPORT,
+			     1, /* structured */
+			     CMD_EXIT_MODE);
+
+		vdm[0] |= VDO_OPOS(modep->opos);
+		vdm[0] |= VDO_CMDT(CMDT_INIT);
+		vdm[0] |= VDO_SVDM_VERS(pd_get_vdo_ver(port, TCPC_TX_SOP));
+		vdo_count_ret = 1;
+		dp_state[port] = (dp_state[port] == DP_ACTIVE)
+				  ? DP_EXIT_SENT
+				  : DP_EXIT_RETRY_SENT;
+		break;
+	case DP_INACTIVE:
+		/*
+		 * DP mode is inactive.
+		 */
+		return -1;
 	default:
-		CPRINTF("%s called with invalid next VDM command %d\n",
-				__func__, next_vdm_cmd[port]);
+		CPRINTF("%s called with invalid state %d\n",
+				__func__, dp_state[port]);
 		return -1;
 	}
 	return vdo_count_ret;