charge_manager: Prefer current active port in best port selection

If two identical suppliers are capable of supplying equal power, select
the port which is currently active.

BUG=chrome-os-partner:34912
TEST=Manual on Samus. Plug Zinger into right port, verify that it
becomes active. Plug a new Zinger into left port, verify that the right
port stays active.
BRANCH=Samus

Change-Id: Ib1baf4bd3f619169f0e31ec509a2fe7dbd8c897e
Signed-off-by: Shawn Nematbakhsh <shawnn@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/238766
Reviewed-by: Alec Berg <alecaberg@chromium.org>

2 files changed, 39 insertions, 5 deletions

diff --git a/common/charge_manager.c b/common/charge_manager.c
index 1622997f02..9a9629407f 100644
--- a/common/charge_manager.c
+++ b/common/charge_manager.c
@@ -110,6 +110,7 @@ static void charge_manager_get_best_charge_port(int *new_port,
 {
 	int supplier = CHARGE_SUPPLIER_NONE;
 	int port = CHARGE_PORT_NONE;
+	int best_port_power, candidate_port_power;
 	int i, j;
 
 	/* Skip port selection on OVERRIDE_DONT_CHARGE. */
@@ -118,12 +119,22 @@ static void charge_manager_get_best_charge_port(int *new_port,
 		 * Charge supplier selection logic:
 		 * 1. Prefer higher priority supply.
 		 * 2. Prefer higher power over lower in case priority is tied.
+		 * 3. Prefer current charge port over new port in case (1)
+		 *    and (2) are tied.
 		 * available_charge can be changed at any time by other tasks,
 		 * so make no assumptions about its consistency.
 		 */
 		for (i = 0; i < CHARGE_SUPPLIER_COUNT; ++i)
 			for (j = 0; j < PD_PORT_COUNT; ++j) {
 				/*
+				 * Skip this supplier if there is no
+				 * available charge.
+				 */
+				if (available_charge[i][j].current == 0 ||
+				    available_charge[i][j].voltage == 0)
+					continue;
+
+				/*
 				 * Don't select this port if we have a
 				 * charge on another override port.
 				 */
@@ -140,19 +151,31 @@ static void charge_manager_get_best_charge_port(int *new_port,
 				    override_port != j)
 					continue;
 
-				if (available_charge[i][j].current > 0 &&
-				    available_charge[i][j].voltage > 0 &&
-				   (supplier == CHARGE_SUPPLIER_NONE ||
+				candidate_port_power =
+					POWER(available_charge[i][j]);
+
+				/* Select if no supplier chosen yet. */
+				if (supplier == CHARGE_SUPPLIER_NONE ||
+				/* ..or if supplier priority is higher. */
 				    supplier_priority[i] <
 				    supplier_priority[supplier] ||
+				/* ..or if this is our override port. */
 				   (j == override_port &&
 				    port != override_port) ||
+				/* ..or if priority is tied and.. */
 				   (supplier_priority[i] ==
 				    supplier_priority[supplier] &&
-				    POWER(available_charge[i][j]) >
-				    POWER(available_charge[supplier][port])))) {
+				/* candidate port can supply more power or.. */
+				   (candidate_port_power > best_port_power ||
+				/*
+				 * candidate port is the active port and can
+				 * supply the same amount of power.
+				 */
+				   (candidate_port_power == best_port_power &&
+				    charge_port == j)))) {
 					supplier = i;
 					port = j;
+					best_port_power = candidate_port_power;
 				}
 			}
 
diff --git a/test/charge_manager.c b/test/charge_manager.c
index db81105ff6..b0c9423632 100644
--- a/test/charge_manager.c
+++ b/test/charge_manager.c
@@ -228,6 +228,17 @@ static int test_priority(void)
 	TEST_ASSERT(active_charge_port == 1);
 	TEST_ASSERT(active_charge_limit == 3000);
 
+	/*
+	 * Add a charge at equal priority and equal power, verify that the
+	 * active port doesn't change since the first plugged port is
+	 * selected as the tiebreaker.
+	 */
+	charge.current = 3000;
+	charge_manager_update(CHARGE_SUPPLIER_TEST6, 0, &charge);
+	wait_for_charge_manager_refresh();
+	TEST_ASSERT(active_charge_port == 1);
+	TEST_ASSERT(active_charge_limit == 3000);
+
 	return EC_SUCCESS;
 }