pd: refactor pd policy layer request voltage functions

Remove common code across all PD policy layers to select the requested
voltage and build a Request Data Object (RDO).

BUG=none
BRANCH=samus
TEST=Load onto samus and connect zinger. Make sure we request the right
voltage (first 5V, then after initial contract is made, 20V). Make
sure input current limit is set appropriately by checking limit on EC
console using charger command.

Change-Id: Ic6bda5e23b2d7b7d710ffdf085e7fbc1b0c3add9
Signed-off-by: Alec Berg <alecaberg@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/233673
Reviewed-by: Vincent Palatin <vpalatin@chromium.org>
Commit-Queue: Shawn Nematbakhsh <shawnn@chromium.org>
Tested-by: Shawn Nematbakhsh <shawnn@chromium.org>

24 files changed, 239 insertions, 702 deletions

diff --git a/board/dingdong/usb_pd_config.h b/board/dingdong/usb_pd_config.h
index 3af9e0a189..ba6e545a3f 100644
--- a/board/dingdong/usb_pd_config.h
+++ b/board/dingdong/usb_pd_config.h
@@ -139,4 +139,11 @@ static inline int pd_snk_is_vbus_provided(int port)
 
 /* we are never a source : don't care about power supply */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 0 /* us */
+
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 1000
+#define PD_MAX_POWER_MW       1500
+#define PD_MAX_CURRENT_MA     300
+#define PD_MAX_VOLTAGE_MV     5000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/dingdong/usb_pd_policy.c b/board/dingdong/usb_pd_policy.c
index 2a3c027e61..29c90282e9 100644
--- a/board/dingdong/usb_pd_policy.c
+++ b/board/dingdong/usb_pd_policy.c
@@ -27,65 +27,17 @@
 const uint32_t pd_src_pdo[] = {};
 const int pd_src_pdo_cnt = ARRAY_SIZE(pd_src_pdo);
 
-/* Define typical operating power and max power */
-#define OPERATING_POWER_MW 1000
-#define MAX_POWER_MW       1500
-#define MAX_CURRENT_MA     300
-
 /* Fake PDOs : we just want our pre-defined voltages */
 const uint32_t pd_snk_pdo[] = {
 		PDO_FIXED(5000,   500, PDO_FIXED_FLAGS),
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Desired voltage requested as a sink (in millivolts) */
-static unsigned select_mv = 5000;
-
 /* Whether alternate mode has been entered or not */
 static int alt_mode;
 /* When set true, we are in GFU mode */
 static int gfu_mode;
 
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	int i;
-	int ma;
-	int set_mv = select_mv;
-	int max;
-	uint32_t flags;
-
-	/* Default to 5V */
-	if (set_mv <= 0)
-		set_mv = 5000;
-
-	/* Get the selected voltage */
-	for (i = cnt; i >= 0; i--) {
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		int type = src_caps[i] & PDO_TYPE_MASK;
-		if ((mv == set_mv) && (type == PDO_TYPE_FIXED))
-			break;
-	}
-	if (i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* build rdo for desired power */
-	ma = 10 * (src_caps[i] & 0x3FF);
-	max = MIN(ma, MAX_CURRENT_MA);
-	flags = (max * set_mv) < (1000 * OPERATING_POWER_MW) ?
-			RDO_CAP_MISMATCH : 0;
-	*rdo = RDO_FIXED(i + 1, max, max, 0);
-	CPRINTF("Request [%d] %dV %dmA", i, set_mv/1000, max);
-	/* Mismatch bit set if less power offered than the operating power */
-	if (flags & RDO_CAP_MISMATCH)
-		CPRINTF(" Mismatch");
-	CPRINTF("\n");
-
-	*curr_limit = max;
-	*supply_voltage = set_mv;
-	return EC_SUCCESS;
-}
-
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
 {
@@ -93,11 +45,6 @@ void pd_set_input_current_limit(int port, uint32_t max_ma,
 	return;
 }
 
-void pd_set_max_voltage(unsigned mv)
-{
-	select_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	return EC_SUCCESS;
diff --git a/board/firefly/usb_pd_config.h b/board/firefly/usb_pd_config.h
index 77436cb74a..36a05f330c 100644
--- a/board/firefly/usb_pd_config.h
+++ b/board/firefly/usb_pd_config.h
@@ -151,4 +151,10 @@ static inline int pd_snk_is_vbus_provided(int port)
 /* we are never a source : don't care about power supply */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 0
 
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 1000
+#define PD_MAX_POWER_MW       1500
+#define PD_MAX_CURRENT_MA     300
+#define PD_MAX_VOLTAGE_MV     20000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/firefly/usb_pd_policy.c b/board/firefly/usb_pd_policy.c
index 07b8e7745d..412ca105ed 100644
--- a/board/firefly/usb_pd_policy.c
+++ b/board/firefly/usb_pd_policy.c
@@ -21,11 +21,6 @@
 /* Acceptable margin between requested VBUS and measured value */
 #define MARGIN_MV 400 /* mV */
 
-/* Define typical operating power and max power */
-#define OPERATING_POWER_MW 1000
-#define MAX_POWER_MW       1500
-#define MAX_CURRENT_MA     300
-
 #define PDO_FIXED_FLAGS (PDO_FIXED_EXTERNAL)
 
 /* we are not acting as a source */
@@ -42,49 +37,6 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Desired voltage requested as a sink (in millivolts) */
-static unsigned select_mv = 20000;
-
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	int i;
-	int ma;
-	int set_mv = select_mv;
-	int max;
-	uint32_t flags;
-
-	/* Default to 5V */
-	if (set_mv <= 0)
-		set_mv = 5000;
-
-	/* Get the selected voltage */
-	for (i = cnt; i >= 0; i--) {
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		int type = src_caps[i] & PDO_TYPE_MASK;
-		if ((mv == set_mv) && (type == PDO_TYPE_FIXED))
-			break;
-	}
-	if (i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* build rdo for desired power */
-	ma = 10 * (src_caps[i] & 0x3FF);
-	max = MIN(ma, MAX_CURRENT_MA);
-	flags = (max * set_mv) < (1000 * OPERATING_POWER_MW) ?
-			RDO_CAP_MISMATCH : 0;
-	*rdo = RDO_FIXED(i + 1, max, max, 0);
-	CPRINTF("Request [%d] %dV %dmA", i, set_mv/1000, max);
-	/* Mismatch bit set if less power offered than the operating power */
-	if (flags & RDO_CAP_MISMATCH)
-		CPRINTF(" Mismatch");
-	CPRINTF("\n");
-
-	*curr_limit = max;
-	*supply_voltage = set_mv;
-	return EC_SUCCESS;
-}
-
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
 {
@@ -92,11 +44,6 @@ void pd_set_input_current_limit(int port, uint32_t max_ma,
 	return;
 }
 
-void pd_set_max_voltage(unsigned mv)
-{
-	select_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	/* Never acting as a source */
@@ -123,6 +70,7 @@ int pd_board_checks(void)
 	static int blinking;
 	int vbus;
 	int led5 = 0, led12 = 0, led20 = 0;
+	unsigned select_mv = pd_get_max_voltage();
 
 	/* LED blinking state for the default indicator */
 	blinking = (blinking + 1) & 3;
diff --git a/board/fruitpie/usb_pd_config.h b/board/fruitpie/usb_pd_config.h
index 3851e9a263..069829e196 100644
--- a/board/fruitpie/usb_pd_config.h
+++ b/board/fruitpie/usb_pd_config.h
@@ -164,4 +164,10 @@ static inline int pd_snk_is_vbus_provided(int port)
 /* delay necessary for the voltage transition on the power supply */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 50000 /* us */
 
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 1000
+#define PD_MAX_POWER_MW       60000
+#define PD_MAX_CURRENT_MA     3000
+#define PD_MAX_VOLTAGE_MV     20000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/fruitpie/usb_pd_policy.c b/board/fruitpie/usb_pd_policy.c
index 976c43edc2..8844f16014 100644
--- a/board/fruitpie/usb_pd_policy.c
+++ b/board/fruitpie/usb_pd_policy.c
@@ -34,56 +34,6 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Cap on the max voltage requested as a sink (in millivolts) */
-static unsigned max_mv = -1; /* no cap */
-
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	int i;
-	int sel_mv;
-	int max_uw = 0;
-	int max_ma;
-	int max_i = -1;
-
-	/* Get max power */
-	for (i = 0; i < cnt; i++) {
-		int uw;
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		if ((src_caps[i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-			uw = 250000 * (src_caps[i] & 0x3FF);
-		} else {
-			int ma = (src_caps[i] & 0x3FF) * 10;
-			uw = ma * mv;
-		}
-		if ((uw > max_uw) && (mv <= max_mv)) {
-			max_i = i;
-			max_uw = uw;
-			sel_mv = mv;
-		}
-	}
-	if (max_i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* request all the power ... */
-	if ((src_caps[max_i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-		int uw = 250000 * (src_caps[max_i] & 0x3FF);
-		max_ma = uw / sel_mv;
-		*rdo = RDO_BATT(max_i + 1, uw/2, uw, 0);
-		CPRINTF("Request [%d] %dV %dmW\n",
-			 max_i, sel_mv/1000, uw/1000);
-	} else {
-		int ma = 10 * (src_caps[max_i] & 0x3FF);
-		max_ma = ma;
-		*rdo = RDO_FIXED(max_i + 1, ma / 2, ma, 0);
-		CPRINTF("Request [%d] %dV %dmA\n",
-			 max_i, sel_mv/1000, ma);
-	}
-	*curr_limit = max_ma;
-	*supply_voltage = sel_mv;
-	return EC_SUCCESS;
-}
-
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
 {
@@ -93,11 +43,6 @@ void pd_set_input_current_limit(int port, uint32_t max_ma,
 		CPRINTS("Failed to set input current limit for PD");
 }
 
-void pd_set_max_voltage(unsigned mv)
-{
-	max_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	int max_ma = rdo & 0x3FF;
diff --git a/board/hoho/usb_pd_config.h b/board/hoho/usb_pd_config.h
index 3af9e0a189..ba6e545a3f 100644
--- a/board/hoho/usb_pd_config.h
+++ b/board/hoho/usb_pd_config.h
@@ -139,4 +139,11 @@ static inline int pd_snk_is_vbus_provided(int port)
 
 /* we are never a source : don't care about power supply */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 0 /* us */
+
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 1000
+#define PD_MAX_POWER_MW       1500
+#define PD_MAX_CURRENT_MA     300
+#define PD_MAX_VOLTAGE_MV     5000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/hoho/usb_pd_policy.c b/board/hoho/usb_pd_policy.c
index c9525f2ebe..195aaacb25 100644
--- a/board/hoho/usb_pd_policy.c
+++ b/board/hoho/usb_pd_policy.c
@@ -27,65 +27,17 @@
 const uint32_t pd_src_pdo[] = {};
 const int pd_src_pdo_cnt = ARRAY_SIZE(pd_src_pdo);
 
-/* Define typical operating power and max power */
-#define OPERATING_POWER_MW 1000
-#define MAX_POWER_MW       1500
-#define MAX_CURRENT_MA     300
-
 /* Fake PDOs : we just want our pre-defined voltages */
 const uint32_t pd_snk_pdo[] = {
 		PDO_FIXED(5000, 500, PDO_FIXED_FLAGS),
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Desired voltage requested as a sink (in millivolts) */
-static unsigned select_mv = 5000;
-
 /* Whether alternate mode has been entered or not */
 static int alt_mode;
 /* When set true, we are in GFU mode */
 static int gfu_mode;
 
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	int i;
-	int ma;
-	int set_mv = select_mv;
-	int max;
-	uint32_t flags;
-
-	/* Default to 5V */
-	if (set_mv <= 0)
-		set_mv = 5000;
-
-	/* Get the selected voltage */
-	for (i = cnt; i >= 0; i--) {
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		int type = src_caps[i] & PDO_TYPE_MASK;
-		if ((mv == set_mv) && (type == PDO_TYPE_FIXED))
-			break;
-	}
-	if (i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* build rdo for desired power */
-	ma = 10 * (src_caps[i] & 0x3FF);
-	max = MIN(ma, MAX_CURRENT_MA);
-	flags = (max * set_mv) < (1000 * OPERATING_POWER_MW) ?
-			RDO_CAP_MISMATCH : 0;
-	*rdo = RDO_FIXED(i + 1, max, max, 0);
-	CPRINTF("Request [%d] %dV %dmA", i, set_mv/1000, max);
-	/* Mismatch bit set if less power offered than the operating power */
-	if (flags & RDO_CAP_MISMATCH)
-		CPRINTF(" Mismatch");
-	CPRINTF("\n");
-
-	*curr_limit = max;
-	*supply_voltage = set_mv;
-	return EC_SUCCESS;
-}
-
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
 {
@@ -93,11 +45,6 @@ void pd_set_input_current_limit(int port, uint32_t max_ma,
 	return;
 }
 
-void pd_set_max_voltage(unsigned mv)
-{
-	select_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	return EC_SUCCESS;
diff --git a/board/host/usb_pd_config.h b/board/host/usb_pd_config.h
index d9eb607874..0e6fe65ac4 100644
--- a/board/host/usb_pd_config.h
+++ b/board/host/usb_pd_config.h
@@ -38,4 +38,10 @@ int pd_snk_is_vbus_provided(int port);
 /* delay necessary for the voltage transition on the power supply */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 20000 /* us */
 
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 15000
+#define PD_MAX_POWER_MW       60000
+#define PD_MAX_CURRENT_MA     3000
+#define PD_MAX_VOLTAGE_MV     20000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/host/usb_pd_policy.c b/board/host/usb_pd_policy.c
index 7b7a931d86..f477115e6f 100644
--- a/board/host/usb_pd_policy.c
+++ b/board/host/usb_pd_policy.c
@@ -11,11 +11,6 @@
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
 
-/* Define typical operating power and max power */
-#define OPERATING_POWER_MW 15000
-#define MAX_POWER_MW       60000
-#define MAX_CURRENT_MA     3000
-
 #define PDO_FIXED_FLAGS (PDO_FIXED_DUAL_ROLE | PDO_FIXED_DATA_SWAP)
 
 const uint32_t pd_src_pdo[] = {
@@ -30,73 +25,6 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Cap on the max voltage requested as a sink (in millivolts) */
-static unsigned max_mv = -1; /* no cap */
-
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	int i;
-	int sel_mv;
-	int max_uw = 0;
-	int max_ma;
-	int max_i = -1;
-	int max;
-	uint32_t flags;
-
-	/* Get max power */
-	for (i = 0; i < cnt; i++) {
-		int uw;
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		if ((src_caps[i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-			uw = 250000 * (src_caps[i] & 0x3FF);
-		} else {
-			int ma = (src_caps[i] & 0x3FF) * 10;
-			uw = ma * mv;
-		}
-		if ((uw > max_uw) && (mv <= max_mv)) {
-			max_i = i;
-			max_uw = uw;
-			sel_mv = mv;
-		}
-	}
-	if (max_i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* build rdo for desired power */
-	if ((src_caps[max_i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-		int uw = 250000 * (src_caps[max_i] & 0x3FF);
-		max = MIN(1000 * uw, MAX_POWER_MW);
-		flags = (max < OPERATING_POWER_MW) ? RDO_CAP_MISMATCH : 0;
-		max_ma = 1000 * max / sel_mv;
-		*rdo = RDO_BATT(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmW",
-			max_i, sel_mv/1000, max);
-	} else {
-		int ma = 10 * (src_caps[max_i] & 0x3FF);
-		max = MIN(ma, MAX_CURRENT_MA);
-		flags = (max * sel_mv) < (1000 * OPERATING_POWER_MW) ?
-				RDO_CAP_MISMATCH : 0;
-		max_ma = max;
-		*rdo = RDO_FIXED(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmA",
-			max_i, sel_mv/1000, max);
-	}
-	/* Mismatch bit set if less power offered than the operating power */
-	if (flags & RDO_CAP_MISMATCH)
-		CPRINTF(" Mismatch");
-	CPRINTF("\n");
-
-	*curr_limit = max_ma;
-	*supply_voltage = sel_mv;
-	return EC_SUCCESS;
-}
-
-void pd_set_max_voltage(unsigned mv)
-{
-	max_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	int max_ma = rdo & 0x3FF;
diff --git a/board/plankton/usb_pd_config.h b/board/plankton/usb_pd_config.h
index 08f6e00452..d3eb0429b7 100644
--- a/board/plankton/usb_pd_config.h
+++ b/board/plankton/usb_pd_config.h
@@ -179,4 +179,10 @@ static inline int pd_snk_is_vbus_provided(int port)
 /* delay necessary for the voltage transition on the power supply */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 50000 /* us */
 
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 5000
+#define PD_MAX_POWER_MW       60000
+#define PD_MAX_CURRENT_MA     3000
+#define PD_MAX_VOLTAGE_MV     20000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/plankton/usb_pd_policy.c b/board/plankton/usb_pd_policy.c
index 08d2dd4df3..286e69229c 100644
--- a/board/plankton/usb_pd_policy.c
+++ b/board/plankton/usb_pd_policy.c
@@ -21,11 +21,6 @@
 /* Acceptable margin between requested VBUS and measured value */
 #define MARGIN_MV 400 /* mV */
 
-/* Define typical operating power and max power */
-#define OPERATING_POWER_MW 5000
-#define MAX_POWER_MW       60000
-#define MAX_CURRENT_MA     3000
-
 #define PDO_FIXED_FLAGS (PDO_FIXED_DATA_SWAP | PDO_FIXED_EXTERNAL)
 
 /* Source PDOs */
@@ -50,9 +45,6 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Desired voltage requested as a sink (in millivolts) */
-static unsigned select_mv = 5000;
-
 void board_set_source_cap(enum board_src_cap cap)
 {
 	pd_src_pdo_idx = cap;
@@ -64,46 +56,6 @@ int pd_get_source_pdo(const uint32_t **src_pdo)
 	return pd_src_pdo_cnts[pd_src_pdo_idx];
 }
 
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	int i;
-	int ma;
-	int set_mv = select_mv;
-	int max;
-	uint32_t flags;
-
-	/* Default to 5V */
-	if (set_mv <= 0)
-		set_mv = 5000;
-
-	/* Get the selected voltage */
-	for (i = cnt; i >= 0; i--) {
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		int type = src_caps[i] & PDO_TYPE_MASK;
-		if ((mv == set_mv) && (type == PDO_TYPE_FIXED))
-			break;
-	}
-	if (i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* build rdo for desired power */
-	ma = 10 * (src_caps[i] & 0x3FF);
-	max = MIN(ma, MAX_CURRENT_MA);
-	flags = (max * set_mv) < (1000 * OPERATING_POWER_MW) ?
-			RDO_CAP_MISMATCH : 0;
-	*rdo = RDO_FIXED(i + 1, max, max, 0);
-	CPRINTF("Request [%d] %dV %dmA", i, set_mv/1000, max);
-	/* Mismatch bit set if less power offered than the operating power */
-	if (flags & RDO_CAP_MISMATCH)
-		CPRINTF(" Mismatch");
-	CPRINTF("\n");
-
-	*curr_limit = max;
-	*supply_voltage = set_mv;
-	return EC_SUCCESS;
-}
-
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
 {
@@ -111,11 +63,6 @@ void pd_set_input_current_limit(int port, uint32_t max_ma,
 	return;
 }
 
-void pd_set_max_voltage(unsigned mv)
-{
-	select_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	int max_ma = rdo & 0x3FF;
diff --git a/board/ryu/usb_pd_config.h b/board/ryu/usb_pd_config.h
index 374cb083d7..994759163b 100644
--- a/board/ryu/usb_pd_config.h
+++ b/board/ryu/usb_pd_config.h
@@ -173,4 +173,10 @@ static inline int pd_snk_is_vbus_provided(int port)
 /* delay for the voltage transition on the power supply, chip max is 16us */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 20000 /* us */
 
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 10000
+#define PD_MAX_POWER_MW       60000
+#define PD_MAX_CURRENT_MA     3000
+#define PD_MAX_VOLTAGE_MV     20000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/ryu/usb_pd_policy.c b/board/ryu/usb_pd_policy.c
index 9bafd2435f..8c5b1aa85a 100644
--- a/board/ryu/usb_pd_policy.c
+++ b/board/ryu/usb_pd_policy.c
@@ -18,11 +18,6 @@
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
 
-/* Define typical operating power and max power */
-#define OPERATING_POWER_MW 10000
-#define MAX_POWER_MW       60000
-#define MAX_CURRENT_MA     3000
-
 #define PDO_FIXED_FLAGS (PDO_FIXED_DUAL_ROLE | PDO_FIXED_DATA_SWAP)
 
 const uint32_t pd_src_pdo[] = {
@@ -37,68 +32,6 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Cap on the max voltage requested as a sink (in millivolts) */
-static unsigned max_mv = -1; /* no cap */
-
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	int i;
-	int sel_mv;
-	int max_uw = 0;
-	int max_ma;
-	int max_i = -1;
-	int max;
-	uint32_t flags;
-
-	/* Get max power */
-	for (i = 0; i < cnt; i++) {
-		int uw;
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		if ((src_caps[i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-			uw = 250000 * (src_caps[i] & 0x3FF);
-		} else {
-			int ma = (src_caps[i] & 0x3FF) * 10;
-			uw = ma * mv;
-		}
-		if ((uw > max_uw) && (mv <= max_mv)) {
-			max_i = i;
-			max_uw = uw;
-			sel_mv = mv;
-		}
-	}
-	if (max_i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* build rdo for desired power */
-	if ((src_caps[max_i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-		int uw = 250000 * (src_caps[max_i] & 0x3FF);
-		max = MIN(1000 * uw, MAX_POWER_MW);
-		flags = (max < OPERATING_POWER_MW) ? RDO_CAP_MISMATCH : 0;
-		max_ma = 1000 * max / sel_mv;
-		*rdo = RDO_BATT(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmW",
-			max_i, sel_mv/1000, max);
-	} else {
-		int ma = 10 * (src_caps[max_i] & 0x3FF);
-		max = MIN(ma, MAX_CURRENT_MA);
-		flags = (max * sel_mv) < (1000 * OPERATING_POWER_MW) ?
-				RDO_CAP_MISMATCH : 0;
-		max_ma = max;
-		*rdo = RDO_FIXED(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmA",
-			max_i, sel_mv/1000, max);
-	}
-	/* Mismatch bit set if less power offered than the operating power */
-	if (flags & RDO_CAP_MISMATCH)
-		CPRINTF(" Mismatch");
-	CPRINTF("\n");
-
-	*curr_limit = max_ma;
-	*supply_voltage = sel_mv;
-	return EC_SUCCESS;
-}
-
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
 {
@@ -108,11 +41,6 @@ void pd_set_input_current_limit(int port, uint32_t max_ma,
 		CPRINTS("Failed to set input current limit for PD");
 }
 
-void pd_set_max_voltage(unsigned mv)
-{
-	max_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	int max_ma = rdo & 0x3FF;
diff --git a/board/ryu_p1/usb_pd_config.h b/board/ryu_p1/usb_pd_config.h
index 8adc002e8c..0328a2c1db 100644
--- a/board/ryu_p1/usb_pd_config.h
+++ b/board/ryu_p1/usb_pd_config.h
@@ -173,4 +173,10 @@ static inline int pd_snk_is_vbus_provided(int port)
 /* delay for the voltage transition on the power supply, chip max is 16us */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 20000 /* us */
 
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 10000
+#define PD_MAX_POWER_MW       60000
+#define PD_MAX_CURRENT_MA     3000
+#define PD_MAX_VOLTAGE_MV     20000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/ryu_p1/usb_pd_policy.c b/board/ryu_p1/usb_pd_policy.c
index 45cf597a11..6877ab6803 100644
--- a/board/ryu_p1/usb_pd_policy.c
+++ b/board/ryu_p1/usb_pd_policy.c
@@ -18,11 +18,6 @@
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
 
-/* Define typical operating power and max power */
-#define OPERATING_POWER_MW 10000
-#define MAX_POWER_MW       60000
-#define MAX_CURRENT_MA     3000
-
 #define PDO_FIXED_FLAGS (PDO_FIXED_DUAL_ROLE | PDO_FIXED_DATA_SWAP)
 
 const uint32_t pd_src_pdo[] = {
@@ -37,68 +32,6 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Cap on the max voltage requested as a sink (in millivolts) */
-static unsigned max_mv = -1; /* no cap */
-
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	int i;
-	int sel_mv;
-	int max_uw = 0;
-	int max_ma;
-	int max_i = -1;
-	int max;
-	uint32_t flags;
-
-	/* Get max power */
-	for (i = 0; i < cnt; i++) {
-		int uw;
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		if ((src_caps[i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-			uw = 250000 * (src_caps[i] & 0x3FF);
-		} else {
-			int ma = (src_caps[i] & 0x3FF) * 10;
-			uw = ma * mv;
-		}
-		if ((uw > max_uw) && (mv <= max_mv)) {
-			max_i = i;
-			max_uw = uw;
-			sel_mv = mv;
-		}
-	}
-	if (max_i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* build rdo for desired power */
-	if ((src_caps[max_i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-		int uw = 250000 * (src_caps[max_i] & 0x3FF);
-		max = MIN(1000 * uw, MAX_POWER_MW);
-		flags = (max < OPERATING_POWER_MW) ? RDO_CAP_MISMATCH : 0;
-		max_ma = 1000 * max / sel_mv;
-		*rdo = RDO_BATT(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmW",
-			max_i, sel_mv/1000, max);
-	} else {
-		int ma = 10 * (src_caps[max_i] & 0x3FF);
-		max = MIN(ma, MAX_CURRENT_MA);
-		flags = (max * sel_mv) < (1000 * OPERATING_POWER_MW) ?
-				RDO_CAP_MISMATCH : 0;
-		max_ma = max;
-		*rdo = RDO_FIXED(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmA",
-			max_i, sel_mv/1000, max);
-	}
-	/* Mismatch bit set if less power offered than the operating power */
-	if (flags & RDO_CAP_MISMATCH)
-		CPRINTF(" Mismatch");
-	CPRINTF("\n");
-
-	*curr_limit = max_ma;
-	*supply_voltage = sel_mv;
-	return EC_SUCCESS;
-}
-
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
 {
@@ -108,11 +41,6 @@ void pd_set_input_current_limit(int port, uint32_t max_ma,
 		CPRINTS("Failed to set input current limit for PD");
 }
 
-void pd_set_max_voltage(unsigned mv)
-{
-	max_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	int max_ma = rdo & 0x3FF;
diff --git a/board/samus_pd/usb_pd_config.h b/board/samus_pd/usb_pd_config.h
index d38999e2be..aafdd2f3b1 100644
--- a/board/samus_pd/usb_pd_config.h
+++ b/board/samus_pd/usb_pd_config.h
@@ -261,4 +261,10 @@ static inline int pd_snk_is_vbus_provided(int port)
 /* delay for the voltage transition on the power supply, chip max is 16us */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 20000 /* us */
 
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 15000
+#define PD_MAX_POWER_MW       60000
+#define PD_MAX_CURRENT_MA     3000
+#define PD_MAX_VOLTAGE_MV     20000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/samus_pd/usb_pd_policy.c b/board/samus_pd/usb_pd_policy.c
index 0831209ac1..89a7dad404 100644
--- a/board/samus_pd/usb_pd_policy.c
+++ b/board/samus_pd/usb_pd_policy.c
@@ -39,99 +39,6 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Cap on the max voltage requested as a sink (in millivolts) */
-static unsigned max_mv = -1; /* no cap */
-
-int pd_choose_voltage_common(int cnt, uint32_t *src_caps, uint32_t *rdo,
-			     uint32_t *curr_limit, uint32_t *supply_voltage,
-			     int choose_min)
-{
-	int i;
-	int sel_mv;
-	int max_uw = 0;
-	int max_ma;
-	int max_i = -1;
-	int max;
-	uint32_t flags;
-
-	/* Get max power */
-	for (i = 0; i < cnt; i++) {
-		int uw;
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		if ((src_caps[i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-			uw = 250000 * (src_caps[i] & 0x3FF);
-		} else {
-			int ma = (src_caps[i] & 0x3FF) * 10;
-			uw = ma * mv;
-		}
-		if ((uw > max_uw) && (mv <= max_mv)) {
-			max_i = i;
-			max_uw = uw;
-			sel_mv = mv;
-		}
-		/*
-		 * Choose the first entry if seaching for minimum, which will
-		 * always be vSafe5V.
-		 */
-		if (choose_min)
-			break;
-	}
-	if (max_i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* build rdo for desired power */
-	if ((src_caps[max_i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-		int uw = 250000 * (src_caps[max_i] & 0x3FF);
-		max = MIN(1000 * uw, MAX_POWER_MW);
-		flags = (max < OPERATING_POWER_MW) ? RDO_CAP_MISMATCH : 0;
-		max_ma = 1000 * max / sel_mv;
-		*rdo = RDO_BATT(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmW",
-			max_i, sel_mv/1000, max);
-	} else {
-		int ma = 10 * (src_caps[max_i] & 0x3FF);
-		/*
-		 * If we're choosing the minimum charge mode, limit our current
-		 * to what we can set with ilim PWM (500mA)
-		 */
-		max = MIN(ma, choose_min ? CONFIG_CHARGER_INPUT_CURRENT :
-					   MAX_CURRENT_MA);
-		flags = (max * sel_mv) < (1000 * OPERATING_POWER_MW) ?
-				RDO_CAP_MISMATCH : 0;
-		max_ma = max;
-		*rdo = RDO_FIXED(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmA",
-			max_i, sel_mv/1000, max);
-	}
-	/* Mismatch bit set if less power offered than the operating power */
-	if (flags & RDO_CAP_MISMATCH)
-		CPRINTF(" Mismatch");
-	CPRINTF("\n");
-
-	*curr_limit = max_ma;
-	*supply_voltage = sel_mv;
-	return EC_SUCCESS;
-}
-
-int pd_choose_voltage_min(int cnt, uint32_t *src_caps, uint32_t *rdo,
-			  uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	return pd_choose_voltage_common(cnt, src_caps, rdo, curr_limit,
-					supply_voltage, 1);
-}
-
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	return pd_choose_voltage_common(cnt, src_caps, rdo, curr_limit,
-					supply_voltage, 0);
-}
-
-void pd_set_max_voltage(unsigned mv)
-{
-	max_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	int max_ma = rdo & 0x3FF;
diff --git a/board/twinkie/injector.c b/board/twinkie/injector.c
index 4f1ece6f78..03db1c4af5 100644
--- a/board/twinkie/injector.c
+++ b/board/twinkie/injector.c
@@ -66,11 +66,6 @@ static const struct res_cfg {
 #define CC_RD(cc) ((cc > PD_SRC_RD_THRESHOLD) && (cc < PD_SRC_VNC))
 #define GET_POLARITY(cc1, cc2) (CC_RD(cc2) || CC_RA(cc1))
 
-/* Stub the function as we are not using the RX path */
-void pd_set_max_voltage(unsigned mv)
-{
-}
-
 /* we don't have the default DMA handlers */
 void dma_event_interrupt_channel_3(void)
 {
diff --git a/board/twinkie/usb_pd_config.h b/board/twinkie/usb_pd_config.h
index 8e5768a8c4..72dbe72ee4 100644
--- a/board/twinkie/usb_pd_config.h
+++ b/board/twinkie/usb_pd_config.h
@@ -191,4 +191,10 @@ static inline int pd_snk_is_vbus_provided(int port)
 /* delay necessary for the voltage transition on the power supply */
 #define PD_POWER_SUPPLY_TRANSITION_DELAY 50000 /* us */
 
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 15000
+#define PD_MAX_POWER_MW       60000
+#define PD_MAX_CURRENT_MA     3000
+#define PD_MAX_VOLTAGE_MV     20000
+
 #endif /* __USB_PD_CONFIG_H */
diff --git a/board/twinkie/usb_pd_policy.c b/board/twinkie/usb_pd_policy.c
index 2ba6a21538..4d568aff83 100644
--- a/board/twinkie/usb_pd_policy.c
+++ b/board/twinkie/usb_pd_policy.c
@@ -17,11 +17,6 @@
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
 
-/* Define typical operating power and max power */
-#define OPERATING_POWER_MW 15000
-#define MAX_POWER_MW       60000
-#define MAX_CURRENT_MA     3000
-
 #define PDO_FIXED_FLAGS (PDO_FIXED_EXTERNAL | PDO_FIXED_DATA_SWAP)
 
 const uint32_t pd_src_pdo[] = {
@@ -38,68 +33,6 @@ const uint32_t pd_snk_pdo[] = {
 };
 const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
 
-/* Cap on the max voltage requested as a sink (in millivolts) */
-static unsigned max_mv = -1; /* no cap */
-
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage)
-{
-	int i;
-	int sel_mv;
-	int max_uw = 0;
-	int max_ma;
-	int max_i = -1;
-	int max;
-	uint32_t flags;
-
-	/* Get max power */
-	for (i = 0; i < cnt; i++) {
-		int uw;
-		int mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
-		if ((src_caps[i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-			uw = 250000 * (src_caps[i] & 0x3FF);
-		} else {
-			int ma = (src_caps[i] & 0x3FF) * 10;
-			uw = ma * mv;
-		}
-		if ((uw > max_uw) && (mv <= max_mv)) {
-			max_i = i;
-			max_uw = uw;
-			sel_mv = mv;
-		}
-	}
-	if (max_i < 0)
-		return -EC_ERROR_UNKNOWN;
-
-	/* build rdo for desired power */
-	if ((src_caps[max_i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
-		int uw = 250000 * (src_caps[max_i] & 0x3FF);
-		max = MIN(1000 * uw, MAX_POWER_MW);
-		flags = (max < OPERATING_POWER_MW) ? RDO_CAP_MISMATCH : 0;
-		max_ma = 1000 * max / sel_mv;
-		*rdo = RDO_BATT(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmW",
-			max_i, sel_mv/1000, max);
-	} else {
-		int ma = 10 * (src_caps[max_i] & 0x3FF);
-		max = MIN(ma, MAX_CURRENT_MA);
-		flags = (max * sel_mv) < (1000 * OPERATING_POWER_MW) ?
-				RDO_CAP_MISMATCH : 0;
-		max_ma = max;
-		*rdo = RDO_FIXED(max_i + 1, max, max, flags);
-		CPRINTF("Request [%d] %dV %dmA",
-			max_i, sel_mv/1000, max);
-	}
-	/* Mismatch bit set if less power offered than the operating power */
-	if (flags & RDO_CAP_MISMATCH)
-		CPRINTF(" Mismatch");
-	CPRINTF("\n");
-
-	*curr_limit = max_ma;
-	*supply_voltage = sel_mv;
-	return EC_SUCCESS;
-}
-
 void pd_set_input_current_limit(int port, uint32_t max_ma,
 				uint32_t supply_voltage)
 {
@@ -111,11 +44,6 @@ void pd_set_input_current_limit(int port, uint32_t max_ma,
 	gpio_set_level(GPIO_LED_B_L, !blue);
 }
 
-void pd_set_max_voltage(unsigned mv)
-{
-	max_mv = mv;
-}
-
 int pd_check_requested_voltage(uint32_t rdo)
 {
 	int max_ma = rdo & 0x3FF;
diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index 118a04fbed..bfaccc8e29 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -4,6 +4,7 @@
  */
 #include "adc.h"
 #include "atomic.h"
+#include "charge_manager.h"
 #include "common.h"
 #include "console.h"
 #include "flash.h"
@@ -32,6 +33,140 @@
 
 static int rw_flash_changed = 1;
 
+#ifdef CONFIG_USB_PD_DUAL_ROLE
+/* Cap on the max voltage requested as a sink (in millivolts) */
+static unsigned max_request_mv = -1; /* no cap */
+
+/**
+ * Find PDO index that offers the most amount of power and stays within
+ * max_mv voltage.
+ *
+ * @param cnt  the number of Power Data Objects.
+ * @param src_caps Power Data Objects representing the source capabilities.
+ * @param max_mv maximum voltage (or -1 if no limit)
+ * @return index of PDO within source cap packet
+ */
+static int pd_find_pdo_index(int cnt, uint32_t *src_caps, int max_mv)
+{
+	int i, uw, max_uw = 0, mv, ma;
+	int ret = -1;
+
+	/* max_mv of -1 represents max limit */
+	if (max_mv == -1)
+		max_mv = PD_MAX_VOLTAGE_MV;
+
+	/* max voltage is always limited by this boards max request */
+	max_mv = MIN(max_mv, PD_MAX_VOLTAGE_MV);
+
+	/* Get max power that is under our max voltage input */
+	for (i = 0; i < cnt; i++) {
+		mv = ((src_caps[i] >> 10) & 0x3FF) * 50;
+		if ((src_caps[i] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
+			uw = 250000 * (src_caps[i] & 0x3FF);
+		} else {
+			ma = (src_caps[i] & 0x3FF) * 10;
+			uw = ma * mv;
+		}
+		if ((uw > max_uw) && (mv <= max_mv)) {
+			ret = i;
+			max_uw = uw;
+		}
+	}
+	return ret;
+}
+
+/**
+ * Extract power information out of a Power Data Object (PDO)
+ *
+ * @pdo Power data object
+ * @ma Current we can request from that PDO
+ * @mv Voltage of the PDO
+ */
+static void pd_extract_pdo_power(uint32_t pdo, uint32_t *ma, uint32_t *mv)
+{
+	int max_ma, uw;
+	*mv = ((pdo >> 10) & 0x3FF) * 50;
+
+	if ((pdo & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
+		uw = 250000 * (pdo & 0x3FF);
+		max_ma = 1000 * MIN(1000 * uw, PD_MAX_POWER_MW) / *mv;
+	} else {
+		max_ma = 10 * (pdo & 0x3FF);
+		max_ma = MIN(max_ma, PD_MAX_CURRENT_MA);
+	}
+
+	*ma = max_ma;
+}
+
+int pd_build_request(int cnt, uint32_t *src_caps, uint32_t *rdo,
+		     uint32_t *ma, uint32_t *mv, enum pd_request_type req_type)
+{
+	int pdo_index, flags = 0;
+	int uw;
+
+	if (req_type == PD_REQUEST_VSAFE5V)
+		/* src cap 0 should be vSafe5V */
+		pdo_index = 0;
+	else
+		/* find pdo index for max voltage we can request */
+		pdo_index = pd_find_pdo_index(cnt, src_caps, max_request_mv);
+
+	/* If could not find desired pdo_index, then return error */
+	if (pdo_index == -1)
+		return -EC_ERROR_UNKNOWN;
+
+	pd_extract_pdo_power(src_caps[pdo_index], ma, mv);
+	uw = *ma * *mv;
+	if (uw < (1000 * PD_OPERATING_POWER_MW))
+		flags |= RDO_CAP_MISMATCH;
+
+	if ((src_caps[pdo_index] & PDO_TYPE_MASK) == PDO_TYPE_BATTERY) {
+		int mw = uw / 1000;
+		*rdo = RDO_BATT(pdo_index + 1, mw, mw, flags);
+		CPRINTF("Request [%d] %dmV %dmW",
+			pdo_index, *mv, mw);
+	} else {
+		*rdo = RDO_FIXED(pdo_index + 1, *ma, *ma, flags);
+		CPRINTF("Request [%d] %dmV %dmA",
+			pdo_index, *mv, *ma);
+	}
+	/* Mismatch bit set if less power offered than the operating power */
+	if (flags & RDO_CAP_MISMATCH)
+		CPRINTF(" Mismatch");
+	CPRINTF("\n");
+
+	return EC_SUCCESS;
+}
+
+void pd_process_source_cap(int port, int cnt, uint32_t *src_caps)
+{
+#ifdef CONFIG_CHARGE_MANAGER
+	uint32_t ma, mv;
+	int pdo_index;
+
+	/* Get max power info that we could request */
+	pdo_index = pd_find_pdo_index(cnt, src_caps, -1);
+	if (pdo_index < 0)
+		pdo_index = 0;
+	pd_extract_pdo_power(src_caps[pdo_index], &ma, &mv);
+
+	/* Set max. limit, but apply 500mA ceiling */
+	charge_manager_set_ceil(port, PD_MIN_MA);
+	pd_set_input_current_limit(port, ma, mv);
+#endif
+}
+
+void pd_set_max_voltage(unsigned mv)
+{
+	max_request_mv = mv;
+}
+
+unsigned pd_get_max_voltage(void)
+{
+	return max_request_mv;
+}
+#endif /* CONFIG_USB_PD_DUAL_ROLE */
+
 #ifdef CONFIG_USB_PD_ALT_MODE
 
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index 82a3e5a91c..e522d4bac4 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -195,11 +195,6 @@ enum vdm_states {
 	VDM_STATE_BUSY = 2,
 };
 
-enum pd_request_types {
-	PD_REQUEST_MIN,
-	PD_REQUEST_MAX,
-};
-
 #ifdef CONFIG_USB_PD_DUAL_ROLE
 /* Port dual-role state */
 enum pd_dual_role_states drp_state = PD_DRP_TOGGLE_OFF;
@@ -776,14 +771,25 @@ static void pd_store_src_cap(int port, int cnt, uint32_t *src_caps)
 		pd_src_caps[port][i] = *src_caps++;
 }
 
-static void pd_send_request_msg(int port, enum pd_request_types request)
+static void pd_send_request_msg(int port)
 {
 	uint32_t rdo, curr_limit, supply_voltage;
 	int res;
 
-	/* we were waiting for them, let's process them */
-	res = pd_choose_voltage(pd_src_cap_cnt[port], pd_src_caps[port], &rdo,
-				&curr_limit, &supply_voltage);
+#ifdef CONFIG_CHARGE_MANAGER
+	int charging = (charge_manager_get_active_charge_port() == port);
+#else
+	const int charging = 1;
+#endif
+	/* Clear new power request */
+	pd[port].new_power_request = 0;
+
+	/* Build and send request RDO */
+	/* If this port is not actively charging, select vSafe5V */
+	res = pd_build_request(pd_src_cap_cnt[port], pd_src_caps[port],
+			       &rdo, &curr_limit, &supply_voltage,
+			       charging ? PD_REQUEST_MAX : PD_REQUEST_VSAFE5V);
+
 	if (res != EC_SUCCESS)
 		/*
 		 * If fail to choose voltage, do nothing, let source re-send
@@ -791,15 +797,6 @@ static void pd_send_request_msg(int port, enum pd_request_types request)
 		 */
 		return;
 
-#ifdef CONFIG_CHARGE_MANAGER
-	/* Set max. limit, but apply 500mA ceiling */
-	charge_manager_set_ceil(port, PD_MIN_MA);
-	pd_set_input_current_limit(port, curr_limit, supply_voltage);
-	/* Negotiate for Vsafe5V, if requested */
-	if (request == PD_REQUEST_MIN)
-		pd_choose_voltage_min(pd_src_cap_cnt[port], pd_src_caps[port],
-				      &rdo, &curr_limit, &supply_voltage);
-#endif
 	pd[port].curr_limit = curr_limit;
 	pd[port].supply_voltage = supply_voltage;
 	res = send_request(port, rdo);
@@ -844,12 +841,10 @@ static void handle_data_request(int port, uint16_t head,
 			pd_store_src_cap(port, cnt, payload);
 			/* src cap 0 should be fixed PDO */
 			pd_update_pdo_flags(port, payload[0]);
-#ifdef CONFIG_CHARGE_MANAGER
-			if (charge_manager_get_active_charge_port() == port)
-				pd_send_request_msg(port, PD_REQUEST_MAX);
-			else
-#endif
-				pd_send_request_msg(port, PD_REQUEST_MIN);
+
+			pd_process_source_cap(port, pd_src_cap_cnt[port],
+					      pd_src_caps[port]);
+			pd_send_request_msg(port);
 		}
 		break;
 #endif /* CONFIG_USB_PD_DUAL_ROLE */
@@ -2031,17 +2026,7 @@ void pd_task(void)
 
 			/* Check for new power to request */
 			if (pd[port].new_power_request) {
-				pd[port].new_power_request = 0;
-#ifdef CONFIG_CHARGE_MANAGER
-				if (charge_manager_get_active_charge_port()
-				    != port)
-					pd_send_request_msg(port,
-							    PD_REQUEST_MIN);
-				else if (charge_manager_get_active_charge_port()
-					 == port)
-#endif
-					pd_send_request_msg(port,
-							    PD_REQUEST_MAX);
+				pd_send_request_msg(port);
 				break;
 			}
 
diff --git a/include/usb_pd.h b/include/usb_pd.h
index 045322c80d..b7c41c7b93 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -678,33 +678,34 @@ enum pd_data_msg_type {
 
 /* --- Policy layer functions --- */
 
+/* Request types for pd_build_request() */
+enum pd_request_type {
+	PD_REQUEST_VSAFE5V,
+	PD_REQUEST_MAX,
+};
+
 /**
- * Decide which voltage to use from the source capabilities - prefer the
- * mode which delivers the maximum allowable power.
+ * Decide which PDO to choose from the source capabilities.
  *
  * @param cnt  the number of Power Data Objects.
  * @param src_caps Power Data Objects representing the source capabilities.
  * @param rdo  requested Request Data Object.
- * @param curr_limit  selected current limit (stored on success)
- * @param supply_voltage  selected supply voltage (stored on success)
+ * @param ma  selected current limit (stored on success)
+ * @param mv  selected supply voltage (stored on success)
+ * @param req_type request type
  * @return <0 if invalid, else EC_SUCCESS
  */
-int pd_choose_voltage(int cnt, uint32_t *src_caps, uint32_t *rdo,
-		      uint32_t *curr_limit, uint32_t *supply_voltage);
+int pd_build_request(int cnt, uint32_t *src_caps, uint32_t *rdo,
+		     uint32_t *ma, uint32_t *mv, enum pd_request_type req_type);
 
 /**
- * Decide which voltage to use from the source capabilities - prefer the
- * mode which delivers the minimum allowable power.
+ * Process source capabilities packet
  *
+ * @param port USB-C port number
  * @param cnt  the number of Power Data Objects.
  * @param src_caps Power Data Objects representing the source capabilities.
- * @param rdo  requested Request Data Object.
- * @param curr_limit  selected current limit (stored on success)
- * @param supply_voltage  selected supply voltage (stored on success)
- * @return <0 if invalid, else EC_SUCCESS
  */
-int pd_choose_voltage_min(int cnt, uint32_t *src_caps, uint32_t *rdo,
-			  uint32_t *curr_limit, uint32_t *supply_voltage);
+void pd_process_source_cap(int port, int cnt, uint32_t *src_caps);
 
 /**
  * Put a cap on the max voltage requested as a sink.
@@ -713,6 +714,12 @@ int pd_choose_voltage_min(int cnt, uint32_t *src_caps, uint32_t *rdo,
 void pd_set_max_voltage(unsigned mv);
 
 /**
+ * Get the max voltage that can be requested as set by pd_set_max_voltage().
+ * @return max voltage
+ */
+unsigned pd_get_max_voltage(void);
+
+/**
  * Request a new operating voltage.
  *
  * @param rdo  Request Data Object with the selected operating point.