pd: add PDO selection for choosing highest voltage

pyro board is using 3S1P battery, it is near 13V.
To avoid the charger entering buck-boost mode, sinking as higher voltage
to get the higher power efficiency.

BRANCH=none
BUG=chrome-os-partner:59276
TEST=plug in pyro's charger (offering 20V@2.25A, 15V@3A, 9V@3A and 5V@3A),
see it selecting 20V as below:
C1 st2
C1 st3
C1 st15
C1 st3
C1 st6
Req C1 [1] 5000mV 3000mA
[6391.078044 New chg p1]
[6391.081020 Ramp reset: st1]
[6391.081664 CL: p1 s0 i500 v20000]
C1 st7
C1 st8
C1 st9
[6391.192437 Ramp reset: st1]
[6391.193339 CL: p1 s0 i3000 v5000]
Req C1 [4] 20000mV 2250mA
C1 st7
C1 st8
C1 st9
[6391.457545 Ramp reset: st1]
[6391.458340 CL: p1 s0 i2250 v20000]
C1 st27
C1 st9
[6392.081252 AC on]
[6392.113477 charge_request(0mV, 0mA)]
[6393.116998 Ramp p1 st5 2250mA 2250mA]

plug in other charger (offering 15V@3A, 12V@3A and 5V@3A),
see it selecting 15V as below:
C1 st2
C1 st3
C1 st15
C1 st3
C1 st6
Req C1 [1] 5000mV 3000mA
[6636.084963 New chg p1]
[6636.087117 Ramp reset: st1]
[6636.087786 CL: p1 s0 i500 v15000]
C1 st7
C1 st8
C1 st9
[6636.165409 Ramp reset: st1]
[6636.166314 CL: p1 s0 i3000 v5000]
Req C1 [3] 15000mV 3000mA
C1 st7
C1 st8
C1 st9
C1 st27
C1 st9
[6637.092559 AC on]
[6637.125043 charge_request(0mV, 0mA)]
[6638.091158 Ramp p1 st5 3000mA 3000mA]
[6639.374815 charge_request(13040mV, 3712mA)]

Change-Id: I74fe4cbd6b9d1b416a94eec3fe944a9b725f0ced
Signed-off-by: Devin Lu <Devin.Lu@quantatw.com>
Reviewed-on: https://chromium-review.googlesource.com/411621
Commit-Ready: Shawn N <shawnn@chromium.org>
Reviewed-by: Shawn N <shawnn@chromium.org>

1 files changed, 18 insertions, 13 deletions

diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index 5a37a943e3..1fa53d5695 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -95,11 +95,11 @@ static unsigned max_request_mv = PD_MAX_VOLTAGE_MV; /* no cap */
  */
 static int pd_find_pdo_index(int cnt, uint32_t *src_caps, int max_mv)
 {
-	int i, uw, max_uw = 0, mv, ma;
+	int i, uw, mv, ma;
 	int ret = -1;
-#ifdef PD_PREFER_LOW_VOLTAGE
-	int cur_mv = 0;
-#endif
+	int __attribute__((unused)) cur_mv = 0;
+	int cur_uw = 0;
+	int prefer_cur;
 
 	/* max voltage is always limited by this boards max request */
 	max_mv = MIN(max_mv, PD_MAX_VOLTAGE_MV);
@@ -118,21 +118,26 @@ static int pd_find_pdo_index(int cnt, uint32_t *src_caps, int max_mv)
 			ma = MIN(ma, PD_MAX_CURRENT_MA);
 			uw = ma * mv;
 		}
-#ifdef PD_PREFER_LOW_VOLTAGE
+
 		if (mv > max_mv)
 			continue;
 		uw = MIN(uw, PD_MAX_POWER_MW * 1000);
-		if ((uw > max_uw) || ((uw == max_uw) && mv < cur_mv)) {
+		prefer_cur = 0;
+
+		/* Apply special rules in case of 'tie' */
+#ifdef PD_PREFER_LOW_VOLTAGE
+		if (uw == cur_uw && mv < cur_mv)
+			prefer_cur = 1;
+#elif defined(PD_PREFER_HIGH_VOLTAGE)
+		if (uw == cur_uw && mv > cur_mv)
+			prefer_cur = 1;
+#endif
+		/* Prefer higher power, except for tiebreaker */
+		if (uw > cur_uw || prefer_cur) {
 			ret = i;
-			max_uw = uw;
+			cur_uw = uw;
 			cur_mv = mv;
 		}
-#else
-		if ((uw > max_uw) && (mv <= max_mv)) {
-			ret = i;
-			max_uw = uw;
-		}
-#endif
 	}
 	return ret;
 }