usbpd: support dynamic PDO selection

Support dynamic PDO selection CONFIG_USB_PD_DPS.
This config controls the charging voltage and power according to the
input power and battery configuration.

DPS would continuously evaluate the system load and current charging
voltage, and decide a new one by below:
1. If the PDO can fulfill system desired power.
2. If the PDO is efficient for the battery configuration.

To detect if the system load cannot be fulfilled by the current PDO,
it checks:
1. if the input current closes to the PDO current limit.
2. if the input power closes to the PDO maximum power.

To detect if the system load can be fulfilled by a more efficient PDO,
it checks:
- if the voltage of a new PDO is closer to the battery voltage than the
   current PDO, and the power is able fulfill the system load.

BUG=b:169532537
TEST=1. tested on asurada, the charging voltage is able to switch to
     different PDOs under different system loads
     2. tested that the DPS is able to switch charge port
     (e.g. C1 12V -> C0 9V) based on the provided PDOs.
BRANCH=asurada

Change-Id: I7c7706b331dc0d4f8ac68569dc7ed852fc9308e3
Signed-off-by: Eric Yilun Lin <yllin@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2897064
Tested-by: Eric Yilun Lin <yllin@google.com>
Reviewed-by: Diana Z <dzigterman@chromium.org>
Commit-Queue: Eric Yilun Lin <yllin@google.com>
(cherry picked from commit 61bbfcaa37ecec87a9cea89e0f8fc65ecbecd13e)
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3133187
Auto-Submit: Eric Yilun Lin <yllin@google.com>
Reviewed-by: Ting Shen <phoenixshen@chromium.org>
Commit-Queue: Ting Shen <phoenixshen@chromium.org>

10 files changed, 774 insertions, 5 deletions

diff --git a/common/build.mk b/common/build.mk
index 3ca7a0fa56..1579f94fb2 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -166,6 +166,7 @@ common-$(CONFIG_USB_PD_CONSOLE_CMD)+=usb_pd_console_cmd.o
 endif
 common-$(CONFIG_USB_PD_ALT_MODE_DFP)+=usb_pd_alt_mode_dfp.o
 common-$(CONFIG_USB_PD_ALT_MODE_UFP)+=usb_pd_alt_mode_ufp.o
+common-$(CONFIG_USB_PD_DPS)+=dps.o
 common-$(CONFIG_USB_PD_LOGGING)+=event_log.o pd_log.o
 common-$(CONFIG_USB_PD_TCPC)+=usb_pd_tcpc.o
 common-$(CONFIG_USB_UPDATE)+=usb_update.o update_fw.o
diff --git a/common/charge_manager.c b/common/charge_manager.c
index 9619209990..29990e4d38 100644
--- a/common/charge_manager.c
+++ b/common/charge_manager.c
@@ -11,6 +11,8 @@
 #include "charge_state_v2.h"
 #include "charger.h"
 #include "console.h"
+#include "dps.h"
+#include "extpower.h"
 #include "gpio.h"
 #include "hooks.h"
 #include "host_command.h"
@@ -616,11 +618,13 @@ static void charge_manager_get_best_charge_port(int *new_port,
 
 	/* Skip port selection on OVERRIDE_DONT_CHARGE. */
 	if (override_port != OVERRIDE_DONT_CHARGE) {
+
 		/*
 		 * 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)
+		 * 1. Prefer DPS charge port.
+		 * 2. Prefer higher priority supply.
+		 * 3. Prefer higher power over lower in case priority is tied.
+		 * 4. 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.
@@ -662,8 +666,16 @@ static void charge_manager_get_best_charge_port(int *new_port,
 				candidate_port_power =
 					POWER(available_charge[i][j]);
 
+				/* Select DPS port if provided. */
+				if (IS_ENABLED(CONFIG_USB_PD_DPS) &&
+				    override_port == OVERRIDE_OFF &&
+				    i == CHARGE_SUPPLIER_PD &&
+				    j == dps_get_charge_port()) {
+					supplier = i;
+					port = j;
+					break;
 				/* Select if no supplier chosen yet. */
-				if (supplier == CHARGE_SUPPLIER_NONE ||
+				} else if (supplier == CHARGE_SUPPLIER_NONE ||
 				/* ..or if supplier priority is higher. */
 				    supplier_priority[i] <
 				    supplier_priority[supplier] ||
diff --git a/common/dps.c b/common/dps.c
new file mode 100644
index 0000000000..235f4d4e08
--- /dev/null
+++ b/common/dps.c
@@ -0,0 +1,639 @@
+/* Copyright 2021 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ *
+ * Dynamic PDO Selection.
+ */
+
+#include <stdint.h>
+
+#include "adc.h"
+#include "dps.h"
+#include "atomic.h"
+#include "battery.h"
+#include "console.h"
+#include "charger.h"
+#include "charge_manager.h"
+#include "charge_state.h"
+#include "charge_state_v2.h"
+#include "math_util.h"
+#include "task.h"
+#include "timer.h"
+#include "usb_common.h"
+#include "usb_pd.h"
+#include "util.h"
+#include "usb_pe_sm.h"
+
+
+#define K_MORE_PWR 96
+#define K_LESS_PWR 93
+#define K_SAMPLE 1
+#define K_WINDOW 3
+#define T_REQUEST_STABLE_TIME (10 * SECOND)
+#define T_NEXT_CHECK_TIME (5 * SECOND)
+
+#define DPS_FLAG_DISABLED		BIT(0)
+#define DPS_FLAG_NO_SRCCAP		BIT(1)
+#define DPS_FLAG_WAITING		BIT(2)
+#define DPS_FLAG_SAMPLED		BIT(3)
+#define DPS_FLAG_NEED_MORE_PWR		BIT(4)
+
+#define DPS_FLAG_STOP_EVENTS		(DPS_FLAG_DISABLED | \
+					 DPS_FLAG_NO_SRCCAP)
+#define DPS_FLAG_ALL			GENMASK(31, 0)
+
+#define MAX_MOVING_AVG_WINDOW		5
+
+BUILD_ASSERT(K_MORE_PWR > K_LESS_PWR && 100 >= K_MORE_PWR && 100 >= K_LESS_PWR);
+
+/* lock for updating timeout value */
+static mutex_t dps_lock;
+static timestamp_t timeout;
+static bool is_enabled = true;
+static int debug_level;
+static bool fake_enabled;
+static int fake_mv, fake_ma;
+static int dynamic_mv;
+static int dps_port = CHARGE_PORT_NONE;
+static uint32_t flag;
+
+#define CPRINTF(format, args...) cprintf(CC_USBPD, "DPS " format, ##args)
+#define CPRINTS(format, args...) cprints(CC_USBPD, "DPS " format, ##args)
+
+__overridable struct dps_config_t dps_config = {
+	.k_less_pwr = K_LESS_PWR,
+	.k_more_pwr = K_MORE_PWR,
+	.k_sample = K_SAMPLE,
+	.k_window = K_WINDOW,
+	.t_stable = T_REQUEST_STABLE_TIME,
+	.t_check = T_NEXT_CHECK_TIME,
+	.is_more_efficient = NULL,
+};
+
+int dps_get_dynamic_voltage(void)
+{
+	return dynamic_mv;
+}
+
+int dps_get_charge_port(void)
+{
+	return dps_port;
+}
+
+bool dps_is_enabled(void)
+{
+	return is_enabled;
+}
+
+static void dps_enable(bool en)
+{
+	bool prev_en = is_enabled;
+
+	is_enabled = en;
+
+	if (is_enabled && !prev_en)
+		task_wake(TASK_ID_DPS);
+}
+
+static void update_timeout(int us)
+{
+	timestamp_t new_timeout;
+
+	new_timeout.val = get_time().val + us;
+
+	mutex_lock(&dps_lock);
+	if (new_timeout.val > timeout.val)
+		timeout = new_timeout;
+	mutex_unlock(&dps_lock);
+}
+
+/*
+ * DPS reset.
+ */
+static void dps_reset(void)
+{
+	dynamic_mv = PD_MAX_VOLTAGE_MV;
+	dps_port = CHARGE_PORT_NONE;
+}
+
+/*
+ * DPS initialization.
+ */
+static void dps_init(void)
+{
+	dps_reset();
+
+	if (dps_config.k_window > MAX_MOVING_AVG_WINDOW) {
+		dps_config.k_window = MAX_MOVING_AVG_WINDOW;
+		CPRINTS("ERR:WIN");
+	}
+
+	if (dps_config.k_less_pwr > 100 ||
+	    dps_config.k_more_pwr > 100 ||
+	    dps_config.k_more_pwr <= dps_config.k_less_pwr) {
+		dps_config.k_less_pwr = K_LESS_PWR;
+		dps_config.k_more_pwr = K_MORE_PWR;
+		CPRINTS("ERR:COEF");
+	}
+}
+
+static bool is_near_limit(int val, int limit)
+{
+	return val >= (limit * dps_config.k_more_pwr / 100);
+}
+
+bool is_more_efficient(int curr_mv, int prev_mv, int batt_mv, int batt_mw,
+		       int input_mw)
+{
+	if (dps_config.is_more_efficient)
+		return dps_config.is_more_efficient(curr_mv, prev_mv, batt_mv,
+						    batt_mw, input_mw);
+
+	return ABS(curr_mv - batt_mv) < ABS(prev_mv - batt_mv);
+}
+
+/*
+ * Get the input power of the active port.
+ *
+ * input_power = vbus * input_current
+ *
+ * @param vbus: VBUS in mV
+ * @param input_curr: input current in mA
+ *
+ * @return input_power of the result of vbus * input_curr in mW
+ */
+static int get_desired_input_power(int *vbus, int *input_current)
+{
+	int active_port;
+	int charger_id;
+	enum ec_error_list rv;
+
+	active_port = charge_manager_get_active_charge_port();
+
+	if (active_port == CHARGE_PORT_NONE)
+		return 0;
+
+	charger_id = charge_get_active_chg_chip();
+
+	if (fake_enabled) {
+		*vbus = fake_mv;
+		*input_current = fake_ma;
+		return fake_mv * fake_ma / 1000;
+	}
+
+	rv = charger_get_input_current(charger_id, input_current);
+	if (rv)
+		return 0;
+
+	*vbus = charge_manager_get_vbus_voltage(active_port);
+
+	return (*vbus) * (*input_current) / 1000;
+}
+
+/*
+ * Get the most efficient PDO voltage for the battery of the charging port
+ *
+ * | W\Batt | 1S(3.7V) | 2S(7.4V) | 3S(11.1V) | 4S(14.8V) |
+ * --------------------------------------------------------
+ * | 0-15W  | 5V       | 9V       | 12V       | 15V       |
+ * | 15-27W | 9V       | 9V       | 12V       | 15V       |
+ * | 27-36W | 12V      | 12V      | 12V       | 15V       |
+ * | 36-45W | 15V      | 15V      | 15V       | 15V       |
+ * | 45-60W | 20V      | 20V      | 20V       | 20V       |
+ *
+ *
+ * @return 0 if error occurs, else battery efficient voltage in mV
+ */
+int get_efficient_voltage(void)
+{
+	int eff_mv = 0;
+	int batt_mv;
+	int batt_pwr;
+	int input_pwr, vbus, input_curr;
+	const struct batt_params *batt = charger_current_battery_params();
+
+	input_pwr = get_desired_input_power(&vbus, &input_curr);
+
+	if (!input_pwr)
+		return 0;
+
+	if (battery_design_voltage(&batt_mv))
+		return 0;
+
+	batt_pwr = batt->current * batt->voltage / 1000;
+
+	for (int i = 0; i < board_get_usb_pd_port_count(); ++i) {
+		const int cnt = pd_get_src_cap_cnt(i);
+		const uint32_t *src_caps = pd_get_src_caps(i);
+
+		for (int j = 0; j < cnt; ++j) {
+			int ma, mv, unused;
+
+			pd_extract_pdo_power(src_caps[j], &ma, &mv, &unused);
+			/*
+			 * If the eff_mv is not picked, or we have more
+			 * efficient voltage (less voltage diff)
+			 */
+			if (eff_mv == 0 ||
+			    is_more_efficient(mv, eff_mv, batt_mv, batt_pwr,
+					      input_pwr))
+				eff_mv = mv;
+		}
+	}
+
+	return eff_mv;
+}
+
+struct pdo_candidate {
+	int port;
+	int mv;
+	int mw;
+};
+
+#define UPDATE_CANDIDATE(new_port, new_mv, new_mw) \
+	do { \
+		cand->port = new_port; \
+		cand->mv = new_mv; \
+		cand->mw = new_mw; \
+	} while (0)
+
+#define CLEAR_AND_RETURN() \
+	do { \
+		moving_avg_count = 0; \
+		return false; \
+	} while (0)
+
+/*
+ * Evaluate the system power if a new PD power request is needed.
+ *
+ * @param struct pdo_candidate: The candidate PDO. (Return value)
+ * @return true if a new power request, or false otherwise.
+ */
+static bool has_new_power_request(struct pdo_candidate *cand)
+{
+	int vbus, input_curr, input_pwr;
+	int input_pwr_avg = 0, input_curr_avg = 0;
+	int batt_pwr, batt_mv;
+	int max_mv = pd_get_max_voltage();
+	int req_pwr, req_ma, req_mv;
+	int input_curr_limit;
+	int active_port = charge_manager_get_active_charge_port();
+	int charger_id;
+	static int input_pwrs[MAX_MOVING_AVG_WINDOW];
+	static int input_currs[MAX_MOVING_AVG_WINDOW];
+	static int prev_active_port = CHARGE_PORT_NONE;
+	static int prev_req_mv;
+	static int moving_avg_count;
+	const struct batt_params *batt = charger_current_battery_params();
+
+	/* set a default value in case it early returns. */
+	UPDATE_CANDIDATE(CHARGE_PORT_NONE, INT32_MAX, 0);
+
+	if (active_port == CHARGE_PORT_NONE)
+		CLEAR_AND_RETURN();
+
+	req_mv = pd_get_requested_voltage(active_port);
+	req_ma = pd_get_requested_current(active_port);
+
+	if (!req_mv)
+		CLEAR_AND_RETURN();
+
+	if (battery_design_voltage(&batt_mv))
+		CLEAR_AND_RETURN();
+
+	/* if last sample is not the same as the current one, reset counting. */
+	if (prev_req_mv != req_mv || prev_active_port != active_port)
+		moving_avg_count = 0;
+	prev_active_port = active_port;
+	prev_req_mv = req_mv;
+
+	req_pwr = req_mv * req_ma / 1000;
+	batt_pwr = batt->current * batt->voltage / 1000;
+	input_pwr = get_desired_input_power(&vbus, &input_curr);
+
+	if (!input_pwr)
+		CLEAR_AND_RETURN();
+
+	/* record moving average */
+	input_pwrs[moving_avg_count % dps_config.k_window] = input_pwr;
+	input_currs[moving_avg_count % dps_config.k_window] = input_curr;
+	if (++moving_avg_count < dps_config.k_window)
+		return false;
+
+	for (int i = 0; i < dps_config.k_window; i++) {
+		input_curr_avg += input_currs[i];
+		input_pwr_avg += input_pwrs[i];
+	}
+	input_curr_avg /= dps_config.k_window;
+	input_pwr_avg /= dps_config.k_window;
+
+	charger_id = charge_get_active_chg_chip();
+
+	if (!charger_get_input_current_limit(charger_id, &input_curr_limit))
+		/* set as last requested mA if we're unable to get the limit. */
+		input_curr_limit = req_ma;
+
+	/*
+	 * input power might be insufficient, force it to negotiate a more
+	 * powerful PDO.
+	 */
+	if (is_near_limit(input_pwr_avg, req_pwr) ||
+	    is_near_limit(input_curr_avg, MIN(req_ma, input_curr_limit))) {
+		flag |= DPS_FLAG_NEED_MORE_PWR;
+		if (!fake_enabled)
+			input_pwr_avg = req_pwr + 1;
+	} else {
+		flag &= ~DPS_FLAG_NEED_MORE_PWR;
+	}
+
+	if (debug_level)
+		CPRINTS("C%d 0x%x last (%dmW %dmV) input (%dmW %dmV %dmA) "
+			"avg (%dmW, %dmA)",
+			active_port, flag, req_pwr, req_mv, input_pwr, vbus,
+			input_curr, input_pwr_avg, input_curr_avg);
+
+	for (int i = 0; i < board_get_usb_pd_port_count(); ++i) {
+		const uint32_t * const src_caps = pd_get_src_caps(i);
+
+		for (int j = 0; j < pd_get_src_cap_cnt(i); ++j) {
+			int ma, mv, unused;
+			int mw;
+			bool efficient;
+
+			/* TODO(b:169532537): support augmented PDO. */
+			if ((src_caps[j] & PDO_TYPE_MASK) != PDO_TYPE_FIXED)
+				continue;
+
+			pd_extract_pdo_power(src_caps[j], &ma, &mv, &unused);
+
+			if (mv > max_mv)
+				continue;
+
+			mw = ma * mv / 1000;
+			efficient = is_more_efficient(mv, cand->mv, batt_mv,
+						      batt_pwr, input_pwr_avg);
+
+			if (flag & DPS_FLAG_NEED_MORE_PWR) {
+				/* the insufficient case.*/
+				if (input_pwr_avg > cand->mw &&
+				    (mw > cand->mw ||
+				     (mw == cand->mw && efficient))) {
+					UPDATE_CANDIDATE(i, mv, mw);
+				} else if (input_pwr_avg <= mw && efficient) {
+					UPDATE_CANDIDATE(i, mv, mw);
+				}
+			} else {
+				int adjust_pwr =
+					mw * dps_config.k_less_pwr / 100;
+				int adjust_cand_mw =
+					cand->mw * dps_config.k_less_pwr / 100;
+
+				/* Pick if we don't have a candidate yet. */
+				if (!cand->mw) {
+					UPDATE_CANDIDATE(i, mv, mw);
+					/*
+					 * if the candidate is insufficient, and
+					 * we get one provides more.
+					 */
+				} else if ((adjust_cand_mw < input_pwr_avg &&
+					    cand->mw < mw) ||
+					   /*
+					    * if the candidate is sufficient,
+					    * and we pick a more efficient one.
+					    */
+					   (adjust_cand_mw >= input_pwr_avg &&
+					    adjust_pwr >= input_pwr_avg &&
+					    efficient)) {
+					UPDATE_CANDIDATE(i, mv, mw);
+				}
+			}
+
+
+			/*
+			 * if the candidate is the same as the current one, pick
+			 * the one at active charge port.
+			 */
+			if (mw == cand->mw && mv == cand->mv &&
+			    i == active_port)
+				UPDATE_CANDIDATE(i, mv, mw);
+		}
+	}
+
+	if (!cand->mv)
+		CPRINTS("ERR:CNDMV");
+
+	return (cand->mv != req_mv);
+}
+
+static bool has_srccap(void)
+{
+	for (int i = 0; i < board_get_usb_pd_port_count(); ++i) {
+		if (pd_is_connected(i) &&
+		    pd_get_power_role(i) == PD_ROLE_SINK &&
+		    pd_get_src_cap_cnt(i) > 0)
+			return true;
+	}
+	return false;
+}
+
+void dps_update_stabilized_time(int port)
+{
+	update_timeout(dps_config.t_stable);
+}
+
+void dps_task(void *u)
+{
+	struct pdo_candidate last_cand = {CHARGE_PORT_NONE, 0, 0};
+	int sample_count = 0;
+
+	dps_init();
+	update_timeout(dps_config.t_check);
+
+	while (1) {
+		struct pdo_candidate curr_cand = {CHARGE_PORT_NONE, 0, 0};
+		timestamp_t now;
+
+		now = get_time();
+		if (flag & DPS_FLAG_STOP_EVENTS) {
+			dps_reset();
+			task_wait_event(-1);
+			/* clear flags after wake up. */
+			flag = 0;
+			update_timeout(dps_config.t_check);
+			continue;
+		} else if (now.val < timeout.val) {
+			flag |= DPS_FLAG_WAITING;
+			task_wait_event(timeout.val - now.val);
+			flag &= ~DPS_FLAG_WAITING;
+		}
+
+		if (!is_enabled) {
+			flag |= DPS_FLAG_DISABLED;
+			continue;
+		}
+
+		if (!has_srccap()) {
+			flag |= DPS_FLAG_NO_SRCCAP;
+			continue;
+		}
+
+		if (!has_new_power_request(&curr_cand)) {
+			sample_count = 0;
+			flag &= ~DPS_FLAG_SAMPLED;
+		} else {
+			if (last_cand.port == curr_cand.port &&
+			    last_cand.mv == curr_cand.mv &&
+			    last_cand.mw == curr_cand.mw)
+				sample_count++;
+			else
+				sample_count = 1;
+			flag |= DPS_FLAG_SAMPLED;
+		}
+
+		if (sample_count == dps_config.k_sample) {
+			dynamic_mv = curr_cand.mv;
+			dps_port = curr_cand.port;
+			pd_dpm_request(dps_port,
+				       DPM_REQUEST_NEW_POWER_LEVEL);
+			sample_count = 0;
+			flag &= ~(DPS_FLAG_SAMPLED | DPS_FLAG_NEED_MORE_PWR);
+		}
+
+		last_cand.port = curr_cand.port;
+		last_cand.mv = curr_cand.mv;
+		last_cand.mw = curr_cand.mw;
+
+		update_timeout(dps_config.t_check);
+	}
+}
+
+static int command_dps(int argc, char **argv)
+{
+	int port = charge_manager_get_active_charge_port();
+	int input_pwr, vbus, input_curr;
+	int holder;
+
+	if (argc == 1) {
+		uint32_t last_ma = 0, last_mv = 0;
+		int batt_mv;
+
+		ccprintf("flag=0x%x k_more=%d k_less=%d k_sample=%d k_win=%d\n",
+			 flag, dps_config.k_more_pwr, dps_config.k_less_pwr,
+			 dps_config.k_sample, dps_config.k_window);
+		ccprintf("t_stable=%d t_check=%d\n",
+			 dps_config.t_stable / SECOND,
+			 dps_config.t_check / SECOND);
+		if (!is_enabled) {
+			ccprintf("DPS Disabled\n");
+			return EC_SUCCESS;
+		}
+
+		if (port == CHARGE_PORT_NONE) {
+			ccprintf("No charger attached\n");
+			return EC_SUCCESS;
+		}
+
+		battery_design_voltage(&batt_mv);
+		input_pwr = get_desired_input_power(&vbus, &input_curr);
+		if (!(flag & DPS_FLAG_NO_SRCCAP)) {
+			last_mv = pd_get_requested_voltage(port);
+			last_ma = pd_get_requested_current(port);
+		}
+		ccprintf("C%d DPS Enabled\n"
+			 "Requested: %dmV/%dmA\n"
+			 "Measured:  %dmV/%dmA/%dmW\n"
+			 "Efficient: %dmV\n"
+			 "Batt:      %dmv\n"
+			 "PDMaxMV:   %dmV\n",
+			 port, last_mv, last_ma,
+			 vbus, input_curr, input_pwr,
+			 get_efficient_voltage(),
+			 batt_mv,
+			 pd_get_max_voltage());
+		return EC_SUCCESS;
+	}
+
+	if (!strcasecmp(argv[1], "en")) {
+		dps_enable(true);
+		return EC_SUCCESS;
+	} else if (!strcasecmp(argv[1], "dis")) {
+		dps_enable(false);
+		return EC_SUCCESS;
+	} else if (!strcasecmp(argv[1], "fakepwr")) {
+		if (argc == 2) {
+			ccprintf("%sabled %dmV/%dmA\n",
+				 fake_enabled ? "en" : "dis", fake_mv, fake_ma);
+			return EC_SUCCESS;
+		}
+
+		if (!strcasecmp(argv[2], "dis")) {
+			fake_enabled = false;
+			return EC_SUCCESS;
+		}
+
+		if (argc < 4)
+			return EC_ERROR_PARAM_COUNT;
+
+		holder = atoi(argv[2]);
+		if (holder <= 0)
+			return EC_ERROR_PARAM2;
+		fake_mv = holder;
+
+		holder = atoi(argv[3]);
+		if (holder <= 0)
+			return EC_ERROR_PARAM3;
+		fake_ma = holder;
+
+		fake_enabled = true;
+		return EC_SUCCESS;
+	}
+
+	if (argc != 3)
+		return EC_ERROR_PARAM2;
+
+	if (!strcasecmp(argv[1], "debug")) {
+		debug_level = atoi(argv[2]);
+	} else if (!strcasecmp(argv[1], "setkmore")) {
+		holder = atoi(argv[2]);
+		if (holder > 100 || holder <= 0 ||
+		    holder < dps_config.k_less_pwr)
+			return EC_ERROR_PARAM2;
+		dps_config.k_more_pwr = holder;
+	} else if (!strcasecmp(argv[1], "setkless")) {
+		holder = atoi(argv[2]);
+		if (holder > 100 || holder <= 0 ||
+		    holder > dps_config.k_more_pwr)
+			return EC_ERROR_PARAM2;
+		dps_config.k_less_pwr = holder;
+	} else if (!strcasecmp(argv[1], "setksample")) {
+		holder = atoi(argv[2]);
+		if (holder <= 0)
+			return EC_ERROR_PARAM2;
+		dps_config.k_sample = holder;
+	} else if (!strcasecmp(argv[1], "setkwin")) {
+		holder = atoi(argv[2]);
+		if (holder <= 0 || holder > MAX_MOVING_AVG_WINDOW)
+			return EC_ERROR_PARAM2;
+		dps_config.k_window = holder;
+	} else if (!strcasecmp(argv[1], "settcheck")) {
+		holder = atoi(argv[2]);
+		if (holder <= 0)
+			return EC_ERROR_PARAM2;
+		dps_config.t_check = holder * SECOND;
+	} else if (!strcasecmp(argv[1], "settstable")) {
+		holder = atoi(argv[2]);
+		if (holder <= 0)
+			return EC_ERROR_PARAM2;
+		dps_config.t_stable = holder * SECOND;
+	} else {
+		return EC_ERROR_PARAM1;
+	}
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(dps, command_dps,
+			"en|dis|debug <int>\n"
+			"\t\t set(kmore|kless|ksample|kwindow) <int>\n"
+			"\t\t set(tstable|tcheck) <int>\n"
+			"\t\t fakepwr [dis|<mV> <mA>]",
+			"Print/set Dynamic PDO Selection state.");
diff --git a/common/usb_pd_dual_role.c b/common/usb_pd_dual_role.c
index 3c6aff6adf..3371bc6750 100644
--- a/common/usb_pd_dual_role.c
+++ b/common/usb_pd_dual_role.c
@@ -7,6 +7,7 @@
 
 #include "charge_manager.h"
 #include "charge_state.h"
+#include "dps.h"
 #include "system.h"
 #include "usb_common.h"
 #include "usb_pd.h"
@@ -21,6 +22,7 @@
  */
 static unsigned int max_request_mv = PD_MAX_VOLTAGE_MV;
 
+/* TODO(b:169532537): deprecate CONFIG_USB_PD_PREFER_MV */
 STATIC_IF_NOT(CONFIG_USB_PD_PREFER_MV)
 struct pd_pref_config_t __maybe_unused pd_pref_config;
 
@@ -236,6 +238,10 @@ void pd_build_request(int32_t vpd_vdo, uint32_t *rdo, uint32_t *ma,
 	else
 		max_request_allowed = 1;
 
+	if (IS_ENABLED(CONFIG_USB_PD_DPS) && dps_is_enabled())
+		max_request_mv =
+			MIN(max_request_mv, dps_get_dynamic_voltage());
+
 	/*
 	 * If currently charging on a different port, or we are not allowed to
 	 * request the max voltage, then select vSafe5V
@@ -342,11 +348,15 @@ void pd_process_source_cap(int port, int cnt, uint32_t *src_caps)
 
 	if (IS_ENABLED(CONFIG_CHARGE_MANAGER)) {
 		uint32_t ma, mv, pdo, unused;
+		uint32_t max_mv = pd_get_max_voltage();
+
+		if (IS_ENABLED(CONFIG_USB_PD_DPS) && dps_is_enabled())
+			max_mv = MIN(max_mv, dps_get_dynamic_voltage());
 
 		/* Get max power info that we could request */
 		pd_find_pdo_index(pd_get_src_cap_cnt(port),
 					pd_get_src_caps(port),
-					pd_get_max_voltage(), &pdo);
+					max_mv, &pdo);
 		pd_extract_pdo_power(pdo, &ma, &mv, &unused);
 
 		/* Set max. limit, but apply 500mA ceiling */
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index 2dc4649fe3..cffdd7f768 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -10,6 +10,7 @@
 #include "charge_state.h"
 #include "common.h"
 #include "console.h"
+#include "dps.h"
 #include "driver/tcpm/tcpm.h"
 #include "ec_commands.h"
 #include "hooks.h"
@@ -3081,6 +3082,11 @@ static void pe_snk_evaluate_capability_entry(int port)
 
 	/* Device Policy Response Received */
 	set_state_pe(port, PE_SNK_SELECT_CAPABILITY);
+
+#ifdef HAS_TASK_DPS
+	/* Wake DPS task to evaluate the SrcCaps */
+	task_wake(TASK_ID_DPS);
+#endif
 }
 
 /**
@@ -3310,6 +3316,10 @@ static void pe_snk_transition_sink_exit(int port)
 				CEIL_REQUESTOR_PD, pe[port].curr_limit);
 
 	pd_timer_disable(port, PE_TIMER_PS_TRANSITION);
+
+	if (IS_ENABLED(CONFIG_USB_PD_DPS))
+		if (charge_manager_get_active_charge_port() == port)
+			dps_update_stabilized_time(port);
 }
 
 
diff --git a/include/config.h b/include/config.h
index 93014582de..66ed4a4b97 100644
--- a/include/config.h
+++ b/include/config.h
@@ -4073,6 +4073,15 @@
 #undef CONFIG_USB_PD_TCPMV2
 
 /*
+ * Enable dynamic PDO selection.
+ *
+ * DPS picks a power efficient voltage regarding to the battery configuration
+ * and the system loading. It monitors PIn (Power In), so VBUS/IBUS ADC
+ * should be supported on the platform.
+ */
+#undef CONFIG_USB_PD_DPS
+
+/*
  * Device Types for TCPMv2.
  *
  * Exactly one must be defined when CONFIG_USB_PD_TCPMV2 is defined.
diff --git a/include/dps.h b/include/dps.h
new file mode 100644
index 0000000000..151c6b3f09
--- /dev/null
+++ b/include/dps.h
@@ -0,0 +1,71 @@
+/* Copyright 2021 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#ifndef __CROS_EC_DPS__H
+#define __CROS_EC_DPS__H
+
+#include <stdbool.h>
+
+#include "common.h"
+
+/* Dynamic PDO Selection config. */
+struct dps_config_t {
+	/* (0, 100) coeff for transition to a lower power PDO*/
+	uint32_t k_less_pwr;
+	/* (0, 100) coeff for transition to a higher power PDO*/
+	uint32_t k_more_pwr;
+	/* Number for how many the same consecutive sample to transist */
+	uint32_t k_sample;
+	/* Number for moving average window for the power and the current. */
+	uint32_t k_window;
+	/* Power stabilized time after a new contract in us */
+	uint32_t t_stable;
+	/* Next power evaluation time interval in us */
+	uint32_t t_check;
+	/*
+	 * If the current voltage is more efficient than the previous voltage
+	 *
+	 * @param curr_mv: current PDO voltage
+	 * @param prev_mv: previous PDO voltage
+	 * @param batt_mv: battery desired voltage
+	 * @param batt_mw: current battery power
+	 * @param input_mw: current adapter input power
+	 * @return true is curr_mv is more efficient otherwise false
+	 */
+	bool (*is_more_efficient)(int curr_mv, int prev_mv, int batt_mv,
+				  int batt_mw, int input_mw);
+};
+
+/*
+ * Get voltage in the current system load
+ *
+ * @return a voltage(mV) that the adapter supports to charge at the given port.
+ */
+int dps_get_dynamic_voltage(void);
+
+/*
+ * Get DPS charge port
+ *
+ * @return the DPS charge port, or CHARGE_PORT_NONE if unavailable.
+ */
+int dps_get_charge_port(void);
+
+/*
+ * Check if DPS is enabled.
+ *
+ * @return true if enabled, false otherwise.
+ */
+bool dps_is_enabled(void);
+
+/*
+ * Update DPS stablized timeout
+ *
+ * This is called at the exit of PE_SNK_TRANSITION_SINK
+ *
+ * @param port: the port for timer reset.
+ */
+void dps_update_stabilized_time(int port);
+
+#endif /* __CROS_EC_DPS__H */
diff --git a/util/config_allowed.txt b/util/config_allowed.txt
index ea62090d2a..dac8a0321f 100644
--- a/util/config_allowed.txt
+++ b/util/config_allowed.txt
@@ -1196,6 +1196,7 @@ CONFIG_USB_PD_DISCHARGE
 CONFIG_USB_PD_DISCHARGE_GPIO
 CONFIG_USB_PD_DISCHARGE_PPC
 CONFIG_USB_PD_DISCHARGE_TCPC
+CONFIG_USB_PD_DPS
 CONFIG_USB_PD_DP_HPD_GPIO
 CONFIG_USB_PD_DP_HPD_GPIO_CUSTOM
 CONFIG_USB_PD_DUAL_ROLE
diff --git a/zephyr/Kconfig.usbc b/zephyr/Kconfig.usbc
index 17e2c7f182..e8daf858ef 100644
--- a/zephyr/Kconfig.usbc
+++ b/zephyr/Kconfig.usbc
@@ -367,6 +367,17 @@ endchoice # Trigger implementation
 
 endif # PLATFORM_EC_USB_PD_FRS
 
+config PLATFORM_EC_USB_PD_DPS
+	bool "Board can support Dynamic PDO Selection"
+	depends on PLATFORM_EC_BATTERY
+	default n
+	help
+	  Enable this if the board needs dynamic PDO selection.
+	  DPS picks a power efficient PDO regarding to the underlying battery
+	  configuration and the system loading.
+	  Default configuration can be overrided by `dps_config` to adapt
+	  to each board's need.
+
 config PLATFORM_EC_USB_PD_DUAL_ROLE_AUTO_TOGGLE
 	bool "Board can use TCPC-controlled DRP toggle"
 	depends on PLATFORM_EC_USB_PD_DUAL_ROLE
diff --git a/zephyr/shim/include/config_chip.h b/zephyr/shim/include/config_chip.h
index 71b7167e20..0a960ccca9 100644
--- a/zephyr/shim/include/config_chip.h
+++ b/zephyr/shim/include/config_chip.h
@@ -887,6 +887,11 @@
 #define CONFIG_USB_PD_ALT_MODE_UFP
 #endif
 
+#undef CONFIG_USB_PD_DPS
+#ifdef CONFIG_PLATFORM_EC_USB_PD_DPS
+#define CONFIG_USB_PD_DPS
+#endif
+
 #undef CONFIG_USBC_RETIMER_FW_UPDATE
 #ifdef CONFIG_PLATFORM_EC_USBC_RETIMER_FW_UPDATE
 #define CONFIG_USBC_RETIMER_FW_UPDATE