1 files changed, 100 insertions, 0 deletions

diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
index c581b0077d..abf2ef4125 100644
--- a/common/usb_pd_alt_mode_dfp.c
+++ b/common/usb_pd_alt_mode_dfp.c
@@ -5,9 +5,66 @@
  * Alternate Mode Downstream Facing Port (DFP) USB-PD module.
  */
 
+#include "console.h"
 #include "usb_pd.h"
 #include "util.h"
 
+#ifdef CONFIG_COMMON_RUNTIME
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
+#else
+#define CPRINTS(format, args...)
+#define CPRINTF(format, args...)
+#endif
+
+static int pd_get_mode_idx(int port, uint16_t svid)
+{
+	int i;
+	struct pd_policy *pe = pd_get_am_policy(port);
+
+	for (i = 0; i < PD_AMODE_COUNT; i++) {
+		if (pe->amodes[i].fx &&
+		    (pe->amodes[i].fx->svid == svid))
+			return i;
+	}
+	return -1;
+}
+
+static int pd_allocate_mode(int port, uint16_t svid)
+{
+	int i, j;
+	struct svdm_amode_data *modep;
+	int mode_idx = pd_get_mode_idx(port, svid);
+	struct pd_policy *pe = pd_get_am_policy(port);
+
+	if (mode_idx != -1)
+		return mode_idx;
+
+	/* There's no space to enter another mode */
+	if (pe->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++) {
+		for (j = 0; j < pe->svid_cnt; j++) {
+			struct svdm_svid_data *svidp = &pe->svids[j];
+
+			if ((svidp->svid != supported_modes[i].svid) ||
+			    (svid && (svidp->svid != svid)))
+				continue;
+
+			modep = &pe->amodes[pe->amode_idx];
+			modep->fx = &supported_modes[i];
+			modep->data = &pe->svids[j];
+			pe->amode_idx++;
+			return pe->amode_idx - 1;
+		}
+	}
+	return -1;
+}
+
 /*
  * This algorithm defaults to choosing higher pin config over lower ones in
  * order to prefer multi-function if desired.
@@ -59,3 +116,46 @@ int pd_dfp_dp_get_pin_mode(int port, uint32_t status)
 
 	return 1 << get_next_bit(&pin_caps);
 }
+
+struct svdm_amode_data *pd_get_amode_data(int port, uint16_t svid)
+{
+	int idx = pd_get_mode_idx(port, svid);
+	struct pd_policy *pe = pd_get_am_policy(port);
+
+	return (idx == -1) ? NULL : &pe->amodes[idx];
+}
+
+/*
+ * 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 = pd_allocate_mode(port, svid);
+	struct pd_policy *pe = pd_get_am_policy(port);
+	struct svdm_amode_data *modep;
+	uint32_t mode_caps;
+
+	if (mode_idx == -1)
+		return 0;
+	modep = &pe->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;
+	}
+
+	mode_caps = modep->data->mode_vdo[modep->opos - 1];
+	if (modep->fx->enter(port, mode_caps) == -1)
+		return 0;
+
+	pd_set_dfp_enter_mode_flag(port, true);
+
+	/* SVDM to send to UFP for mode entry */
+	return VDO(modep->fx->svid, 1, CMD_ENTER_MODE | VDO_OPOS(modep->opos));
+}