TCPMv1/v2: Move pd_dfp_enter_mode() to common file

BUG=b:148528713
BRANCH=none
TEST=make buildall -j

Change-Id: Id1d3e8bc27d895a53b53a77cf1c8fd36c69b47dc
Signed-off-by: Vijay Hiremath <vijay.p.hiremath@intel.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2032156
Reviewed-by: Keith Short <keithshort@chromium.org>

4 files changed, 127 insertions, 162 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));
+}
diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index 022cfab1ab..968c02d1b7 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -774,23 +774,9 @@ static bool check_tbt_cable_speed(int port)
 						TBT_SS_U32_GEN1_GEN2);
 }
 
-static int get_mode_idx(int port, uint16_t svid)
+struct pd_policy *pd_get_am_policy(int port)
 {
-	int i;
-
-	for (i = 0; i < PD_AMODE_COUNT; i++) {
-		if (pe[port].amodes[i].fx &&
-		    (pe[port].amodes[i].fx->svid == svid))
-			return i;
-	}
-	return -1;
-}
-
-struct svdm_amode_data *pd_get_amode_data(int port, uint16_t svid)
-{
-	int idx = get_mode_idx(port, svid);
-
-	return (idx == -1) ? NULL : &pe[port].amodes[idx];
+	return &pe[port];
 }
 
 int pd_alt_mode(int port, uint16_t svid)
@@ -800,69 +786,9 @@ int pd_alt_mode(int port, uint16_t svid)
 	return (modep) ? modep->opos : -1;
 }
 
-int allocate_mode(int port, uint16_t svid)
+/* Note: Enter mode flag is not needed by TCPMv1 */
+void pd_set_dfp_enter_mode_flag(int port, bool set)
 {
-	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++) {
-		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->data = &pe[port].svids[j];
-			pe[port].amode_idx++;
-			return pe[port].amode_idx - 1;
-		}
-	}
-	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;
-	}
-
-	mode_caps = modep->data->mode_vdo[modep->opos - 1];
-	if (modep->fx->enter(port, mode_caps) == -1)
-		return 0;
-
-	/* SVDM to send to UFP for mode entry */
-	return VDO(modep->fx->svid, 1, CMD_ENTER_MODE | VDO_OPOS(modep->opos));
 }
 
 /*
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index b69b0acfbf..2b101b8e29 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -497,7 +497,6 @@ static void dfp_consume_identity(int port, int cnt, uint32_t *payload);
 static void dfp_consume_svids(int port, int cnt, uint32_t *payload);
 static int dfp_discover_modes(int port, uint32_t *payload);
 static void dfp_consume_modes(int port, int cnt, uint32_t *payload);
-static int get_mode_idx(int port, uint16_t svid);
 #endif
 
 test_export_static enum usb_pe_state get_state_pe(const int port);
@@ -4709,23 +4708,9 @@ static void dfp_consume_modes(int port, int cnt, uint32_t *payload)
 	pe[port].am_policy.svid_idx++;
 }
 
-static int get_mode_idx(int port, uint16_t svid)
+struct pd_policy *pd_get_am_policy(int port)
 {
-	int i;
-
-	for (i = 0; i < PD_AMODE_COUNT; i++) {
-		if (pe[port].am_policy.amodes[i].fx->svid == svid)
-			return i;
-	}
-
-	return -1;
-}
-
-struct svdm_amode_data *pd_get_amode_data(int port, uint16_t svid)
-{
-	int idx = get_mode_idx(port, svid);
-
-	return (idx == -1) ? NULL : &pe[port].am_policy.amodes[idx];
+	return &pe[port].am_policy;
 }
 
 int pd_alt_mode(int port, uint16_t svid)
@@ -4735,74 +4720,12 @@ int pd_alt_mode(int port, uint16_t 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].am_policy.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 (j = 0; j < pe[port].am_policy.svid_cnt; j++) {
-			struct svdm_svid_data *svidp =
-						&pe[port].am_policy.svids[j];
-
-			if ((svidp->svid != supported_modes[i].svid) ||
-					(svid && (svidp->svid != svid)))
-				continue;
-
-			modep =
-		&pe[port].am_policy.amodes[pe[port].am_policy.amode_idx];
-			modep->fx = &supported_modes[i];
-			modep->data = &pe[port].am_policy.svids[j];
-			pe[port].am_policy.amode_idx++;
-			return pe[port].am_policy.amode_idx - 1;
-		}
-	}
-	return -1;
-}
-
-uint32_t pd_dfp_enter_mode(int port, uint16_t svid, int opos)
+void pd_set_dfp_enter_mode_flag(int port, bool set)
 {
-	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].am_policy.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;
-
-	PE_SET_FLAG(port, PE_FLAGS_MODAL_OPERATION);
-
-	/* SVDM to send to UFP for mode entry */
-	return VDO(modep->fx->svid, 1, CMD_ENTER_MODE | VDO_OPOS(modep->opos));
+	if (set)
+		PE_SET_FLAG(port, PE_FLAGS_MODAL_OPERATION);
+	else
+		PE_CLR_FLAG(port, PE_FLAGS_MODAL_OPERATION);
 }
 
 static int validate_mode_request(struct svdm_amode_data *modep,
diff --git a/include/usb_pd.h b/include/usb_pd.h
index 3c84564243..ebb5343abe 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -1728,6 +1728,22 @@ bool consume_sop_prime_prime_repeat_msg(int port, uint8_t msg_id);
  */
 bool is_transmit_msg_sop_prime(int port);
 
+/*
+ * Return the pointer to PD alternate mode policy
+ *
+ * @param port  USB-C port number
+ * @return      pointer to PD alternate mode policy
+ */
+struct pd_policy *pd_get_am_policy(int port);
+
+/**
+ * Set DFP enter mode flags if available
+ *
+ * @param port  USB-C port number
+ * @param set   If true set the flag else clear
+ */
+void pd_set_dfp_enter_mode_flag(int port, bool set);
+
 /**
  * Returns the status of cable flag - CABLE_FLAGS_SOP_PRIME_PRIME_ENABLE
  *