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");