TCPMv2: DPM sourcing policy - 3.0 A PD sinks

Move the sourcing policy from the charge_manager to the DPM for TCPMv2.
The first step of this policy will be to allocate 3.0 A only if a
peripheral reports requiring more than 1.5 A in their Sink Capabilities
vSafe5V operational current.  For this commit, leave in some
charge_manager APIs for linking which will be re-named or removed later.

BRANCH=None
BUG=b:141690755,b:168862110
TEST=on drawcia verify:
- non-PD sink only offered 1.5 A Rp
- PD sink requiring 1.5 A or less Rp isn't offered 3.0 A
- PD sink requiring 3.0 A is offered a new 3.0 A Source Capability after
sink capability probing.  Port continues to receive 3.0 A over both hard
and soft resets.
- When 2 3.0 A sinks are plugged in, only the first is offered 3.0 A.
After the first is unplugged, the second receives a 3.0 A source
capability message

Signed-off-by: Diana Z <dzigterman@chromium.org>
Change-Id: Iec48312df1125086db2919c1503c7ba31fe12bcc
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2597429
Reviewed-by: Keith Short <keithshort@chromium.org>

6 files changed, 179 insertions, 2 deletions

diff --git a/common/charge_manager.c b/common/charge_manager.c
index eb6fd26595..476e5c9a36 100644
--- a/common/charge_manager.c
+++ b/common/charge_manager.c
@@ -1198,7 +1198,9 @@ int charge_manager_get_power_limit_uw(void)
 		return current_ma * voltage_mv;
 }
 
-#ifdef CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT
+#if defined(CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT) && \
+	!defined(CONFIG_USB_PD_TCPMV2)
+/* Note: this functionality is a part of the TCPMv2 Device Poicy Manager */
 
 /* Bitmap of ports used as power source */
 static volatile uint32_t source_port_bitmap;
@@ -1287,7 +1289,7 @@ int charge_manager_get_source_pdo(const uint32_t **src_pdo, const int port)
 	*src_pdo = pd_src_pdo;
 	return pd_src_pdo_cnt;
 }
-#endif /* CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT */
+#endif /* CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT && !CONFIG_USB_PD_TCPMV2 */
 
 #ifndef TEST_BUILD
 static enum ec_status hc_pd_power_info(struct host_cmd_handler_args *args)
diff --git a/common/mock/usb_pd_dpm_mock.c b/common/mock/usb_pd_dpm_mock.c
index ac45837971..a68f56ae3b 100644
--- a/common/mock/usb_pd_dpm_mock.c
+++ b/common/mock/usb_pd_dpm_mock.c
@@ -47,3 +47,11 @@ void dpm_set_mode_exit_request(int port)
 void dpm_run(int port)
 {
 }
+
+void dpm_evaluate_sink_fixed_pdo(int port, uint32_t vsafe5v_pdo)
+{
+}
+
+void dpm_remove_sink(int port)
+{
+}
diff --git a/common/usbc/usb_pd_dpm.c b/common/usbc/usb_pd_dpm.c
index a8bb242954..9748742b75 100644
--- a/common/usbc/usb_pd_dpm.c
+++ b/common/usbc/usb_pd_dpm.c
@@ -13,6 +13,7 @@
 #include "console.h"
 #include "ec_commands.h"
 #include "system.h"
+#include "task.h"
 #include "tcpm/tcpm.h"
 #include "usb_dp_alt_mode.h"
 #include "usb_mode.h"
@@ -342,3 +343,128 @@ void dpm_run(int port)
 	else if (!DPM_CHK_FLAG(port, DPM_FLAG_MODE_ENTRY_DONE))
 		dpm_attempt_mode_entry(port);
 }
+
+/*
+ * Source-out policy variables and APIs
+ *
+ * Priority for the available 3.0 A ports is given in the following order:
+ * - sink partners which report requiring > 1.5 A in their Sink_Capabilities
+ */
+
+/*
+ * Bitmasks of port numbers in each following category
+ *
+ * Note: request bitmasks should be accessed atomically as other ports may alter
+ * them
+ */
+static uint32_t		max_current_claimed;
+static struct mutex	max_current_claimed_lock;
+
+static uint32_t sink_max_pdo_requested;	/* Ports with PD sink needing > 1.5A */
+
+#define LOWEST_PORT(p) __builtin_ctz(p)  /* Undefined behavior if p == 0 */
+
+static int count_port_bits(uint32_t bitmask)
+{
+	int i, total = 0;
+
+	for (i = 0; i < board_get_usb_pd_port_count(); i++) {
+		if (bitmask & BIT(i))
+			total++;
+	}
+
+	return total;
+}
+
+/* TODO(b/141690755): Move to config.h */
+#define CONFIG_USB_PD_3A_PORTS	1
+
+/*
+ * Centralized, mutex-controlled updates to the claimed 3.0 A ports
+ */
+static void balance_source_ports(void)
+{
+	uint32_t removed_ports, new_ports;
+
+	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;
+	max_current_claimed &= ~removed_ports;
+
+	/* Allocate 3.0 A to new PD sink ports that need it */
+	new_ports = sink_max_pdo_requested & ~max_current_claimed;
+	while (new_ports) {
+		int new_max_port = LOWEST_PORT(new_ports);
+
+		if (count_port_bits(max_current_claimed) <
+						CONFIG_USB_PD_3A_PORTS) {
+			max_current_claimed |= BIT(new_max_port);
+			typec_set_source_current_limit(new_max_port,
+						       TYPEC_RP_3A0);
+			typec_select_src_current_limit_rp(new_max_port,
+							  TYPEC_RP_3A0);
+			pd_update_contract(new_max_port);
+
+		} else {
+			/* TODO(b/141690755): Check lower priority claims */
+			goto unlock;
+		}
+		new_ports &= ~BIT(new_max_port);
+	}
+
+unlock:
+	mutex_unlock(&max_current_claimed_lock);
+}
+
+/* Process sink's first Sink_Capabilities PDO for port current consideration */
+void dpm_evaluate_sink_fixed_pdo(int port, uint32_t vsafe5v_pdo)
+{
+	/* Verify partner supplied valid vSafe5V fixed object first */
+	if ((vsafe5v_pdo & PDO_TYPE_MASK) != PDO_TYPE_FIXED)
+		return;
+
+	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;
+
+	atomic_or(&sink_max_pdo_requested, BIT(port));
+
+	balance_source_ports();
+}
+
+void dpm_remove_sink(int port)
+{
+	if (!(BIT(port) & sink_max_pdo_requested))
+		return;
+
+	atomic_clear_bits(&sink_max_pdo_requested, BIT(port));
+
+	balance_source_ports();
+}
+
+#if defined(CONFIG_USB_PD_DYNAMIC_SRC_CAP) || \
+		defined(CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT)
+/*
+ * Charge manager APIs
+ * TODO(b/141690755): Remove and replace with DPM calls
+ */
+void charge_manager_source_port(int port, int enable)
+{
+	/* No-op present for linking */
+}
+
+int charge_manager_get_source_pdo(const uint32_t **src_pdo, const int port)
+{
+	if (max_current_claimed & BIT(port)) {
+		*src_pdo = pd_src_pdo_max;
+		return pd_src_pdo_max_cnt;
+	}
+
+	*src_pdo = pd_src_pdo;
+	return pd_src_pdo_cnt;
+}
+#endif
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index 1cb44bf164..2a116f0cf4 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -1423,6 +1423,8 @@ static void pe_handle_detach(void)
 	 */
 	pd_set_src_caps(port, 0, NULL);
 	pe_set_snk_caps(port, 0, NULL);
+
+	dpm_remove_sink(port);
 }
 DECLARE_HOOK(HOOK_USB_PD_DISCONNECT, pe_handle_detach, HOOK_PRIO_DEFAULT);
 
@@ -2302,6 +2304,14 @@ 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 */
@@ -2792,6 +2802,8 @@ static void pe_snk_startup_entry(int port)
 		 */
 		if (tc_is_vconn_src(port))
 			tcpm_sop_prime_enable(port, false);
+
+		dpm_remove_sink(port);
 	} else {
 		/*
 		 * Set DiscoverIdentityTimer to trigger when we enter
@@ -6300,6 +6312,10 @@ static void pe_dr_get_sink_cap_run(int port)
 							sizeof(uint32_t);
 
 				pe_set_snk_caps(port, cap_cnt, payload);
+
+				if (pe[port].power_role == PD_ROLE_SOURCE)
+					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 ccc7c57ff7..29253f4031 100644
--- a/include/usb_pd_dpm.h
+++ b/include/usb_pd_dpm.h
@@ -58,4 +58,21 @@ void dpm_vdm_naked(int port, enum tcpm_transmit_type type, uint16_t svid,
  */
 void dpm_run(int port);
 
+/*
+ * Determines the current allocation for the connection, past the basic
+ * CONFIG_USB_PD_PULLUP value set by the TC (generally 1.5 A)
+ *
+ * @param port		USB-C port number
+ * @param vsafe5v_pdo	Copy of first Sink_Capability PDO, which should
+ *			represent the vSafe5V fixed PDO
+ */
+void dpm_evaluate_sink_fixed_pdo(int port, uint32_t vsafe5v_pdo);
+
+/*
+ * Remove this port as a sink, and reallocate maximum current as needed.
+ *
+ * @param port		USB-C port number
+ */
+void dpm_remove_sink(int port);
+
 #endif  /* __CROS_EC_USB_DPM_H */
diff --git a/test/fake_usbc.c b/test/fake_usbc.c
index 3f84f26219..8343b9a944 100644
--- a/test/fake_usbc.c
+++ b/test/fake_usbc.c
@@ -275,6 +275,14 @@ void dpm_run(int port)
 {
 }
 
+void dpm_evaluate_sink_fixed_pdo(int port, uint32_t vsafe5v_pdo)
+{
+}
+
+void dpm_remove_sink(int port)
+{
+}
+
 static enum tcpc_rp_value lcl_rp;
 __overridable void typec_select_src_current_limit_rp(int port,
 						  enum tcpc_rp_value rp)