TCPMv2: Allocate FRS ports 3.0 A when needed

When there are no PD sinks requiring 3.0 A, any FRS ports which require
that much current will have FRS detection enabled.  If a PD sink
requiring 3.0 A is attached, then FRS detection will be disabled on that
port again.

FRS port partners which require only USB or 1.5 A of current will always
have FRS detection enabled.

BRANCH=None
BUG=b:141690755
TEST=on volteer-based system, verify:
- FRS source partners requiring 1.5 A or less always have FRS detection
enabled
- FRS detection is disabled on detach
- FRS source partner requiring 3.0 A has FRS detection enabled if no PD
sink needing 3.0 A is present
- Plugging in a 3.0 A PD sink disables FRS detection for the 3.0 A FRS
partner
- Unplugging a 3.0 A PD sink enables FRS detection for the 3.0 A FRS
partner
- When FRS occurs on 3.0 A port with detection enabled, partner is
offered appropriate starting current in first source caps

Signed-off-by: Diana Z <dzigterman@chromium.org>
Change-Id: Ib067ac7c1c2f4a6e64f36aeffd1b8dde5cb93760
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2676084
Reviewed-by: Keith Short <keithshort@chromium.org>
Reviewed-by: Eric Herrmann <eherrmann@chromium.org>
Tested-by: Eric Herrmann <eherrmann@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2715843
Tested-by: Abe Levkoy <alevkoy@chromium.org>
Reviewed-by: Abe Levkoy <alevkoy@chromium.org>
Commit-Queue: Abe Levkoy <alevkoy@chromium.org>

5 files changed, 143 insertions, 97 deletions

diff --git a/common/mock/usb_pd_dpm_mock.c b/common/mock/usb_pd_dpm_mock.c
index 8edae9ee0b..edac099a02 100644
--- a/common/mock/usb_pd_dpm_mock.c
+++ b/common/mock/usb_pd_dpm_mock.c
@@ -56,6 +56,10 @@ void dpm_remove_sink(int port)
 {
 }
 
+void dpm_remove_source(int port)
+{
+}
+
 int dpm_get_source_pdo(const uint32_t **src_pdo, const int port)
 {
 	*src_pdo = pd_src_pdo;
diff --git a/common/usbc/usb_pd_dpm.c b/common/usbc/usb_pd_dpm.c
index b1e6bad468..01c01af4e8 100644
--- a/common/usbc/usb_pd_dpm.c
+++ b/common/usbc/usb_pd_dpm.c
@@ -12,6 +12,7 @@
 #include "compile_time_macros.h"
 #include "console.h"
 #include "ec_commands.h"
+#include "hooks.h"
 #include "system.h"
 #include "task.h"
 #include "tcpm/tcpm.h"
@@ -364,7 +365,10 @@ static mutex_t		max_current_claimed_lock;
 static bool		dpm_mutex_initialized;
 #endif
 
-static uint32_t sink_max_pdo_requested;	/* Ports with PD sink needing > 1.5A */
+/* Ports with PD sink needing > 1.5 A */
+static uint32_t sink_max_pdo_requested;
+/* Ports with FRS source needing > 1.5 A */
+static uint32_t source_frs_max_requested;
 
 #define LOWEST_PORT(p) __builtin_ctz(p)  /* Undefined behavior if p == 0 */
 
@@ -383,9 +387,23 @@ static int count_port_bits(uint32_t bitmask)
 /*
  * Centralized, mutex-controlled updates to the claimed 3.0 A ports
  */
+static void balance_source_ports(void);
+DECLARE_DEFERRED(balance_source_ports);
+
 static void balance_source_ports(void)
 {
 	uint32_t removed_ports, new_ports;
+	static bool deferred_waiting;
+
+	if (task_get_current() == TASK_ID_HOOKS)
+		deferred_waiting = false;
+
+	/*
+	 * Ignore balance attempts while we're waiting for a downgraded port to
+	 * finish the downgrade.
+	 */
+	if (deferred_waiting)
+		return;
 
 #ifdef CONFIG_ZEPHYR
 	if (!dpm_mutex_initialized) {
@@ -397,7 +415,8 @@ static void balance_source_ports(void)
 	mutex_lock(&max_current_claimed_lock);
 
 	/* Remove any ports which no longer require 3.0 A */
-	removed_ports = max_current_claimed & ~sink_max_pdo_requested;
+	removed_ports = max_current_claimed & ~(sink_max_pdo_requested |
+						source_frs_max_requested);
 	max_current_claimed &= ~removed_ports;
 
 	/* Allocate 3.0 A to new PD sink ports that need it */
@@ -410,6 +429,18 @@ static void balance_source_ports(void)
 			max_current_claimed |= BIT(new_max_port);
 			typec_select_src_current_limit_rp(new_max_port,
 							  TYPEC_RP_3A0);
+		} else if (source_frs_max_requested & max_current_claimed) {
+			/* Bump lowest FRS port from 3.0 A slot */
+			int rem_frs = LOWEST_PORT(source_frs_max_requested &
+						 max_current_claimed);
+			pd_dpm_request(rem_frs, DPM_REQUEST_FRS_DET_DISABLE);
+			max_current_claimed &= ~BIT(rem_frs);
+
+			/* Give 20 ms for the PD task to process DPM flag */
+			deferred_waiting = true;
+			hook_call_deferred(&balance_source_ports_data,
+					   20 * MSEC);
+			goto unlock;
 		} else {
 			/* TODO(b/141690755): Check lower priority claims */
 			goto unlock;
@@ -417,16 +448,30 @@ static void balance_source_ports(void)
 		new_ports &= ~BIT(new_max_port);
 	}
 
+	/* Allocate 3.0 A to any new FRS ports that need it */
+	new_ports = source_frs_max_requested & ~max_current_claimed;
+	while (new_ports) {
+		int new_frs_port = LOWEST_PORT(new_ports);
+
+		if (count_port_bits(max_current_claimed) <
+						CONFIG_USB_PD_3A_PORTS) {
+			max_current_claimed |= BIT(new_frs_port);
+			pd_dpm_request(new_frs_port,
+				       DPM_REQUEST_FRS_DET_ENABLE);
+		} else {
+			/* TODO(b/141690755): Check lower priority claims */
+			goto unlock;
+		}
+		new_ports &= ~BIT(new_frs_port);
+	}
+
 unlock:
 	mutex_unlock(&max_current_claimed_lock);
 }
 
-/* Process sink's first Sink_Capabilities PDO for port current consideration */
+/* Process port's first Sink_Capabilities PDO for port current consideration */
 void dpm_evaluate_sink_fixed_pdo(int port, uint32_t vsafe5v_pdo)
 {
-	if (CONFIG_USB_PD_3A_PORTS == 0)
-		return;
-
 	/* Verify partner supplied valid vSafe5V fixed object first */
 	if ((vsafe5v_pdo & PDO_TYPE_MASK) != PDO_TYPE_FIXED)
 		return;
@@ -434,11 +479,42 @@ void dpm_evaluate_sink_fixed_pdo(int port, uint32_t vsafe5v_pdo)
 	if (PDO_FIXED_VOLTAGE(vsafe5v_pdo) != 5000)
 		return;
 
-	/* Valid PDO to process, so evaluate whether > 1.5 A is needed */
-	if (PDO_FIXED_CURRENT(vsafe5v_pdo) <= 1500)
-		return;
+	if (pd_get_power_role(port) == PD_ROLE_SOURCE) {
+		if (CONFIG_USB_PD_3A_PORTS == 0)
+			return;
+
+		/* Valid PDO to process, so evaluate whether >1.5A is needed */
+		if (PDO_FIXED_CURRENT(vsafe5v_pdo) <= 1500)
+			return;
 
-	atomic_or(&sink_max_pdo_requested, BIT(port));
+		atomic_or(&sink_max_pdo_requested, BIT(port));
+	} else {
+		int frs_current = vsafe5v_pdo & PDO_FIXED_FRS_CURR_MASK;
+
+		if (!IS_ENABLED(CONFIG_USB_PD_FRS))
+			return;
+
+		/* FRS is only supported in PD 3.0 and higher */
+		if (pd_get_rev(port, TCPC_TX_SOP) == PD_REV20)
+			return;
+
+		if ((vsafe5v_pdo & PDO_FIXED_DUAL_ROLE) && frs_current) {
+			/* Always enable FRS when 3.0 A is not needed */
+			if (frs_current == PDO_FIXED_FRS_CURR_DFLT_USB_POWER ||
+			    frs_current == PDO_FIXED_FRS_CURR_1A5_AT_5V) {
+				pd_dpm_request(port,
+					       DPM_REQUEST_FRS_DET_ENABLE);
+				return;
+			}
+
+			if (CONFIG_USB_PD_3A_PORTS == 0)
+				return;
+
+			atomic_or(&source_frs_max_requested, BIT(port));
+		} else {
+			return;
+		}
+	}
 
 	balance_source_ports();
 }
@@ -456,6 +532,22 @@ void dpm_remove_sink(int port)
 	balance_source_ports();
 }
 
+void dpm_remove_source(int port)
+{
+	if (CONFIG_USB_PD_3A_PORTS == 0)
+		return;
+
+	if (!IS_ENABLED(CONFIG_USB_PD_FRS))
+		return;
+
+	if (!(BIT(port) & source_frs_max_requested))
+		return;
+
+	atomic_clear_bits(&source_frs_max_requested, BIT(port));
+
+	balance_source_ports();
+}
+
 /*
  * Note: all ports receive the 1.5 A source offering until they are found to
  * match a criteria on the 3.0 A priority list (ex. though sink capability
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index 257ef52a65..8e2581348b 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -1124,48 +1124,6 @@ uint32_t pd_get_requested_current(int port)
 }
 
 /*
- * Evaluate a sink PDO for reported FRS support on the given port.
- *
- * If the requirements in the PDO are compatible with what we can supply,
- * FRS will be enabled on the port. If the provided PDO does not specify
- * FRS requirements (because it is not a fixed PDO) or PD 3.0 and FRS support
- * are not enabled, do nothing.
- */
-__maybe_unused static void pe_evaluate_frs_snk_pdo(int port, uint32_t pdo)
-{
-	if (!(IS_ENABLED(CONFIG_USB_PD_REV30) && IS_ENABLED(CONFIG_USB_PD_FRS)))
-		return;
-
-	if ((pdo & PDO_TYPE_MASK) != PDO_TYPE_FIXED) {
-		/*
-		 * PDO must be a fixed supply: either the caller chose the
-		 * wrong PDO or the partner is not compliant.
-		 */
-		CPRINTS("C%d: Sink PDO %x is not a fixed supply,"
-			" cannot support FRS", port, pdo);
-		return;
-	}
-	/*
-	 * TODO(b/14191267): Make sure we can handle the required current
-	 * before we enable FRS.
-	 */
-	if ((pdo & PDO_FIXED_DUAL_ROLE)) {
-		switch (pdo & PDO_FIXED_FRS_CURR_MASK) {
-		case PDO_FIXED_FRS_CURR_NOT_SUPPORTED:
-			break;
-		case PDO_FIXED_FRS_CURR_DFLT_USB_POWER:
-		case PDO_FIXED_FRS_CURR_1A5_AT_5V:
-		case PDO_FIXED_FRS_CURR_3A0_AT_5V:
-			CPRINTS("C%d: Partner FRS is OK: enabling PE support",
-				port);
-			typec_set_source_current_limit(port, TYPEC_RP_3A0);
-			pe_set_frs_enable(port, 1);
-			break;
-		}
-	}
-}
-
-/*
  * Determine if this port may communicate with the cable plug.
  *
  * In both PD 2.0 and 3.0 (2.5.4 SOP'/SOP'' Communication with Cable Plugs):
@@ -1493,6 +1451,7 @@ static void pe_handle_detach(void)
 	pe_set_snk_caps(port, 0, NULL);
 
 	dpm_remove_sink(port);
+	dpm_remove_source(port);
 
 	/* Exit BIST Test mode, in case the TCPC entered it. */
 	tcpc_set_bist_test_mode(port, false);
@@ -2196,6 +2155,21 @@ static void pe_src_startup_entry(int port)
 		/* Start SwapSourceStartTimer */
 		pe[port].swap_source_start_timer = get_time().val +
 			PD_T_SWAP_SOURCE_START;
+
+		/*
+		 * Evaluate port's sink caps for preferred current, if
+		 * already available
+		 */
+		if (pd_get_snk_cap_cnt(port) > 0)
+			dpm_evaluate_sink_fixed_pdo(port,
+						    *pd_get_snk_caps(port));
+
+		/*
+		 * Remove prior FRS claims to 3.0 A now that sink current has
+		 * been claimed, to avoid issues with lower priority ports
+		 * potentially receiving a 3.0 A claim between calls.
+		 */
+		dpm_remove_source(port);
 	} else {
 		/*
 		 * SwapSourceStartTimer delay is not needed, so trigger now.
@@ -2580,14 +2554,6 @@ static void pe_src_transition_supply_run(int port)
 			if (pd_get_src_cap_cnt(port) == 0)
 				pd_dpm_request(port, DPM_REQUEST_GET_SRC_CAPS);
 
-			/*
-			 * Evaluate port's sink caps for preferred current, if
-			 * already available
-			 */
-			if (pd_get_snk_cap_cnt(port) > 0)
-				dpm_evaluate_sink_fixed_pdo(port,
-							*pd_get_snk_caps(port));
-
 			set_state_pe(port, PE_SRC_READY);
 		} else {
 			/* NOTE: First pass through this code block */
@@ -3344,6 +3310,14 @@ static void pe_snk_transition_sink_run(int port)
 			if (tc_is_vconn_src(port))
 				tcpm_sop_prime_enable(port, true);
 
+			/*
+			 * Evaluate port's sink caps for FRS current, if
+			 * already available
+			 */
+			if (pd_get_snk_cap_cnt(port) > 0)
+				dpm_evaluate_sink_fixed_pdo(port,
+							*pd_get_snk_caps(port));
+
 			set_state_pe(port, PE_SNK_READY);
 			return;
 		}
@@ -3405,40 +3379,6 @@ static void pe_snk_ready_entry(int port)
 	}
 
 	/*
-	 * If port partner sink capabilities are known (because we requested
-	 * and got them earlier), evaluate them for FRS support and enable
-	 * if appropriate. If not known, request that we get them through DPM
-	 * which will eventually come back here with known capabilities. Don't
-	 * do anything if FRS is already enabled.
-	 */
-	if (IS_ENABLED(CONFIG_USB_PD_FRS) &&
-	    !PE_CHK_FLAG(port, PE_FLAGS_FAST_ROLE_SWAP_ENABLED)) {
-		if (pd_get_snk_cap_cnt(port) > 0) {
-			/*
-			 * Have partner sink caps. FRS support is only specified
-			 * in fixed PDOs, and "the vSafe5V Fixed Supply Object
-			 * Shall always be the first object" in a capabilities
-			 * message so take the first one.
-			 */
-			const uint32_t *snk_caps = pd_get_snk_caps(port);
-
-			pe_evaluate_frs_snk_pdo(port, snk_caps[0]);
-		} else {
-			/*
-			 * Don't have caps; request them. A sink port "shall
-			 * minimally offer one Power Data Object," so a
-			 * compliant partner that supports sink operation will
-			 * never fail to return sink capabilities in a way which
-			 * would cause us to endlessly request them. Non-DRPs
-			 * will never support FRS and may not support sink
-			 * operation, so avoid requesting caps from them.
-			 */
-			if (pd_is_port_partner_dualrole(port))
-				pd_dpm_request(port, DPM_REQUEST_GET_SNK_CAPS);
-		}
-	}
-
-	/*
 	 * Wait and add jitter if we are operating in PD2.0 mode and no messages
 	 * have been sent since enter this state.
 	 */
@@ -6570,9 +6510,7 @@ static void pe_dr_get_sink_cap_run(int port)
 
 				pe_set_snk_caps(port, cap_cnt, payload);
 
-				if (pe[port].power_role == PD_ROLE_SOURCE)
-					dpm_evaluate_sink_fixed_pdo(port,
-								payload[0]);
+				dpm_evaluate_sink_fixed_pdo(port, payload[0]);
 				pe_set_ready_state(port);
 				return;
 			} else if (cnt == 0 && (type == PD_CTRL_REJECT ||
diff --git a/include/usb_pd_dpm.h b/include/usb_pd_dpm.h
index d0e49a7383..aa9608f9b4 100644
--- a/include/usb_pd_dpm.h
+++ b/include/usb_pd_dpm.h
@@ -76,6 +76,14 @@ void dpm_evaluate_sink_fixed_pdo(int port, uint32_t vsafe5v_pdo);
 void dpm_remove_sink(int port);
 
 /*
+ * Remove this port as a source, and reallocate reserved FRS maximum current
+ * as needed.
+ *
+ * @param port		USB-C port number
+ */
+void dpm_remove_source(int port);
+
+/*
  * Return the appropriate Source Capability PDO to offer this port
  *
  * @param src_pdo	Will point to appropriate PDO to offer
diff --git a/test/fake_usbc.c b/test/fake_usbc.c
index cdbd41d03d..9eb913ea19 100644
--- a/test/fake_usbc.c
+++ b/test/fake_usbc.c
@@ -283,6 +283,10 @@ void dpm_remove_sink(int port)
 {
 }
 
+void dpm_remove_source(int port)
+{
+}
+
 int dpm_get_source_pdo(const uint32_t **src_pdo, const int port)
 {
 	*src_pdo = pd_src_pdo;