diff --git a/board/fruitpie/usb_pd_policy.c b/board/fruitpie/usb_pd_policy.c
index 216bc1bb18..bd8588bdb3 100644
--- a/board/fruitpie/usb_pd_policy.c
+++ b/board/fruitpie/usb_pd_policy.c
@@ -204,21 +204,4 @@ const struct svdm_amode_fx supported_modes[] = {
 		.exit = &svdm_exit_dp_mode,
 	},
 };
-
-void pd_dfp_choose_modes(struct pd_policy *pe)
-{
-	int i, j;
-	pe->amode_cnt = ARRAY_SIZE(supported_modes);
-	pe->amodes->fx = supported_modes;
-	for (i = 0; i < pe->amode_cnt; i++) {
-		for (j = 0; j < pe->svid_cnt; j++) {
-			if (pe->svids[j].svid == pe->amodes->fx[i].svid) {
-				/* TODO(tbroch) need more elaborate mode
-				   resolution */
-				pe->amodes[i].mode_caps = pe->svids[j].mode_vdo;
-				pe->amodes[i].amode = dfp_amode1;
-				break;
-			}
-		}
-	}
-}
+const int supported_modes_cnt = ARRAY_SIZE(supported_modes);
diff --git a/board/samus_pd/usb_pd_policy.c b/board/samus_pd/usb_pd_policy.c
index dd875f0ec4..ed8a2360de 100644
--- a/board/samus_pd/usb_pd_policy.c
+++ b/board/samus_pd/usb_pd_policy.c
@@ -234,24 +234,7 @@ const struct svdm_amode_fx supported_modes[] = {
 		.exit = &svdm_exit_dp_mode,
 	},
 };
-
-void pd_dfp_choose_modes(struct pd_policy *pe)
-{
-	int i, j;
-	pe->amode_cnt = ARRAY_SIZE(supported_modes);
-	pe->amodes->fx = supported_modes;
-	for (i = 0; i < pe->amode_cnt; i++) {
-		for (j = 0; j < pe->svid_cnt; j++) {
-			if (pe->svids[j].svid == pe->amodes->fx[i].svid) {
-				/* TODO(tbroch) need more elaborate mode
-				   resolution */
-				pe->amodes[i].mode_caps = pe->svids[j].mode_vdo;
-				pe->amodes[i].amode = dfp_amode1;
-				break;
-			}
-		}
-	}
-}
+const int supported_modes_cnt = ARRAY_SIZE(supported_modes);
 /****************************************************************************/
 /* Console commands */
 static int command_ec_int(int argc, char **argv)
diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index 4a2aab55e1..d1f47b6254 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -98,46 +98,49 @@ static int dfp_consume_modes(int port, uint32_t *payload)
 	return (pe[port].svid_idx < pe[port].svid_cnt);
 }
 
-static int dfp_enter_modes(int port, uint32_t *payload)
+/* TODO(tbroch) this function likely needs to move up the stack to where system
+ * policy decisions are made. */
+static int dfp_enter_mode(int port, uint32_t *payload)
 {
-	struct svdm_amode_data *modep;
-	if (pe[port].amodes == NULL)
-		pd_dfp_choose_modes(&pe[port]);
-
-	modep = &pe[port].amodes[pe[port].amode_idx];
-	if (modep->amode == dfp_amode_none)
+	int i, j, done;
+	struct svdm_amode_data *modep = &pe[port].amode;
+	pe[port].amode.index = -1; /* Error condition */
+	for (i = 0, done = 0; !done && (i < supported_modes_cnt); i++) {
+		for (j = 0; j < pe[port].svid_cnt; j++) {
+			if (pe[port].svids[j].svid != supported_modes[i].svid)
+				continue;
+			pe[port].amode.fx = &supported_modes[i];
+			pe[port].amode.mode_caps =
+				pe[port].svids[j].mode_vdo[0];
+			pe[port].amode.index = 0;
+			done = 1;
+			break;
+		}
+	}
+	if (modep->index == -1)
 		return 0;
 
-	modep->fx->enter(port, *modep->mode_caps);
+	modep->fx->enter(port, modep->mode_caps);
 	payload[0] = VDO(modep->fx->svid, 1,
 			 CMD_ENTER_MODE |
-			 VDO_OPOS(modep->amode));
-	pe[port].amode_idx++;
+			 VDO_OPOS((modep->index + 1)));
 	return 1;
 }
 
-int pd_exit_modes(int port, uint32_t *payload)
+int pd_exit_mode(int port, uint32_t *payload)
 {
-	struct svdm_amode_data *modep;
-	if (pe[port].amodes == NULL)
-		return 0;
-
-	modep = &pe[port].amodes[pe[port].amode_idx];
-	if (modep->amode == dfp_amode_none)
-		return 1;
-
+	struct svdm_amode_data *modep = &pe[port].amode;
 	modep->fx->exit(port);
 	payload[0] = VDO(modep->fx->svid, 1,
 			 CMD_EXIT_MODE |
-			 VDO_OPOS(modep->amode));
-	pe[port].amode_idx++;
+			 VDO_OPOS((modep->index + 1)));
 	return 1;
 }
 
 static void dump_pe(int port)
 {
 	int i, j;
-	struct svdm_amode_data *modep = pe[port].amodes;
+	struct svdm_amode_data *modep = &pe[port].amode;
 
 	for (i = 0; i < pe[port].svid_cnt; i++) {
 		ccprintf("SVID[%d]: %04x", i, pe[port].svids[i].svid);
@@ -146,11 +149,8 @@ static void dump_pe(int port)
 				 pe[port].svids[i].mode_vdo[j]);
 		ccprintf("\n");
 	}
-	for (i = 0; i < pe[port].amode_cnt; i++) {
-		ccprintf("MODE[%d]: svid:%04x mode:%d caps:%08x\n", i,
-			 modep->fx->svid, modep->amode, *modep->mode_caps);
-		modep++;
-	}
+	ccprintf("MODE[%d]: svid:%04x mode:%d caps:%08x\n", i,
+		 modep->fx->svid, modep->index + 1, modep->mode_caps);
 }
 
 static int command_pe(int argc, char **argv)
@@ -237,12 +237,12 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 			if (dfp_consume_modes(port, payload))
 				rsize = dfp_discover_modes(port, payload);
 			else
-				rsize = dfp_enter_modes(port, payload);
+				rsize = dfp_enter_mode(port, payload);
 			break;
 		case CMD_ENTER_MODE:
-			rsize = dfp_enter_modes(port, payload);
+			rsize = dfp_enter_mode(port, payload);
 		case CMD_EXIT_MODE:
-			rsize = pd_exit_modes(port, payload);
+			rsize = pd_exit_mode(port, payload);
 			break;
 		}
 		payload[0] &= ~VDO_CMDT(0);
@@ -294,7 +294,7 @@ int pd_vdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 #endif /* !CONFIG_USB_PD_CUSTOM_VDM */
 
 #ifndef CONFIG_USB_PD_ALT_MODE_DFP
-int pd_exit_modes(int port, uint32_t *payload)
+int pd_exit_mode(int port, uint32_t *payload)
 {
 	return 0;
 }
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index 474a031af1..6e57bcfcc6 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -1479,7 +1479,7 @@ void pd_task(void)
 				hard_reset_count++;
 #endif
 
-			pd_exit_modes(port, payload);
+			pd_exit_mode(port, payload);
 			send_hard_reset(port);
 			/* reset our own state machine */
 			execute_hard_reset(port);
diff --git a/include/usb_pd.h b/include/usb_pd.h
index 8146562df4..201ad057bd 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -115,20 +115,6 @@ struct svdm_response {
 	int (*exit_mode)(int port, uint32_t *payload);
 };
 
-/* defined in <board>/usb_pd_policy.c */
-extern const struct svdm_response svdm_rsp;
-
-/* Each SVID is allowed to have 6 modes */
-enum dfp_amode {
-	dfp_amode_none = 0,
-	dfp_amode1,
-	dfp_amode2,
-	dfp_amode3,
-	dfp_amode4,
-	dfp_amode5,
-	dfp_amode6,
-};
-
 struct svdm_svid_data {
 	uint16_t svid;
 	uint32_t mode_vdo[PDO_MODES];
@@ -140,10 +126,17 @@ struct svdm_amode_fx {
 	void (*exit)(int port);
 };
 
+/* defined in <board>/usb_pd_policy.c */
+/* All UFP_U should have */
+extern const struct svdm_response svdm_rsp;
+/* All DFP_U should have */
+extern const struct svdm_amode_fx supported_modes[];
+extern const int supported_modes_cnt;
+
 struct svdm_amode_data {
 	const struct svdm_amode_fx *fx;
-	enum dfp_amode amode;
-	uint32_t *mode_caps;
+	int index;
+	uint32_t mode_caps;
 };
 
 /* Policy structure for driving alternate mode */
@@ -154,12 +147,8 @@ struct pd_policy {
 	int svid_cnt;
 	/* supported svids & corresponding vdo mode data */
 	struct svdm_svid_data svids[SVID_DISCOVERY_MAX];
-	/* index of amode currently being operated on */
-	int amode_idx;
-	/* count of amodes discovered */
-	int amode_cnt;
-	/* supported amodes */
-	struct svdm_amode_data *amodes;
+	/*  active mode */
+	struct svdm_amode_data amode;
 };
 
 /*
@@ -611,13 +600,6 @@ int pd_vdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload);
  */
 int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload);
 
-/**
- * Choose appropriate alternate modes.
- *
- * @param pe        pd_policy data structure
- */
-void pd_dfp_choose_modes(struct pd_policy *pe);
-
 /**
  * Exit alternate mode
  *
@@ -625,7 +607,7 @@ void pd_dfp_choose_modes(struct pd_policy *pe);
  * @param payload  payload data.
  * @return if >0, number of VDOs to send back.
  */
-int pd_exit_modes(int port, uint32_t *payload);
+int pd_exit_mode(int port, uint32_t *payload);
 
 /**
  * Store Device ID & RW hash of device