pd: Allow multiple mode entry.

Current simplified implementation allows single mode entry.  Specification
allows multiple mode entry and its advantageous for things like flashing RW
while staying in DisplayPort mode on video dongles.

CL adds capability on DFP to track as many alternate modes as supported by the
DFP.  Initial mode entered is still the default supported mode ( 1st entry, 1st
opos).  Policy manager can then use host command, EC_CMD_USB_PD_SET_AMODE, to
enter additional supported modes.

On the UFP (hoho, dingdong) a small modification to track multiple svid mode
entries was made.

Signed-off-by: Todd Broch <tbroch@chromium.org>

BRANCH=samus
BUG=chrome-os-partner:33946
TEST=manual, On hoho
1. Still successfully enter default mode DP
2. Using ectool's pdsetmode can successfully enter/exit multiple modes.
   For example,
   # port:1 svid:18d1 opos:1 cmd:1==enter
   ectool --name cros_pd pdsetmode 1 0x18d1 1 1

   Checking with pdgetmode shows both modes entered.

3. Works across hard & soft resets
4. Can flash via ectool --name cros_pd flashpd 4 <port> <RW image>
5. Still drives external display.  With bootarg drm.debug=0x6 and following
   command: 'tail -f /var/log/messages | grep "Received HPD" &'
   I see HPD assert & deassert when switching between GFU and DP mode.
   If both modes entered screen stays lit (after reboot) during write.

Original-Change-Id: I7a21ebea377402eb1b0a0cf1d29df59694e301b1
Signed-off-by: Duncan Laurie <dlaurie@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/243466
(cherry picked from commit 4ed51e567c974f47ae39b90ac978e88ca843764c)
Signed-off-by: Todd Broch <tbroch@chromium.org>

Change-Id: I65867b9c754ce8df645562629b94720894e74fdd
Reviewed-on: https://chromium-review.googlesource.com/244232
Reviewed-by: Alec Berg <alecaberg@chromium.org>
Tested-by: Todd Broch <tbroch@chromium.org>
Commit-Queue: Todd Broch <tbroch@chromium.org>

8 files changed, 254 insertions, 140 deletions

diff --git a/board/dingdong/usb_pd_policy.c b/board/dingdong/usb_pd_policy.c
index 520732cd26..5997028dc6 100644
--- a/board/dingdong/usb_pd_policy.c
+++ b/board/dingdong/usb_pd_policy.c
@@ -33,10 +33,8 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Whether alternate mode has been entered or not */
-static int alt_mode;
-/* When set true, we are in GFU mode */
-static int gfu_mode;
+/* Holds valid object position (opos) for entered mode */
+static int alt_mode[PD_AMODE_COUNT];
 
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
@@ -189,32 +187,36 @@ static int svdm_enter_mode(int port, uint32_t *payload)
 	/* SID & mode request is valid */
 	if ((PD_VDO_VID(payload[0]) == USB_SID_DISPLAYPORT) &&
 	    (PD_VDO_OPOS(payload[0]) == OPOS_DP)) {
-		alt_mode = OPOS_DP;
+		alt_mode[PD_AMODE_DISPLAYPORT] = OPOS_DP;
 		rv = 1;
 	} else if ((PD_VDO_VID(payload[0]) == USB_VID_GOOGLE) &&
 		   (PD_VDO_OPOS(payload[0]) == OPOS_GFU)) {
-		alt_mode = OPOS_GFU;
-		gfu_mode = 1;
+		alt_mode[PD_AMODE_GOOGLE] = OPOS_GFU;
 		rv = 1;
 	}
 	/* TODO(p/33968): Enumerate USB BB here with updated mode choice */
 	return rv;
 }
 
-int pd_alt_mode(int port)
+int pd_alt_mode(int port, uint16_t svid)
 {
-	return alt_mode;
+	if (svid == USB_SID_DISPLAYPORT)
+		return alt_mode[PD_AMODE_DISPLAYPORT];
+	else if (svid == USB_VID_GOOGLE)
+		return alt_mode[PD_AMODE_GOOGLE];
+	return 0;
 }
 
 static int svdm_exit_mode(int port, uint32_t *payload)
 {
-	alt_mode = 0;
-	if (PD_VDO_VID(payload[0]) == USB_SID_DISPLAYPORT)
+	if (PD_VDO_VID(payload[0]) == USB_SID_DISPLAYPORT) {
 		gpio_set_level(GPIO_PD_SBU_ENABLE, 0);
-	else if (PD_VDO_VID(payload[0]) == USB_VID_GOOGLE)
-		gfu_mode = 0;
-	else
+		alt_mode[PD_AMODE_DISPLAYPORT] = 0;
+	} else if (PD_VDO_VID(payload[0]) == USB_VID_GOOGLE) {
+		alt_mode[PD_AMODE_GOOGLE] = 0;
+	} else {
 		CPRINTF("Unknown exit mode req:0x%08x\n", payload[0]);
+	}
 
 	return 1; /* Must return ACK */
 }
@@ -238,7 +240,8 @@ int pd_custom_vdm(int port, int cnt, uint32_t *payload,
 {
 	int rsize;
 
-	if (PD_VDO_VID(payload[0]) != USB_VID_GOOGLE || !gfu_mode)
+	if (PD_VDO_VID(payload[0]) != USB_VID_GOOGLE ||
+	    !alt_mode[PD_AMODE_GOOGLE])
 		return 0;
 
 	*rpayload = payload;
diff --git a/board/hoho/usb_pd_policy.c b/board/hoho/usb_pd_policy.c
index efd1bbc243..3f8f23013e 100644
--- a/board/hoho/usb_pd_policy.c
+++ b/board/hoho/usb_pd_policy.c
@@ -33,10 +33,8 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Whether alternate mode has been entered or not */
-static int alt_mode;
-/* When set true, we are in GFU mode */
-static int gfu_mode;
+/* Holds valid object position (opos) for entered mode */
+static int alt_mode[PD_AMODE_COUNT];
 
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
@@ -188,32 +186,36 @@ static int svdm_enter_mode(int port, uint32_t *payload)
 	/* SID & mode request is valid */
 	if ((PD_VDO_VID(payload[0]) == USB_SID_DISPLAYPORT) &&
 	    (PD_VDO_OPOS(payload[0]) == OPOS_DP)) {
-		alt_mode = OPOS_DP;
+		alt_mode[PD_AMODE_DISPLAYPORT] = OPOS_DP;
 		rv = 1;
 	} else if ((PD_VDO_VID(payload[0]) == USB_VID_GOOGLE) &&
 		   (PD_VDO_OPOS(payload[0]) == OPOS_GFU)) {
-		alt_mode = OPOS_GFU;
-		gfu_mode = 1;
+		alt_mode[PD_AMODE_GOOGLE] = OPOS_GFU;
 		rv = 1;
 	}
 	/* TODO(p/33968): Enumerate USB BB here with updated mode choice */
 	return rv;
 }
 
-int pd_alt_mode(int port)
+int pd_alt_mode(int port, uint16_t svid)
 {
-	return alt_mode;
+	if (svid == USB_SID_DISPLAYPORT)
+		return alt_mode[PD_AMODE_DISPLAYPORT];
+	else if (svid == USB_VID_GOOGLE)
+		return alt_mode[PD_AMODE_GOOGLE];
+	return 0;
 }
 
 static int svdm_exit_mode(int port, uint32_t *payload)
 {
-	alt_mode = 0;
-	if (PD_VDO_VID(payload[0]) == USB_SID_DISPLAYPORT)
+	if (PD_VDO_VID(payload[0]) == USB_SID_DISPLAYPORT) {
 		gpio_set_level(GPIO_PD_SBU_ENABLE, 0);
-	else if (PD_VDO_VID(payload[0]) == USB_VID_GOOGLE)
-		gfu_mode = 0;
-	else
+		alt_mode[PD_AMODE_DISPLAYPORT] = 0;
+	} else if (PD_VDO_VID(payload[0]) == USB_VID_GOOGLE) {
+		alt_mode[PD_AMODE_GOOGLE] = 0;
+	} else {
 		CPRINTF("Unknown exit mode req:0x%08x\n", payload[0]);
+	}
 
 	return 1; /* Must return ACK */
 }
@@ -237,7 +239,8 @@ int pd_custom_vdm(int port, int cnt, uint32_t *payload,
 {
 	int rsize;
 
-	if (PD_VDO_VID(payload[0]) != USB_VID_GOOGLE || !gfu_mode)
+	if (PD_VDO_VID(payload[0]) != USB_VID_GOOGLE ||
+	    !alt_mode[PD_AMODE_GOOGLE])
 		return 0;
 
 	*rpayload = payload;
diff --git a/board/samus_pd/usb_pd_policy.c b/board/samus_pd/usb_pd_policy.c
index ca16c5759d..f11c7cf984 100644
--- a/board/samus_pd/usb_pd_policy.c
+++ b/board/samus_pd/usb_pd_policy.c
@@ -238,8 +238,9 @@ static int svdm_enter_dp_mode(int port, uint32_t mode_caps)
 
 static int svdm_dp_status(int port, uint32_t *payload)
 {
+	int opos = pd_alt_mode(port, USB_SID_DISPLAYPORT);
 	payload[0] = VDO(USB_SID_DISPLAYPORT, 1,
-			 CMD_DP_STATUS | VDO_OPOS(pd_alt_mode(port)));
+			 CMD_DP_STATUS | VDO_OPOS(opos));
 	payload[1] = VDO_DP_STATUS(0, /* HPD IRQ  ... not applicable */
 				   0, /* HPD level ... not applicable */
 				   0, /* exit DP? ... no */
@@ -253,9 +254,10 @@ 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);
 	board_set_usb_mux(port, TYPEC_MUX_DP, pd_get_polarity(port));
 	payload[0] = VDO(USB_SID_DISPLAYPORT, 1,
-			 CMD_DP_CONFIG | VDO_OPOS(pd_alt_mode(port)));
+			 CMD_DP_CONFIG | VDO_OPOS(opos));
 	payload[1] = VDO_DP_CFG(MODE_DP_PIN_E, /* sink pins */
 				MODE_DP_PIN_E, /* src pins */
 				1,             /* DPv1.3 signaling */
diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index 8a02383936..b6e679f997 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -248,46 +248,97 @@ static void dfp_consume_modes(int port, int cnt, uint32_t *payload)
 	pe[port].svid_idx++;
 }
 
-static struct svdm_amode_data *get_modep(int port)
+static int get_mode_idx(int port, uint16_t svid)
 {
-	return &pe[port].amode;
+	int i;
+
+	for (i = 0; i < PD_AMODE_COUNT; i++) {
+		if (pe[port].amodes[i].fx->svid == svid)
+			return i;
+	}
+	return -1;
 }
 
-int pd_alt_mode(int port)
+static struct svdm_amode_data *get_modep(int port, uint16_t svid)
 {
-	return get_modep(port)->opos;
+	int idx = get_mode_idx(port, svid);
+
+	return (idx == -1) ? NULL : &pe[port].amodes[idx];
 }
 
-/* Enter default mode or attempt to enter mode via svid & index arguments */
-static int dfp_enter_mode(int port, uint32_t *payload, int use_payload)
+int pd_alt_mode(int port, uint16_t svid)
 {
-	int i, j, done;
-	struct svdm_amode_data *modep = get_modep(port);
-	uint16_t svid = (use_payload) ? PD_VDO_VID(payload[0]) : 0;
-	uint8_t opos = (use_payload) ? PD_VDO_OPOS(payload[0]) : 0;
+	struct svdm_amode_data *modep = get_modep(port, svid);
+
+	return (modep) ? modep->opos : -1;
+}
+
+int allocate_mode(int port, uint16_t svid)
+{
+	int i, j;
+	struct svdm_amode_data *modep;
+	int mode_idx = get_mode_idx(port, svid);
+
+	if (mode_idx != -1)
+		return mode_idx;
+
+	/* There's no space to enter another mode */
+	if (pe[port].amode_idx == PD_AMODE_COUNT) {
+		CPRINTF("ERR:NO AMODE SPACE\n");
+		return -1;
+	}
+
+	/* Allocate ...  if SVID == 0 enter default supported policy */
+	for (i = 0; i < supported_modes_cnt; i++) {
+		if (!&supported_modes[i])
+			continue;
 
-	for (i = 0, done = 0; !done && (i < supported_modes_cnt); i++) {
 		for (j = 0; j < pe[port].svid_cnt; j++) {
 			struct svdm_svid_data *svidp = &pe[port].svids[j];
 			if ((svidp->svid != supported_modes[i].svid) ||
 			    (svid && (svidp->svid != svid)))
 				continue;
+
+			modep = &pe[port].amodes[pe[port].amode_idx];
 			modep->fx = &supported_modes[i];
-			modep->mode_caps = pe[port].svids[j].mode_vdo[0];
-			modep->opos = (opos && (opos < 7)) ? opos : 1;
-			done = 1;
-			break;
+			modep->data = &pe[port].svids[j];
+			pe[port].amode_idx++;
+			return pe[port].amode_idx - 1;
 		}
 	}
-	if (!modep->opos)
+	return -1;
+}
+
+/*
+ * Enter default mode ( payload[0] == 0 ) or attempt to enter mode via svid &
+ * opos
+*/
+uint32_t pd_dfp_enter_mode(int port, uint16_t svid, int opos)
+{
+	int mode_idx = allocate_mode(port, svid);
+	struct svdm_amode_data *modep;
+	uint32_t mode_caps;
+
+	if (mode_idx == -1)
+		return 0;
+	modep = &pe[port].amodes[mode_idx];
+
+	if (!opos) {
+		/* choose the lowest as default */
+		modep->opos = 1;
+	} else if (opos <= modep->data->mode_cnt) {
+		modep->opos = opos;
+	} else {
+		CPRINTF("opos error\n");
 		return 0;
+	}
 
-	if (modep->fx->enter(port, modep->mode_caps) == -1)
+	mode_caps = modep->data->mode_vdo[modep->opos - 1];
+	if (modep->fx->enter(port, mode_caps) == -1)
 		return 0;
 
-	payload[0] = VDO(modep->fx->svid, 1,
-			 CMD_ENTER_MODE | VDO_OPOS(pd_alt_mode(port)));
-	return 1;
+	/* SVDM to send to UFP for mode entry */
+	return VDO(modep->fx->svid, 1, CMD_ENTER_MODE | VDO_OPOS(modep->opos));
 }
 
 static int validate_mode_request(struct svdm_amode_data *modep,
@@ -313,9 +364,9 @@ static int validate_mode_request(struct svdm_amode_data *modep,
 
 static void dfp_consume_attention(int port, uint32_t *payload)
 {
-	struct svdm_amode_data *modep = get_modep(port);
 	uint16_t svid = PD_VDO_VID(payload[0]);
 	int opos = PD_VDO_OPOS(payload[0]);
+	struct svdm_amode_data *modep = get_modep(port, svid);
 
 	if (!validate_mode_request(modep, svid, opos))
 		return;
@@ -324,28 +375,40 @@ static void dfp_consume_attention(int port, uint32_t *payload)
 		modep->fx->attention(port, payload);
 }
 
-uint32_t pd_dfp_exit_mode(int port)
+int pd_dfp_exit_mode(int port, uint16_t svid, int opos)
 {
-	struct svdm_amode_data *modep = get_modep(port);
+	struct svdm_amode_data *modep;
+	int idx;
 
-	if (!modep->fx)
-		return 0;
+	/*
+	 * Empty svid signals we should reset DFP VDM state by exiting all
+	 * entered modes then clearing state.  This occurs when we've
+	 * disconnected or for hard reset.
+	 */
+	if (!svid) {
+		for (idx = 0; idx < PD_AMODE_COUNT; idx++)
+			if (pe[port].amodes[idx].fx)
+				pe[port].amodes[idx].fx->exit(port);
 
-	modep->fx->exit(port);
+		pd_dfp_pe_init(port);
+		return 0;
+	}
 
 	/*
 	 * TODO(crosbug.com/p/33946) : below needs revisited to allow multiple
-	 * mode entry.  Additionally it should honor OPOS == 7 as DFP's request
-	 * to exit all modes.
+	 * mode exit.  Additionally it should honor OPOS == 7 as DFP's request
+	 * to exit all modes.  We currently don't have any UFPs that support
+	 * multiple modes on one SVID.
 	 */
-	if (pd_is_connected(port)) {
-		int cur_opos = modep->opos;
-		modep->opos = 0;
-		return VDO(modep->fx->svid, 1, (CMD_EXIT_MODE | cur_opos));
-	} else {
-		pd_dfp_pe_init(port);
-	}
-	return 0;
+	modep = get_modep(port, svid);
+	if (!validate_mode_request(modep, svid, opos))
+		return 0;
+
+	/* call DFPs exit function */
+	modep->fx->exit(port);
+	/* exit the mode */
+	modep->opos = 0;
+	return 1;
 }
 
 #ifdef CONFIG_CMD_USB_PD_PE
@@ -357,6 +420,7 @@ static void dump_pe(int port)
 
 	int i, j, idh_ptype;
 	struct svdm_amode_data *modep;
+	uint32_t mode_caps;
 
 	if (pe[port].identity[0] == 0) {
 		ccprintf("No identity discovered yet.\n");
@@ -385,14 +449,13 @@ static void dump_pe(int port)
 			ccprintf(" [%d] %08x", j + 1,
 				 pe[port].svids[i].mode_vdo[j]);
 		ccprintf("\n");
+		modep = get_modep(port, pe[port].svids[i].svid);
+		if (modep) {
+			mode_caps = modep->data->mode_vdo[modep->opos - 1];
+			ccprintf("MODE[%d]: svid:%04x caps:%08x\n", modep->opos,
+				 modep->fx->svid, mode_caps);
+		}
 	}
-	if (!modep->opos) {
-		ccprintf("No mode chosen yet.\n");
-		return;
-	}
-	modep = get_modep(port);
-	ccprintf("MODE[%d]: svid:%04x caps:%08x\n", modep->opos,
-		 modep->fx->svid, modep->mode_caps);
 }
 
 static int command_pe(int argc, char **argv)
@@ -480,6 +543,11 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 			rsize = 1;
 		}
 	} else if (cmd_type == CMDT_RSP_ACK) {
+#ifdef CONFIG_USB_PD_ALT_MODE_DFP
+		struct svdm_amode_data *modep;
+
+		modep = get_modep(port, PD_VDO_VID(payload[0]));
+#endif
 		switch (cmd) {
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
 		case CMD_DISCOVER_IDENT:
@@ -493,36 +561,33 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 		case CMD_DISCOVER_MODES:
 			dfp_consume_modes(port, cnt, payload);
 			rsize = dfp_discover_modes(port, payload);
-			if (!rsize)
-				rsize = dfp_enter_mode(port, payload, 0);
+			/* enter the default mode for DFP */
+			if (!rsize) {
+				payload[0] = pd_dfp_enter_mode(port, 0, 0);
+				if (payload[0])
+					rsize = 1;
+			}
 			break;
 		case CMD_ENTER_MODE:
-			/*
-			 * TODO(crosbug.com/p/33946): Fix won't allow multiple
-			 * mode entry.
-			 */
-			if (!pe[port].amode.opos)
-				dfp_enter_mode(port, payload, 1);
-			if (pe[port].amode.opos) {
-				rsize = pe[port].amode.fx->status(port,
-								  payload);
-				payload[0] |= VDO_OPOS(pd_alt_mode(port));
+			if (!modep->opos)
+				pd_dfp_enter_mode(port, 0, 0);
+			if (modep->opos) {
+				rsize = modep->fx->status(port, payload);
+				payload[0] |= PD_VDO_OPOS(modep->opos);
 			}
 			break;
 		case CMD_DP_STATUS:
 			/* DP status response & UFP's DP attention have same
 			   payload */
 			dfp_consume_attention(port, payload);
-			if (pe[port].amode.opos)
-				rsize = pe[port].amode.fx->config(port,
-								  payload);
+			if (modep->opos)
+				rsize = modep->fx->config(port, payload);
 			else
 				rsize = 0;
 			break;
 		case CMD_DP_CONFIG:
-			if (pe[port].amode.opos &&
-			    pe[port].amode.fx->post_config)
-				pe[port].amode.fx->post_config(port);
+			if (modep->opos && modep->fx->post_config)
+				modep->fx->post_config(port);
 			/* no response after DFPs ack */
 			rsize = 0;
 			break;
@@ -592,8 +657,12 @@ void pd_usb_billboard_deferred(void)
 #if defined(CONFIG_USB_PD_ALT_MODE) && !defined(CONFIG_USB_PD_ALT_MODE_DFP) \
 	&& !defined(CONFIG_USB_PD_SIMPLE_DFP)
 
-	/* port always zero for these UFPs */
-	if (!pd_alt_mode(0))
+	/*
+	 * TODO(tbroch)
+	 * 1. Will we have multiple type-C port UFPs
+	 * 2. Will there be other modes applicable to DFPs besides DP
+	 */
+	if (!pd_alt_mode(0, USB_SID_DISPLAYPORT))
 		usb_connect();
 
 #endif
@@ -624,6 +693,7 @@ DECLARE_HOST_COMMAND(EC_CMD_USB_PD_DISCOVERY,
 
 static int hc_remote_pd_get_amode(struct host_cmd_handler_args *args)
 {
+	struct svdm_amode_data *modep;
 	const struct ec_params_usb_pd_get_mode_request *p = args->params;
 	struct ec_params_usb_pd_get_mode_response *r = args->response;
 
@@ -638,13 +708,13 @@ static int hc_remote_pd_get_amode(struct host_cmd_handler_args *args)
 	}
 
 	r->svid = pe[p->port].svids[p->svid_idx].svid;
-	r->active = 0;
+	r->opos = 0;
 	memcpy(r->vdo, pe[p->port].svids[p->svid_idx].mode_vdo, 24);
+	modep = get_modep(p->port, r->svid);
+
+	if (modep)
+		r->opos = pd_alt_mode(p->port, r->svid);
 
-	if (pe[p->port].amode.opos && pe[p->port].amode.fx->svid == r->svid) {
-		r->active = 1;
-		r->opos = pd_alt_mode(p->port);
-	}
 	args->response_size = sizeof(*r);
 	return EC_RES_SUCCESS;
 }
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index e1890ddb2d..5911f5d570 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -391,7 +391,7 @@ static inline void set_state(int port, enum pd_states next_state)
 		pd[port].dev_id = 0;
 		pd[port].flags &= ~PD_FLAGS_RESET_ON_DISCONNECT_MASK;
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
-		pd_dfp_exit_mode(port);
+		pd_dfp_exit_mode(port, 0, 0);
 #endif
 #ifdef CONFIG_USBC_SS_MUX
 		board_set_usb_mux(port, TYPEC_MUX_NONE,
@@ -811,8 +811,7 @@ static void execute_hard_reset(int port)
 
 	pd[port].msg_id = 0;
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
-	pd_dfp_exit_mode(port);
-	pd_dfp_pe_init(port);
+	pd_dfp_exit_mode(port, 0, 0);
 #endif
 
 #ifdef CONFIG_USB_PD_DUAL_ROLE
@@ -2870,7 +2869,7 @@ static int remote_flashing(int argc, char **argv)
 void pd_send_hpd(int port, enum hpd_event hpd)
 {
 	uint32_t data[1];
-	int opos = pd_alt_mode(port);
+	int opos = pd_alt_mode(port, USB_SID_DISPLAYPORT);
 	if (!opos)
 		return;
 
@@ -3382,26 +3381,27 @@ static int hc_remote_pd_set_amode(struct host_cmd_handler_args *args)
 {
 	const struct ec_params_usb_pd_set_mode_request *p = args->params;
 
-	if (p->port >= PD_PORT_COUNT)
+	if ((p->port >= PD_PORT_COUNT) || (!p->svid) || (!p->opos))
 		return EC_RES_INVALID_PARAM;
 
-	/* if in a mode exit it */
-	/* TODO(crosbug.com/p/33946): allow entry of multiple modes */
-	if (pd_alt_mode(p->port)) {
-		uint32_t vdo = pd_dfp_exit_mode(p->port);
-		if (vdo) {
-			queue_vdm(p->port, &vdo, NULL, 0);
-			task_wake(PORT_TO_TASK_ID(p->port));
-			/* Wait until exit VDM is done */
-			while (pd[p->port].vdm_state > 0)
-				task_wait_event(PD_T_VDM_E_MODE);
+	switch (p->cmd) {
+	case PD_EXIT_MODE:
+		if (pd_dfp_exit_mode(p->port, p->svid, p->opos))
+			pd_send_vdm(p->port, p->svid,
+				    CMD_EXIT_MODE | VDO_OPOS(p->opos), NULL, 0);
+		else {
+			CPRINTF("Failed exit mode\n");
+			return EC_RES_ERROR;
 		}
+		break;
+	case PD_ENTER_MODE:
+		if (pd_dfp_enter_mode(p->port, p->svid, p->opos))
+			pd_send_vdm(p->port, p->svid, CMD_ENTER_MODE |
+				    VDO_OPOS(p->opos), NULL, 0);
+		break;
+	default:
+		return EC_RES_INVALID_PARAM;
 	}
-
-	/* now try to enter new one. */
-	pd_send_vdm(p->port, p->svid,
-		    CMD_ENTER_MODE | VDO_OPOS(p->opos), NULL, 0);
-
 	return EC_RES_SUCCESS;
 }
 DECLARE_HOST_COMMAND(EC_CMD_USB_PD_SET_AMODE,
diff --git a/include/ec_commands.h b/include/ec_commands.h
index 926e317eee..58a698fb8d 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -2929,16 +2929,23 @@ struct ec_params_usb_pd_get_mode_request {
 
 struct ec_params_usb_pd_get_mode_response {
 	uint16_t svid;   /* SVID */
-	uint8_t active;  /* Active SVID */
-	uint8_t opos;    /* Object Position */
+	uint16_t opos;    /* Object Position */
 	uint32_t vdo[6]; /* Mode VDOs */
 } __packed;
 
 #define EC_CMD_USB_PD_SET_AMODE 0x117
+
+enum pd_mode_cmd {
+	PD_EXIT_MODE = 0,
+	PD_ENTER_MODE = 1,
+	/* Not a command.  Do NOT remove. */
+	PD_MODE_CMD_COUNT,
+};
+
 struct ec_params_usb_pd_set_mode_request {
-	int opos;      /* Object Position */
-	int svid_idx;  /* Index of svid to get */
+	uint32_t cmd;  /* enum pd_mode_cmd */
 	uint16_t svid; /* SVID to set */
+	uint8_t opos;  /* Object Position */
 	uint8_t port;  /* port */
 } __packed;
 
diff --git a/include/usb_pd.h b/include/usb_pd.h
index 27c3ae1740..61a93528d0 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -110,7 +110,6 @@ enum pd_errors {
 
 #define BDO(mode, cnt)      ((mode) | ((cnt) & 0xFFFF))
 
-/* TODO(tbroch) is there a finite number for these in the spec */
 #define SVID_DISCOVERY_MAX 16
 
 /* Timers */
@@ -195,7 +194,7 @@ struct svdm_amode_data {
 	/* VDM object position */
 	int opos;
 	/* mode capabilities specific to SVID amode. */
-	uint32_t mode_caps;
+	struct svdm_svid_data *data;
 };
 
 enum hpd_event {
@@ -209,6 +208,14 @@ enum hpd_event {
 #define DP_FLAGS_DP_ON              (1 << 0) /* Display port mode is on */
 #define DP_FLAGS_HPD_HI_PENDING     (1 << 1) /* Pending HPD_HI */
 
+/* supported alternate modes */
+enum pd_alternate_modes {
+	PD_AMODE_GOOGLE,
+	PD_AMODE_DISPLAYPORT,
+	/* not a real mode */
+	PD_AMODE_COUNT,
+};
+
 /* Policy structure for driving alternate mode */
 struct pd_policy {
 	/* index of svid currently being operated on */
@@ -219,8 +226,10 @@ struct pd_policy {
 	uint32_t identity[PDO_MAX_OBJECTS - 1];
 	/* supported svids & corresponding vdo mode data */
 	struct svdm_svid_data svids[SVID_DISCOVERY_MAX];
-	/*  active mode */
-	struct svdm_amode_data amode;
+	/*  active modes */
+	struct svdm_amode_data amodes[PD_AMODE_COUNT];
+	/* Next index to insert DFP alternate mode into amodes */
+	int amode_idx;
 };
 
 /*
@@ -921,12 +930,24 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload);
 int pd_custom_flash_vdm(int port, int cnt, uint32_t *payload);
 
 /**
- * Exit alternate mode on DFP
+ * Enter alternate mode on DFP
  *
  * @param port     USB-C port number
- * @return VDO to send to UFP or zero if none
+ * @param svid USB standard or vendor id to exit or zero for DFP amode reset.
+ * @param opos object position of mode to exit.
+ * @return vdm for UFP to be sent to enter mode or zero if not.
+ */
+uint32_t pd_dfp_enter_mode(int port, uint16_t svid, int opos);
+
+/**
+ * Exit alternate mode on DFP
+ *
+ * @param port USB-C port number
+ * @param svid USB standard or vendor id to exit or zero for DFP amode reset.
+ * @param opos object position of mode to exit.
+ * @return 1 if UFP should be sent exit mode VDM.
  */
-uint32_t pd_dfp_exit_mode(int port);
+int pd_dfp_exit_mode(int port, uint16_t svid, int opos);
 
 /**
  * Initialize policy engine for DFP
@@ -1016,9 +1037,10 @@ void board_flip_usb_mux(int port);
  * Determine if in alternate mode or not.
  *
  * @param port port number.
+ * @param svid USB standard or vendor id
  * @return object position of mode chosen in alternate mode otherwise zero.
  */
-int pd_alt_mode(int port);
+int pd_alt_mode(int port, uint16_t svid);
 
 /**
  * Send hpd over USB PD.
diff --git a/util/ectool.c b/util/ectool.c
index de1b378fdc..cb7e7e904e 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -876,7 +876,7 @@ static int in_gfu_mode(int *opos, int port)
 		}
 	}
 
-	return r->active && (r->opos == *opos);
+	return r->opos == *opos;
 }
 
 /**
@@ -905,6 +905,7 @@ static int enter_gfu_mode(int port)
 		p->port = port;
 		p->svid = USB_VID_GOOGLE;
 		p->opos = opos;
+		p->cmd = PD_ENTER_MODE;
 
 		ec_command(EC_CMD_USB_PD_SET_AMODE, 0, p, sizeof(*p),
 			   NULL, 0);
@@ -1113,8 +1114,9 @@ int cmd_pd_set_amode(int argc, char *argv[])
 	struct ec_params_usb_pd_set_mode_request *p =
 		(struct ec_params_usb_pd_set_mode_request *)ec_outbuf;
 
-	if (argc < 4) {
-		fprintf(stderr, "Usage: %s <port> <svid> <opos>\n", argv[0]);
+	if (argc < 5) {
+		fprintf(stderr, "Usage: %s <port> <svid> <opos> <cmd>\n",
+			argv[0]);
 		return -1;
 	}
 
@@ -1125,17 +1127,22 @@ int cmd_pd_set_amode(int argc, char *argv[])
 	}
 
 	p->svid = strtol(argv[2], &e, 0);
-	if (e && *e) {
+	if ((e && *e) || !p->svid) {
 		fprintf(stderr, "Bad svid\n");
 		return -1;
 	}
 
 	p->opos = strtol(argv[3], &e, 0);
-	if (e && *e) {
-		fprintf(stderr, "Bad mode\n");
+	if ((e && *e) || !p->opos) {
+		fprintf(stderr, "Bad opos\n");
 		return -1;
 	}
 
+	p->cmd = strtol(argv[4], &e, 0);
+	if ((e && *e) || (p->cmd >= PD_MODE_CMD_COUNT)) {
+		fprintf(stderr, "Bad cmd\n");
+		return -1;
+	}
 	return ec_command(EC_CMD_USB_PD_SET_AMODE, 0, p, sizeof(*p), NULL, 0);
 }
 
@@ -1165,10 +1172,10 @@ int cmd_pd_get_amode(int argc, char *argv[])
 			   ec_inbuf, ec_max_insize);
 		if (!r->svid)
 			break;
-		printf("%cSVID:0x%04x ", (r->active) ? '*' : ' ',
+		printf("%cSVID:0x%04x ", (r->opos) ? '*' : ' ',
 		       r->svid);
 		for (i = 0; i < PDO_MODES; i++) {
-			printf("%c0x%08x ", (r->active && (r->opos == i + 1)) ?
+			printf("%c0x%08x ", (r->opos && (r->opos == i + 1)) ?
 			       '*' : ' ', r->vdo[i]);
 		}
 		printf("\n");