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>

5 files changed, 676 insertions, 5 deletions

diff --git a/common/build.mk b/common/build.mk
index 48a247d58a..867dcf465f 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -174,6 +174,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 28fe109f1d..862bb28725 100644
--- a/common/charge_manager.c
+++ b/common/charge_manager.c
@@ -11,6 +11,7 @@
 #include "charge_state_v2.h"
 #include "charger.h"
 #include "console.h"
+#include "dps.h"
 #include "extpower.h"
 #include "gpio.h"
 #include "hooks.h"
@@ -624,11 +625,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.
@@ -670,8 +673,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 ae8429262d..3981353101 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 f3faa1eabb..bf134c5449 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"
@@ -3159,6 +3160,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
 }
 
 /**
@@ -3388,6 +3394,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);
 }